Merge branch 'feature' into develop

Reviewed-on: #5

vision_detect/configs/launch_configs/bottle_detect_service_pca.json

@@ -19,10 +19,6 @@
     "PCA_configs": {
         "depth_scale": 1000.0,
         "depth_trunc": 3.0,
-        "voxel_size": 0.020,
-        "nb_points": 10,
-        "radius": 0.1,
-        "nb_neighbors": 20,
-        "std_ratio": 3.0
+        "voxel_size": 0.010
     }
 }

vision_detect/configs/launch_configs/default_config.json

@@ -32,11 +32,7 @@
     "PCA_configs": {
         "depth_scale": 1000.0,
         "depth_trunc": 3.0,
-        "voxel_size": 0.020,
-        "nb_points": 10,
-        "radius": 0.1,
-        "nb_neighbors": 20,
-        "std_ratio": 3.0
+        "voxel_size": 0.010
     },
     "ICP_configs": {
         "complete_model_path": "pointclouds/bottle_model.pcd",

vision_detect/configs/launch_configs/detect_service_pca.json

@@ -19,10 +19,6 @@
     "PCA_configs": {
         "depth_scale": 1000.0,
         "depth_trunc": 3.0,
-        "voxel_size": 0.020,
-        "nb_points": 10,
-        "radius": 0.1,
-        "nb_neighbors": 20,
-        "std_ratio": 3.0
+        "voxel_size": 0.010
     }
 }

vision_detect/vision_detect/VisionDetect/node/detect_node.py

@@ -457,7 +457,7 @@ class DetectNode(Node):
                 continue
 
             grab_width = calculate_grav_width(mask, self.k, rmat[2, 3])
-            rmat[2, 3] = rmat[2, 3] + grab_width * 0.38
+            rmat[2, 3] = rmat[2, 3] + grab_width * 0.30
 
             rmat = self.hand_eye_mat @ rmat
 
@@ -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

@@ -52,7 +52,7 @@ def calculate_pose_pca(
         depth_trunc=kwargs.get("depth_trunc", 3.0),
     )
 
-    point_cloud = point_cloud_denoising(point_cloud, kwargs.get("voxel_size", 0.010))
+    point_cloud = point_cloud_denoising(point_cloud, kwargs.get("voxel_size", 0.005))
     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_1, rmat)
 
     return rmat
 
@@ -124,10 +124,18 @@ def calculate_pose_icp(
     return rmat
 
 
-def point_cloud_denoising(point_cloud: o3d.geometry.PointCloud, voxel_size: float = 0.010):
+def point_cloud_denoising(point_cloud: o3d.geometry.PointCloud, voxel_size: float = 0.005):
     """点云去噪"""
     point_cloud = point_cloud.remove_non_finite_points()
-    down_pcd = point_cloud.voxel_down_sample(voxel_size=voxel_size)
+    while True:
+        down_pcd = point_cloud.voxel_down_sample(voxel_size=voxel_size * 0.5)
+        point_num = len(down_pcd.points)
+        if point_num <= 1000:
+            break
+        voxel_size *= 1.5
+
+    # logging.fatal("point_cloud_denoising: point_num={}".format(len(point_cloud.points)))
+    # logging.fatal("point_cloud_denoising: point_num={}".format(point_num))
 
     # 半径滤波
     clean_pcd, _ = down_pcd.remove_radius_outlier(
@@ -177,8 +185,8 @@ def point_cloud_denoising(point_cloud: o3d.geometry.PointCloud, voxel_size: floa
     clean_pcd = point_cloud_clusters[0][0]
 
     # 使用最近簇判断噪音强度
-    largest_cluster_ratio = len(clean_pcd.points) / len(points)
-    if largest_cluster_ratio < 0.2:
+    largest_cluster_ratio = len(clean_pcd.points) / point_num
+    if largest_cluster_ratio < 0.20:
         return None
 
     return clean_pcd

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: