1

vision_detect/vision_detect/VisionDetect/utils/calculate_tools.py

@@ -38,7 +38,6 @@ def calculate_pose_pca(
         depth_img: np.ndarray,
         intrinsics,
         source=None,
-        hand_eye_mat=np.eye(4),
         configs=None
 ):
     """计算位态"""
@@ -86,7 +85,7 @@ def calculate_pose_pca(
 
     if w is None or v is None:
         logging.warning("PCA output w or v is None")
-        return 0.0, 0.0, 0.0, None, None, None, None
+        return np.eye(4)
     
     vx, vy, vz = v[:,0], v[:,1], v[:,2]
 
@@ -99,11 +98,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])
-    rmat = hand_eye_mat @ rmat
-    rw, rx, ry, rz = tfs.quaternions.mat2quat(rmat[0:3, 0:3])
-    x, y, z = rmat[0:3, 3].flatten()
 
-    return x, y, z, rw, rx, ry, rz
+    return rmat
 
 
 def calculate_pose_icp(