抓取宽度设置为物体y轴方向

2025-12-10 14:42:12 +08:00
parent 1702ed2fcb
commit bce708ed05
1 changed files with 6 additions and 2 deletions
--- a/vision_detect/vision_detect/VisionDetect/utils/calculate_tools.py
+++ b/vision_detect/vision_detect/VisionDetect/utils/calculate_tools.py
@@ -64,11 +64,15 @@ def calculate_pose_pca(
    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]
+        extent = obb.extent
+        order = np.argsort(-extent)
+
+        grab_width = extent[order][-2]
        z = z + grab_width * 0.20

        v = obb.R
+        v = v[:, order]
+
        if v is None:
            logging.warning("PCA output v is None")
            return np.eye(4), 0.0