3d空间内已知3对点 用numpy求刚性变换矩阵 写出代码实现

在三维空间中，给定三个对应点对（通常是两个物体在不同姿态下的同一点），可以利用最小二乘法求解刚体变换矩阵（通常包括旋转和平移）。这里我们假设您已经知道源坐标系下的一组点 \( P_1 \), \( P_2 \), 和 \( P_3 \)，以及目标坐标系下的对应点 \( Q_1 \), \( Q_2 \), \( Q_3 \)。

首先，我们需要导入 numpy 库，然后按照以下步骤计算：

import numpy as np

# 假设我们有两组3D点，每组3个点
src_points = np.array([[p_x1, p_y1, p_z1], [p_x2, p_y2, p_z2], [p_x3, p_y3, p_z3]])
dst_points = np.array([[q_x1, q_y1, q_z1], [q_x2, q_y2, q_z2], [q_x3, q_y3, q_z3]])

# 定义变换矩阵 R (旋转部分) 和 t (平移部分)
R = np.eye(3)  # 初始状态假设没有旋转，即单位矩阵
t = np.zeros(3)  # 初始状态假设没有平移，即零向量

# 对应点差分
point_diffs = dst_points - src_points

# 使用 SVD 算法（奇异值分解）来找到最小二乘旋转矩阵
U, S, Vh = np.linalg.svd(point_diffs @ point_diffs.T)

# 从Vh的最后一列取旋转矩阵 R
R = U[:, -1]

# 计算平移向量 t
t = np.mean(point_diffs, axis=0)

# 结合旋转和平移得到变换矩阵 T
T = np.block([[R, t], [np.zeros((1, 3)), 1]])

print("刚性变换矩阵 T:\n", T)

注意：这个代码示例是在理想化情况下的计算，如果数据噪声较大或者点对不足以完全确定变换，结果可能会存在误差。实际应用中，你可能需要考虑使用更稳健的方法，如 Levenberg-Marquardt 非线性最小二乘算法来处理这种情况。