Merge branch 'feature' into feature_cpp_test

vision_detect/configs/launch_configs/medical_sense.json

@@ -18,7 +18,7 @@
     "Detect_configs": {
         "checkpoint_path": "checkpoints/medical_sense-seg.pt",
         "confidence": 0.50,
-        "classes": [0, 1]
+        "classes": []
     },
 
     "calculate_mode": "PCA",

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_grab_width",
+    "calculate_pose_pca", "calculate_pose_icp",
     "rmat2quat", "quat2rmat",
 ]
 
@@ -55,11 +55,11 @@ def calculate_pose_pca(
 
     point_cloud = point_cloud_denoising(point_cloud, kwargs.get("voxel_size", 0.005))
     if point_cloud is None:
-        return None, 0.0
+        return None, [0.0, 0.0, 0.0]
 
     if len(point_cloud.points) == 0:
         logging.warning("clean_pcd is empty")
-        return np.eye(4), 0.0
+        return np.eye(4), [0.0, 0.0, 0.0]
 
     if calculate_grab_width:
         obb = point_cloud.get_oriented_bounding_box()
@@ -67,7 +67,7 @@ def calculate_pose_pca(
         extent = obb.extent
         order = np.argsort(-extent)
 
-        grab_width = extent[order][-2]
+        grab_width = extent[order]
         # z = z + grab_width * 0.20
 
         v = obb.R
@@ -75,7 +75,7 @@ def calculate_pose_pca(
 
         if v is None:
             logging.warning("PCA output v is None")
-            return np.eye(4), 0.0
+            return np.eye(4), [0.0, 0.0, 0.0]
 
         grab_width = grab_width * 1.05
 
@@ -88,7 +88,7 @@ def calculate_pose_pca(
         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
+        grab_width = [0.0, 0.0, 0.0]
     
     vx, vy, vz = v[:,0], v[:,1], v[:,2]
 
@@ -130,11 +130,11 @@ def calculate_pose_icp(
 
     point_cloud = point_cloud_denoising(point_cloud, kwargs.get("voxel_size", 0.010))
     if point_cloud is None:
-        return None, 0.0
+        return None, [0.0, 0.0, 0.0]
 
     if len(point_cloud.points) == 0:
         logging.warning("clean_pcd is empty")
-        return np.eye(4), 0.0
+        return np.eye(4), [0.0, 0.0, 0.0]
 
     if calculate_grab_width:
         pass
@@ -147,7 +147,7 @@ def calculate_pose_icp(
         icp_max_iter=kwargs.get("icp_max_iter", [50, 30, 14])
     )
 
-    grab_width = 0.0
+    grab_width = [0.0, 0.0, 0.0]
     return rmat, grab_width
 
 
@@ -219,25 +219,25 @@ def point_cloud_denoising(point_cloud: o3d.geometry.PointCloud, voxel_size: floa
     return clean_pcd
 
 
-def calculate_grab_width(mask, k, depth):
-    """计算重心宽度"""
-    mask = mask.astype(np.uint8) * 255
-    contours, _ = cv2.findContours(mask, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)
-    
-    if not contours:
-        return 0.0
-    
-    c = max(contours, key=cv2.contourArea)
-    box = cv2.boxPoints(cv2.minAreaRect(c))
-    if np.linalg.norm(box[1] - box[0]) < np.linalg.norm(box[1] - box[2]):
-        point_diff = box[1] - box[0]
-    else:
-        point_diff = box[1] - box[2]
-
-    grab_width = depth * np.sqrt(
-        point_diff[0]**2 / k[0]**2 + point_diff[1]**2 / k[4]**2
-        )
-    return grab_width
+# def calculate_grab_width(mask, k, depth):
+#     """计算重心宽度"""
+#     mask = mask.astype(np.uint8) * 255
+#     contours, _ = cv2.findContours(mask, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)
+#
+#     if not contours:
+#         return 0.0
+#
+#     c = max(contours, key=cv2.contourArea)
+#     box = cv2.boxPoints(cv2.minAreaRect(c))
+#     if np.linalg.norm(box[1] - box[0]) < np.linalg.norm(box[1] - box[2]):
+#         point_diff = box[1] - box[0]
+#     else:
+#         point_diff = box[1] - box[2]
+#
+#     grab_width = depth * np.sqrt(
+#         point_diff[0]**2 / k[0]**2 + point_diff[1]**2 / k[4]**2
+#         )
+#     return grab_width
     
 
 def rmat2quat(rmat):