修改抓取宽度计算方法

vision_detect/vision_detect/VisionDetect/node/detect_node.py

@@ -190,18 +190,19 @@ class DetectNode(InitBase):
                 self.k[5] - y_min
             )
 
-            rmat = self.calculate_function(
+            rmat, grab_width = self.calculate_function(
                 mask_crop,
                 depth_crop,
                 intrinsics,
+                calculate_grab_width=True,
                 **self.calculate_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.30
+            # grab_width = calculate_grab_width(mask, self.k, rmat[2, 3])
+            # rmat[2, 3] = rmat[2, 3] + grab_width * 0.30
 
             rmat = self.hand_eye_mat @ rmat
 
@@ -216,7 +217,7 @@ class DetectNode(InitBase):
                         "class_id": int(class_ids[i]),
                         "class_name": labels[class_ids[i]],
                         "pose": pose,
-                        "grab_width": grab_width * 1.05
+                        "grab_width": grab_width
                     }
                 )
 
@@ -279,7 +280,7 @@ class DetectNode(InitBase):
             self.k[5] - y_min
         )
 
-        rmat = self.calculate_function(
+        rmat, _ = self.calculate_function(
             mask_crop,
             depth_crop,
             intrinsics,
@@ -347,7 +348,7 @@ class DetectNode(InitBase):
                 self.k[5] - y_min
             )
 
-            rmat = self.calculate_function(
+            rmat, _ = self.calculate_function(
                 mask_crop,
                 depth_crop,
                 intrinsics,

vision_detect/vision_detect/VisionDetect/utils/calculate_tools.py

@@ -9,7 +9,7 @@ import transforms3d as tfs
 from .object_icp import object_icp
 
 __all__ = [
-    "calculate_pose_pca", "calculate_pose_icp", "calculate_grav_width",
+    "calculate_pose_pca", "calculate_pose_icp", "calculate_grab_width",
     "rmat2quat", "quat2rmat",
 ]
 
@@ -37,6 +37,7 @@ def calculate_pose_pca(
         mask,
         depth_img: np.ndarray,
         intrinsics,
+        calculate_grab_width: bool = False,
         **kwargs
 ):
     """点云主成分分析法计算位态"""
@@ -54,19 +55,36 @@ def calculate_pose_pca(
 
     point_cloud = point_cloud_denoising(point_cloud, kwargs.get("voxel_size", 0.005))
     if point_cloud is None:
-        return None
+        return None, 0.0
 
     if len(point_cloud.points) == 0:
         logging.warning("clean_pcd is empty")
-        return np.eye(4)
-    center = point_cloud.get_center()
-    x, y, z = center
+        return np.eye(4), 0.0
 
-    w, v = pca(np.asarray(point_cloud.points))
+    if calculate_grab_width:
+        obb = point_cloud.get_oriented_bounding_box()
+        x, y, z = obb.center
+        sizes = obb.extent
+        grab_width = np.sort(sizes)[-2]
+        z = z + grab_width * 0.3
 
-    if w is None or v is None:
-        logging.warning("PCA output w or v is None")
-        return np.eye(4)
+        v = obb.R
+        if v is None:
+            logging.warning("PCA output v is None")
+            return np.eye(4), 0.0
+
+        grab_width = grab_width * 1.05
+
+    else:
+        center = point_cloud.get_center()
+        x, y, z = center
+
+        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")
+            return np.eye(4), 0.0
+        grab_width = 0.0
     
     vx, vy, vz = v[:,0], v[:,1], v[:,2]
 
@@ -83,13 +101,14 @@ def calculate_pose_pca(
     # draw(point_cloud_u, rmat)
     # draw(point_cloud, rmat)
 
-    return rmat
+    return rmat, grab_width
 
 
 def calculate_pose_icp(
         mask,
         depth_img: np.ndarray,
         intrinsics,
+        calculate_grab_width: bool = False,
         **kwargs
 ):
     """点云配准法计算位姿"""
@@ -107,11 +126,14 @@ def calculate_pose_icp(
 
     point_cloud = point_cloud_denoising(point_cloud, kwargs.get("voxel_size", 0.010))
     if point_cloud is None:
-        return None
+        return None, 0.0
 
     if len(point_cloud.points) == 0:
         logging.warning("clean_pcd is empty")
-        return np.eye(4)
+        return np.eye(4), 0.0
+
+    if calculate_grab_width:
+        pass
 
     rmat = object_icp(
         kwargs.get("source"),
@@ -121,7 +143,8 @@ def calculate_pose_icp(
         icp_max_iter=kwargs.get("icp_max_iter", [50, 30, 14])
     )
 
-    return rmat
+    grab_width = 0.0
+    return rmat, grab_width
 
 
 def point_cloud_denoising(point_cloud: o3d.geometry.PointCloud, voxel_size: float = 0.005):
@@ -192,7 +215,7 @@ def point_cloud_denoising(point_cloud: o3d.geometry.PointCloud, voxel_size: floa
     return clean_pcd
 
 
-def calculate_grav_width(mask, k, depth):
+def calculate_grab_width(mask, k, depth):
     """计算重心宽度"""
     mask = mask.astype(np.uint8) * 255
     contours, _ = cv2.findContours(mask, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)