solve dependencies problem

data_gen_dependencies/data_utils.py

@@ -0,0 +1,30 @@
+""" Tools for data processing.
+    Author: chenxi-wang
+"""
+
+import numpy as np
+from scipy.spatial.transform import Rotation as R
+
+
+def pose_difference(pose1, pose2):
+    # 提取位置
+    position1 = pose1[:3, 3]
+    position2 = pose2[:3, 3]
+
+    # 计算位置的欧式距离
+    position_distance = np.linalg.norm(position1 - position2)
+
+    # 提取旋转矩阵
+    rotation1 = pose1[:3, :3]
+    rotation2 = pose2[:3, :3]
+
+    # 计算旋转矩阵的角度差
+    r1 = R.from_matrix(rotation1)
+    r2 = R.from_matrix(rotation2)
+
+    # 计算旋转差
+    relative_rotation = r1.inv() * r2
+    angle_difference = relative_rotation.magnitude()
+
+    return position_distance, np.degrees(angle_difference)
+