diff --git a/vision_detect/vision_detect/VisionDetect/utils/calculate_tools.py b/vision_detect/vision_detect/VisionDetect/utils/calculate_tools.py index 3561d48..683c667 100644 --- a/vision_detect/vision_detect/VisionDetect/utils/calculate_tools.py +++ b/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(