add delta_pose_diff

2024-08-22 22:42:34 +08:00
parent 954fed1122
commit eceedd5c15
2 changed files with 24 additions and 5 deletions
--- a/utils/pose.py
+++ b/utils/pose.py
@@ -220,6 +220,13 @@ class PoseUtil:
    @staticmethod
    def matrix_to_rotation_6d_numpy(matrix):
        return np.copy(matrix[:2, :]).reshape((6,))
+    
+    @staticmethod
+    def rotation_angle_distance(R1, R2):
+        R = torch.matmul(R1, R2.transpose(1, 2))
+        trace = torch.diagonal(R, dim1=1, dim2=2).sum(-1)
+        angle = torch.acos(torch.clamp((trace - 1) / 2, -1.0, 1.0))/torch.pi*180
+        return angle


 """ ------------ Debug ------------ """