完善点云去噪功能

vision_detect/vision_detect/VisionDetect/node/detect_node.py

@@ -158,10 +158,12 @@ class DetectNode(Node):
             self.calculate_function = calculate_pose_pca
         elif self.calculate_mode == "ICP":
             self.configs = json.loads(self.get_parameter('ICA_configs').value)
-            self.source = o3d.io.read_point_cloud(
+            source = o3d.io.read_point_cloud(
                 os.path.join(share_dir, self.configs['complete_model_path'])
                 )
+            self.configs["source"] = source
             self.calculate_function = calculate_pose_icp
+
         else:
             self.get_logger().warning("Unknown calculate_mode, use PCA")
             self.configs = json.loads(self.get_parameter('PCA_configs').value)
@@ -297,6 +299,7 @@ class DetectNode(Node):
     def _service_sub_callback(self, msgs):
         """同步回调函数"""
         with self.lock:
+            # self.get_logger().info("get msgs")
             self.camera_data[msgs.position] = [
                 msgs.image_color,
                 msgs.image_depth,
@@ -336,6 +339,7 @@ class DetectNode(Node):
             self.pub_pose_list.publish(pose_list_all)
 
     def _service_callback(self, request, response):
+        # self.get_logger().info("service callback start")
         response.header.stamp = self.get_clock().now().to_msg()
         response.header.frame_id = "camera_detect"
 
@@ -343,10 +347,19 @@ class DetectNode(Node):
             if request.camera_position in self.camera_data:
                 color_img_ros, depth_img_ros, self.k, d = self.camera_data[request.camera_position]
             else:
+                if len(self.camera_data) == 0:
+                    response.success = False
+                    response.info = "Camera data have not objects"
+                    response.objects = []
+
+                    # self.get_logger().info("service callback done")
+                    return response
+
                 response.success = False
-                response.info = f"{request.camera_position} Camera data is empty or name is wrong"
+                response.info = f"Name is wrong: {request.camera_position}"
                 response.objects = []
 
+                # self.get_logger().info("service callback done")
                 return response
 
         if request.camera_position == 'left':
@@ -389,6 +402,7 @@ class DetectNode(Node):
             response.success = False
             response.objects = []
 
+        # self.get_logger().info("service callback done")
         return response
 
     def _seg_image(self, rgb_img: np.ndarray, depth_img: np.ndarray):
@@ -403,6 +417,7 @@ class DetectNode(Node):
 
         # Get masks
         if result.masks is None or len(result.masks) == 0:
+            self.get_logger().info(f"Detect object num: 0")
             return None, None
         masks = result.masks.data.cpu().numpy()
 
@@ -435,12 +450,14 @@ class DetectNode(Node):
                 mask_crop,
                 depth_crop,
                 intrinsics,
-                self.source,
-                self.configs
+                **self.configs
             )
+            if rmat is None:
+                self.get_logger().warning("Object point cloud have too many noise")
+                continue
 
             grab_width = calculate_grav_width(mask, self.k, rmat[2, 3])
-            rmat[2, 3] = rmat[2, 3] + grab_width * 0.22
+            rmat[2, 3] = rmat[2, 3] + grab_width * 0.38
 
             rmat = self.hand_eye_mat @ rmat
 
@@ -463,7 +480,7 @@ class DetectNode(Node):
 
         self.get_logger().info('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'{(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'{(time4 - time1) * 1000} ms, completing a picture entire process')
         self.get_logger().info('end')
@@ -519,8 +536,7 @@ class DetectNode(Node):
             mask_crop,
             depth_crop,
             intrinsics,
-            self.source,
-            self.configs
+            **self.configs
         )
 
         rmat = self.hand_eye_mat @ rmat
@@ -585,8 +601,7 @@ class DetectNode(Node):
                 mask_crop,
                 depth_crop,
                 intrinsics,
-                self.source,
-                self.configs
+                **self.configs
             )
 
             x, y, z, rw, rx, ry, rz = rmat2quat(rmat)

vision_detect/vision_detect/VisionDetect/utils/calculate_tools.py

@@ -37,20 +37,9 @@ def calculate_pose_pca(
         mask,
         depth_img: np.ndarray,
         intrinsics,
-        source=None,
-        configs=None
+        **kwargs
 ):
-    """计算位态"""
-    if configs is None:
-        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
-        }
+    """点云主成分分析法计算位态"""
     depth_img_mask = np.zeros_like(depth_img)
     depth_img_mask[mask > 0] = depth_img[mask > 0]
 
@@ -59,29 +48,21 @@ def calculate_pose_pca(
     point_cloud = o3d.geometry.PointCloud.create_from_depth_image(
         depth=depth_o3d,
         intrinsic=intrinsics,
-        depth_scale=configs["depth_scale"],
-        depth_trunc=configs["depth_trunc"],
+        depth_scale=kwargs.get("depth_scale", 1000.0),
+        depth_trunc=kwargs.get("depth_trunc", 3.0),
     )
 
-    point_cloud = point_cloud.remove_non_finite_points()
+    point_cloud = point_cloud_denoising(point_cloud, kwargs.get("voxel_size", 0.010))
+    if point_cloud is None:
+        return None
 
-    down_pcd = point_cloud.voxel_down_sample(voxel_size=configs["voxel_size"])
-    clean_pcd, _ = down_pcd.remove_radius_outlier(
-        nb_points=configs["nb_points"], 
-        radius=configs["radius"]
-        )
-    clean_pcd, _ = clean_pcd.remove_statistical_outlier(
-        nb_neighbors=configs["nb_neighbors"], 
-        std_ratio=configs["std_ratio"]
-        )
-
-    if len(clean_pcd.points) == 0:
+    if len(point_cloud.points) == 0:
         logging.warning("clean_pcd is empty")
-        return 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0
-    center = clean_pcd.get_center()
+        return np.eye(4)
+    center = point_cloud.get_center()
     x, y, z = center
 
-    w, v = pca(np.asarray(clean_pcd.points))
+    w, v = pca(np.asarray(point_cloud.points))
 
     if w is None or v is None:
         logging.warning("PCA output w or v is None")
@@ -99,6 +80,9 @@ 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, rmat)
+    # draw(point_cloud_1, rmat)
+
     return rmat
 
 
@@ -106,22 +90,9 @@ def calculate_pose_icp(
         mask,
         depth_img: np.ndarray,
         intrinsics,
-        source,
-        configs = None,
+        **kwargs
 ):
-    if configs is None:
-        configs = {
-            "depth_scale": 1000.0,
-            "depth_trunc": 2.0,
-            "nb_points": 10,
-            "radius": 0.1,
-            "nb_neighbors": 20,
-            "std_ratio": 3.0,
-            "ransac_voxel_size": 0.005,
-            "icp_voxel_radius": [0.004, 0.002, 0.001],
-            "icp_max_iter": [50, 30, 14]
-        }
-    """计算位态"""
+    """点云配准法计算位姿"""
     depth_img_mask = np.zeros_like(depth_img)
     depth_img_mask[mask > 0] = depth_img[mask > 0]
 
@@ -130,38 +101,92 @@ def calculate_pose_icp(
     point_cloud = o3d.geometry.PointCloud.create_from_depth_image(
         depth=depth_o3d,
         intrinsic=intrinsics,
-        depth_scale=configs["depth_scale"],
-        depth_trunc=configs["depth_trunc"],
+        depth_scale=kwargs.get("depth_scale", 1000.0),
+        depth_trunc=kwargs.get("depth_trunc", 3.0)
     )
 
-    point_cloud = point_cloud.remove_non_finite_points()
+    point_cloud = point_cloud_denoising(point_cloud, kwargs.get("voxel_size", 0.010))
+    if point_cloud is None:
+        return None
 
-    clean_pcd, _ = point_cloud.remove_radius_outlier(
-        nb_points=configs["nb_points"], 
-        radius=configs["radius"]
-        )
-    clean_pcd, _ = clean_pcd.remove_statistical_outlier(
-        nb_neighbors=configs["nb_neighbors"], 
-        std_ratio=configs["std_ratio"]
-        )
-
-    if len(clean_pcd.points) == 0:
+    if len(point_cloud.points) == 0:
         logging.warning("clean_pcd is empty")
         return np.eye(4)
 
     rmat = object_icp(
-        source,
-        clean_pcd,
-        ransac_voxel_size=configs["ransac_voxel_size"],
-        icp_voxel_radius=configs["icp_voxel_radius"],
-        icp_max_iter=configs["icp_max_iter"]
+        kwargs.get("source"),
+        point_cloud,
+        ransac_voxel_size=kwargs.get("ransac_voxel_size", 0.005),
+        icp_voxel_radius=kwargs.get("icp_voxel_radius", [0.004, 0.002, 0.001]),
+        icp_max_iter=kwargs.get("icp_max_iter", [50, 30, 14])
     )
 
     return rmat
 
 
+def point_cloud_denoising(point_cloud: o3d.geometry.PointCloud, voxel_size: float = 0.010):
+    """点云去噪"""
+    point_cloud = point_cloud.remove_non_finite_points()
+    down_pcd = point_cloud.voxel_down_sample(voxel_size=voxel_size)
+
+    # 半径滤波
+    clean_pcd, _ = down_pcd.remove_radius_outlier(
+        nb_points=10,
+        radius=voxel_size * 5
+        )
+
+    # 统计滤波
+    clean_pcd, _ = clean_pcd.remove_statistical_outlier(
+        nb_neighbors=10,
+        std_ratio=2.0
+        )
+
+    # 过滤过近的点
+    points = np.asarray(clean_pcd.points)
+    clean_pcd.points = o3d.utility.Vector3dVector(points[points[:, 2] >= 0.2])
+
+    # # 使用数量最大簇判定噪声强度
+    # _, counts = np.unique(labels[labels >= 0], return_counts=True)
+    # largest_cluster_ratio = counts.max() / len(labels)
+    # if largest_cluster_ratio < 0.5:
+    #     return None
+
+    labels = np.array(clean_pcd.cluster_dbscan(eps=voxel_size * 5, min_points=10))
+    if len(labels[labels >= 0]) == 0:
+        return clean_pcd
+
+    # 使用距离最近簇作为物体
+    points = np.asarray(clean_pcd.points)
+    cluster_label = set(labels)
+    point_cloud_clusters = []
+    for label in cluster_label:
+        if label == -1:
+            continue
+        idx = np.where(labels == label)[0]
+        point_cloud_cluster = clean_pcd.select_by_index(idx)
+        points_cluster_z = points[idx, 2]
+        z_avg = np.mean(points_cluster_z)
+        if z_avg < 0.2:
+            continue
+        point_cloud_clusters.append((point_cloud_cluster, z_avg))
+
+    if len(point_cloud_clusters) == 0:
+        return clean_pcd
+
+    point_cloud_clusters.sort(key=lambda x: x[1])
+    clean_pcd = point_cloud_clusters[0][0]
+
+    # 使用最近簇判断噪音强度
+    largest_cluster_ratio = len(clean_pcd.points) / len(points)
+    if largest_cluster_ratio < 0.2:
+        return None
+
+    return clean_pcd
+
+
 def calculate_grav_width(mask, k, depth):
     """计算重心宽度"""
+    mask = mask.astype(np.uint8) * 255
     contours, _ = cv2.findContours(mask, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)
     
     if not contours:
@@ -194,3 +219,26 @@ def quat2rmat(quat):
     r = tfs.quaternions.quat2mat([rw, rx, ry, rz])
     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])

vision_detect/vision_detect/flexivaidk_detect_service.py

@@ -333,22 +333,6 @@ class DetectNode(Node):
             p1 = [int((x_center - width / 2)), int((y_center - height / 2))]
             p2 = [int((x_center + width / 2)), int((y_center + height / 2))]
             cv2.rectangle(img, p1, p2, (255, 255, 0), 2)
-            
-            # rgb_crop, depth_crop, mask_crop, (x_min, y_min) = crop_mask_bbox(rgb_img, depth_img, mask, box)
-
-            # if depth_crop is None:
-            #     self.get_logger().error("depth_crop is None")
-            #     continue
-
-            # depth_img_crop_mask = np.zeros_like(depth_crop)
-            # depth_img_crop_mask[mask_crop > 0] = depth_crop[mask_crop > 0]
-
-            # print(rgb_crop.shape)
-            # print(rgb_crop.dtype)
-
-
-            # rgb_bytes = cv2.imencode('.png', rgb_crop)[1]
-            # depth_bytes = cv2.imencode('.png', depth_img_crop_mask)[1]
 
             contours, _ = cv2.findContours(mask, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)
             mask_contours = contours[0].reshape(1, -1, 2)
@@ -378,7 +362,8 @@ class DetectNode(Node):
 
             self.get_logger().info(f"state: {state}")
             self.get_logger().info(
-                f"current detected object names: {self.aidk_client.get_detected_obj_names()}, current detected object nums: {self.aidk_client.get_detected_obj_nums()}"
+                f"current detected object names: {self.aidk_client.get_detected_obj_names()}, "
+                f"current detected object nums: {self.aidk_client.get_detected_obj_nums()}"
             )
 
             self.get_logger().info(
@@ -415,42 +400,6 @@ class DetectNode(Node):
                         }
                     )
 
-            # for key in self.config["keys"]:
-            #     parse_state, result_list = self.aidk_client.parse_result(self.config["command"]["obj_name"], key, -1)
-            #     self.get_logger().info(
-            #         "detected time stamp: {}".format(
-            #             datetime.fromtimestamp(self.aidk_client.get_detected_time())
-            #         )
-            #     )
-            #     if not parse_state:
-            #         self.get_logger().error("Parse result error!!!")
-            #         continue
-            #     else:
-            #         if key in ["bbox", "keypoints", "positions", "obj_pose"]:
-            #             for result in result_list:
-            #                 self.get_logger().info(f'"double_value in result": {getattr(result, "double_value")}')
-            #                 for vec in result.vect:
-            #                     self.get_logger().info(f"vec: {vec}")
-            #                     x, y, z, rw, rx, ry, rz = vec
-            #
-            #                     pose = Pose()
-            #                     pose.position = Point(x=x, y=y, z=z)
-            #                     pose.orientation = Quaternion(w=rw, x=rx, y=ry, z=rz)
-            #                     pose_list.append(
-            #                         {
-            #                             "class_id": int(class_ids[i]),
-            #                             "class_name": labels[class_ids[i]],
-            #                             "pose": pose,
-            #                             "grab_width": getattr(result, "double_value")
-            #                         }
-            #                     )
-            #                     self.get_logger().info(f"pose: position({x}, {y}, {z}), orientation({rw}, {rx}, {ry}, {rz})")
-            #                     self.get_logger().info(f"class_id: {int(class_ids[i])}, class_name: {labels[class_ids[i]]}, grab_width: {getattr(result, 'double_value')}")
-            #
-            #         elif key in ["valid", "double_value", "int_value", "name"]:
-            #             for result in result_list:
-            #                 self.get_logger().info(f"{key}: {getattr(result, key)}")
-
         time4 = time.time()
 
         cv2.imwrite("/home/nvidia/detect_result.png", img)