31 lines
707 B
Python
31 lines
707 B
Python
""" 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)
|
|
|