""" 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)