HiveCoreRD/hivecore_robot_os
6
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

减小计算误差

Browse Source
This commit is contained in:
liangyuxuan
2025-09-15 11:38:22 +08:00
parent 7b194e0d72
commit db3e9c0a57
4 changed files with 24 additions and 21 deletions
Download Patch File Download Diff File Expand all files Collapse all files

39
HiveCoreR0/src/robot_vision/detect_part/detect_part/detect.py
View File

@@ -1,6 +1,6 @@
import os
from collections import defaultdict
import time
# import time
import cv2
import open3d as o3d
@@ -60,7 +60,7 @@ def calculate_pose_cpu(mask, depth_img: np.ndarray, intrinsics):
depth=depth_o3d,
intrinsic=intrinsics,
depth_scale=1000.0,
depth_trunc=5.0,
depth_trunc=8.0,
)
point_cloud = point_cloud.remove_non_finite_points()
@@ -357,9 +357,9 @@ class DetectNode(Node):
pose_dict = defaultdict(list)
'''Get Predict Results'''
time1 = time.time()
# time1 = time.time()
results = self.model(rgb_img)
time2 = time.time()
# time2 = time.time()
result = results[0]
'''Get masks'''
@@ -374,18 +374,21 @@ class DetectNode(Node):
class_ids = result.boxes.cls.cpu().numpy()
labels = result.names
time3 = time.time()
# time3 = time.time()
for i, (mask, box) in enumerate(zip(masks, boxes)):
'''Confidence Filter'''
if confidences[i] >= self.set_confidence:
mask = cv2.resize(mask.astype(np.uint8), orig_shape[::-1], interpolation=cv2.INTER_NEAREST)
depth_crop, mask_crop, (x_min, y_min) = crop_mask_bbox(depth_img, mask, box)
if depth_crop is not None:
if depth_crop is None:
continue
mask_crop = cv2.resize(mask_crop, None, fx=self.fx, fy=self.fy, interpolation=cv2.INTER_NEAREST)
depth_crop = cv2.resize(depth_crop, None, fx=self.fx, fy=self.fy, interpolation=cv2.INTER_NEAREST)
if mask.shape[0] >= (orig_shape[0] * 0.1) and mask.shape[1] >= (orig_shape[1] * 0.1):
mask_crop = cv2.resize(mask_crop, None, fx=self.fx, fy=self.fy, interpolation=cv2.INTER_NEAREST)
depth_crop = cv2.resize(depth_crop, None, fx=self.fx, fy=self.fy, interpolation=cv2.INTER_NEAREST)
intrinsics = o3d.camera.PinholeCameraIntrinsic(
int(self.expect_size[0] * self.fx),
int(self.expect_size[1] * self.fy),
@@ -402,19 +405,19 @@ class DetectNode(Node):
pose.orientation = Quaternion(x=roll, y=pitch, z=yaw)
pose_dict[int(class_ids[i]), labels[class_ids[i]]].append(pose)
time4 = time.time()
# time4 = time.time()
'''mask_img and box_img is or not output'''
if self.output_boxes and not self.output_masks:
draw_box(self.set_confidence, rgb_img, result)
time5 = time.time()
self.get_logger().info(f'start')
self.get_logger().info(f'{(time2 - time1)*1000} ms, model predict')
self.get_logger().info(f'{(time3 - time2)*1000} ms, get mask and some param')
self.get_logger().info(f'{(time4 - time3)*1000} ms, calculate all mask PCA')
self.get_logger().info(f'{(time5 - time4)*1000} ms, draw box')
self.get_logger().info(f'{(time5 - time1)*1000} ms, completing a picture entire process')
self.get_logger().info(f'end')
# time5 = time.time()
# self.get_logger().info(f'start')
# self.get_logger().info(f'{(time2 - time1)*1000} ms, model predict')
# self.get_logger().info(f'{(time3 - time2)*1000} ms, get mask and some param')
# self.get_logger().info(f'{(time4 - time3)*1000} ms, calculate all mask PCA')
# self.get_logger().info(f'{(time5 - time4)*1000} ms, draw box')
# self.get_logger().info(f'{(time5 - time1)*1000} ms, completing a picture entire process')
# self.get_logger().info(f'end')
return self.cv_bridge.cv2_to_imgmsg(rgb_img, "bgr8"), pose_dict
elif self.output_boxes and self.output_masks:
draw_box(self.set_confidence, rgb_img, result)

2
HiveCoreR0/src/robot_vision/detect_part/launch/detect.launch.py
View File

@@ -9,7 +9,7 @@ def generate_launch_description():
DeclareLaunchArgument('checkpoint_name', default_value='yolo11s-seg.pt'),
DeclareLaunchArgument('output_boxes', default_value='True'),
DeclareLaunchArgument('output_masks', default_value='False'),
DeclareLaunchArgument('set_confidence', default_value='0.6'),
DeclareLaunchArgument('set_confidence', default_value='0.5'),
DeclareLaunchArgument('color_image_topic', default_value='/camera/camera/color/image_raw'),
DeclareLaunchArgument('depth_image_topic', default_value='/camera/camera/depth/image_rect_raw'),
DeclareLaunchArgument('camera_info_topic', default_value='/camera/camera/color/camera_info'),

2
HiveCoreR0/src/robot_vision/detect_part/launch/gemini_detect.launch.py
View File

@@ -54,7 +54,7 @@ def generate_launch_description():
DeclareLaunchArgument('checkpoint_name', default_value='yolo11s-seg.pt'),
DeclareLaunchArgument('output_boxes', default_value='True'),
DeclareLaunchArgument('output_masks', default_value='False'),
DeclareLaunchArgument('set_confidence', default_value='0.2'),
DeclareLaunchArgument('set_confidence', default_value='0.5'),
DeclareLaunchArgument('color_image_topic', default_value='/camera/color/image_raw'),
DeclareLaunchArgument('depth_image_topic', default_value='/camera/depth/image_raw'),
DeclareLaunchArgument('camera_info_topic', default_value='/camera/color/camera_info'),

2
HiveCoreR0/src/robot_vision/detect_part/launch/realsense_detect.launch.py
View File

@@ -146,7 +146,7 @@ def generate_launch_description():
DeclareLaunchArgument('checkpoint_name', default_value='yolo11s-seg.pt'),
DeclareLaunchArgument('output_boxes', default_value='True'),
DeclareLaunchArgument('output_masks', default_value='False'),
DeclareLaunchArgument('set_confidence', default_value='0.6'),
DeclareLaunchArgument('set_confidence', default_value='0.5'),
DeclareLaunchArgument('color_image_topic', default_value='/camera/camera/color/image_raw'),
DeclareLaunchArgument('depth_image_topic', default_value='/camera/camera/depth/image_rect_raw'),
DeclareLaunchArgument('camera_info_topic', default_value='/camera/camera/color/camera_info'),