Merge branch 'feature' into develop

vision_detect/vision_detect/VisionDetect/node/detect_node.py

@@ -490,6 +490,7 @@ class DetectNode(Node):
             home = os.path.expanduser("~")
             save_path = os.path.join(home, "detect_image.png")
             draw_box(rgb_img, result, save_path=save_path)
+            # draw_box(rgb_img, result, save_path=False)
             return self.cv_bridge.cv2_to_imgmsg(rgb_img, "bgr8"), pose_list
         elif self.output_boxes and self.output_masks:
             draw_box(rgb_img, result)

vision_detect/vision_detect/VisionDetect/utils/calculate_tools.py

@@ -45,14 +45,14 @@ def calculate_pose_pca(
 
     depth_o3d = o3d.geometry.Image(depth_img_mask.astype(np.uint16))
 
-    point_cloud_u = o3d.geometry.PointCloud.create_from_depth_image(
+    point_cloud = o3d.geometry.PointCloud.create_from_depth_image(
         depth=depth_o3d,
         intrinsic=intrinsics,
         depth_scale=kwargs.get("depth_scale", 1000.0),
         depth_trunc=kwargs.get("depth_trunc", 3.0),
     )
 
-    point_cloud = point_cloud_denoising(point_cloud_u, kwargs.get("voxel_size", 0.010))
+    point_cloud = point_cloud_denoising(point_cloud, kwargs.get("voxel_size", 0.010))
     if point_cloud is None:
         return None
 
@@ -80,8 +80,8 @@ def calculate_pose_pca(
     R = np.column_stack((vx, vy, vz))
     rmat = tfs.affines.compose(np.squeeze(np.asarray((x, y, z))), R, [1, 1, 1])
 
-    draw(point_cloud_u, rmat)
-    draw(point_cloud, rmat)
+    # draw(point_cloud_u, rmat)
+    # draw(point_cloud, rmat)
 
     return rmat
 
@@ -221,25 +221,25 @@ def quat2rmat(quat):
     rmat = tfs.affines.compose(np.squeeze(np.asarray((x, y, z))), r, [1, 1, 1])
     return rmat
 
-def draw(pcd, mat):
-    R = mat[0:3, 0:3]
-    point = mat[0:3, 3:4].flatten()
-    x, y, z = R[:, 0], R[:, 1], R[:, 2]
-
-    points = [
-        [0, 0, 0], [1, 0, 0], [0, 1, 0], [0, 0, 1],
-        point, point + x, point + y, point + z
-
-    ]  # 画点：原点、第一主成分、第二主成分
-    lines = [
-        [0, 1], [0, 2], [0, 3],
-        [4, 5], [4, 6], [4, 7]
-    ]  # 画出三点之间两两连线
-    colors = [
-        [1, 0, 0], [0, 1, 0], [0, 0, 1],
-        [1, 0, 0], [0, 1, 0], [0, 0, 1]
-    ]
-    line_set = o3d.geometry.LineSet(points=o3d.utility.Vector3dVector(points), lines=o3d.utility.Vector2iVector(lines))
-    line_set.colors = o3d.utility.Vector3dVector(colors)
-
-    o3d.visualization.draw_geometries([pcd, line_set])
+# def draw(pcd, mat):
+#     R = mat[0:3, 0:3]
+#     point = mat[0:3, 3:4].flatten()
+#     x, y, z = R[:, 0], R[:, 1], R[:, 2]
+#
+#     points = [
+#         [0, 0, 0], [1, 0, 0], [0, 1, 0], [0, 0, 1],
+#         point, point + x, point + y, point + z
+#
+#     ]  # 画点：原点、第一主成分、第二主成分
+#     lines = [
+#         [0, 1], [0, 2], [0, 3],
+#         [4, 5], [4, 6], [4, 7]
+#     ]  # 画出三点之间两两连线
+#     colors = [
+#         [1, 0, 0], [0, 1, 0], [0, 0, 1],
+#         [1, 0, 0], [0, 1, 0], [0, 0, 1]
+#     ]
+#     line_set = o3d.geometry.LineSet(points=o3d.utility.Vector3dVector(points), lines=o3d.utility.Vector2iVector(lines))
+#     line_set.colors = o3d.utility.Vector3dVector(colors)
+#
+#     o3d.visualization.draw_geometries([pcd, line_set])

vision_detect/vision_detect/VisionDetect/utils/draw_tools.py

@@ -38,7 +38,7 @@ def draw_box(
                         (p1[0], p1[1] - 10), cv2.FONT_HERSHEY_SIMPLEX, 1,
                         (255, 255, 0), 2)
 
-            cv2.putText(rgb_img, f"{i}", (p1[0] + 5, p1[1] + 15),
+            cv2.putText(rgb_img, f"{i}", (p1[0] + 15, p1[1] + 30),
                         cv2.FONT_HERSHEY_SIMPLEX, 1, (255, 255, 0), 2)
 
     if save_path: