由空间三对点求解两个坐标系之间的变换矩阵，python求解

import numpy as np

def compute_transform(A_points, B_points):
    """
    由三对对应点计算两个坐标系之间的变换矩阵（4x4齐次矩阵）
    :param A_points: 源坐标系中的三个点，形状为(3, 3)，每行是一个点的[x, y, z]
    :param B_points: 目标坐标系中的对应点，形状同上
    :return: 4x4变换矩阵
    """
    # 计算中心点
    centroid_A = np.mean(A_points, axis=0)
    centroid_B = np.mean(B_points, axis=0)
    
    # 去中心化
    A_centered = A_points - centroid_A
    B_centered = B_points - centroid_B
    
    # 计算协方差矩阵H
    H = A_centered.T @ B_centered
    
    # 奇异值分解
    U, S, Vt = np.linalg.svd(H)
    
    # 计算旋转矩阵
    R = Vt.T @ U.T
    
    # 处理反射情况，确保行列式为1
    if np.linalg.det(R) < 0:
        Vt[2, :] *= -1
        R = Vt.T @ U.T
    
    # 计算平移向量
    t = centroid_B - R @ centroid_A
    
    # 组合成齐次变换矩阵
    transform = np.eye(4)
    transform[:3, :3] = R
    transform[:3, 3] = t
    
    return transform

# 示例用法
if __name__ == "__main__":
    # 示例点对：A_points是源坐标系中的点，B_points是目标坐标系中的对应点
    A_points = np.array([
        [1, 0, 0],
        [0, 1, 0],
        [0, 0, 1]
    ])
    # 假设变换为绕z轴旋转90度，平移[1, 2, 3]
    R_true = np.array([
        [0, -1, 0],
        [1, 0, 0],
        [0, 0, 1]
    ])
    t_true = np.array([1, 2, 3])
    B_points = (R_true @ A_points.T).T + t_true
    
    # 计算变换矩阵
    transform = compute_transform(A_points, B_points)
    print("变换矩阵:")
    print(transform)

结果：