Python 罗德里格矩阵的空间坐标转换——两组公共点求所属坐标系的旋转矩阵与平移矩阵

import numpy as np

# 罗德里格矩阵模型，根据公共点计算旋转矩阵与平移矩阵
def RodriguesMatrixModel(src, dst):
    # 计算比例关系
    scale = np.sum(np.sqrt(np.sum((dst - np.mean(dst, axis=0)) ** 2, axis=1))) / np.sum(np.sqrt(np.sum((src - np.mean(src, axis=0)) ** 2, axis=1)))
    # 计算旋转矩阵
    src_mean = np.mean(src, axis=0)
    dst_mean = np.mean(dst, axis=0)
    src = src - src_mean
    dst = dst - dst_mean
    H = np.dot(src.T, dst)
    U, S, Vt = np.linalg.svd(H)
    R = np.dot(Vt.T, U.T)
    if np.linalg.det(R) < 0:
        Vt[2, :] *= -1
        R = np.dot(Vt.T, U.T)
    # 计算平移矩阵
    t = dst_mean.T - scale * np.dot(R, src_mean.T)
    return R, t, scale


# TEST
if __name__ == '__main__':   
     # 设置原点
    src = np.array([[1, 1, 0], [-1, 1, 0], [1, -1, 0]])
    # 绕Z轴旋转90°，平移(-1, 1, 0)
    R = np.array([[0, -1, 0], [1, 0, 0], [0, 0, 1]])
    t = np.array([[-1], [1], [0]])
    # 计算目标点
    dst = np.dot(src, R.T) + t.T
    print('src = ', src)
    print('dst = ', dst)
    # 计算旋转矩阵与平移矩阵
    R_, t_, scale_ = RodriguesMatrixModel(src, dst)
    # 输出R_保留一位小数
    print('R = ', np.round(R_, 1))
    print('t = ', t_)
    print('scale = ', scale_)