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

在Python中，我们可以使用NumPy库来处理这种线性代数问题，特别是计算刚体变换矩阵。刚体变换通常涉及到平移、旋转和平面缩放。这里假设我们有三对对应点，通过最小二乘法可以估计旋转变换矩阵（如果只有平移和旋转），或者欧拉角（如果还有缩放）。以下是使用NumPy实现的一个简化版本：

import numpy as np

# 假设我们有三个点对 (p0_i, p1_i) 和 (q0_i, q1_i)，其中 i = 1, 2, 3
points_pair_1 = np.array([[x1, y1], [x2, y2]])  # 根据实际数据填充
points_pair_2 = np.array([[x3, y3], [x4, y4]])
points_pair_3 = np.array([[x5, y5], [x6, y6]])

# 将所有点对组合成一个大数组
all_points = np.concatenate((points_pair_1, points_pair_2, points_pair_3))

# 对应目标位置的点
target_points = np.array([[y01, x01], [y02, x02], [y03, x03]])  # 根据实际数据填充

# 平均值作为初始中心点
mean_point = all_points.mean(axis=0)

# 新的坐标系下，去除平均点的影响
shifted_all_points = all_points - mean_point[:, None]

# 计算协方差矩阵
covariance_matrix = np.cov(shifted_all_points.T)

# 如果只关心旋转变换，忽略缩放，解奇异值分解得到旋转矩阵 R
if np.allclose(covariance_matrix[0, 1], covariance_matrix[1, 0]):
    # 简单的二维情况
    rotation_angle = np.arctan2(covariance_matrix[1, 1], covariance_matrix[0, 0])
    R = np.array([[np.cos(rotation_angle), -np.sin(rotation_angle)],
                  [np.sin(rotation_angle), np.cos(rotation_angle)]])
else:
    # 解SVD获取旋转矩阵 R 和缩放因子 s
    U, S, Vh = np.linalg.svd(covariance_matrix)
    R = Vh.dot(U.T)

# 组合旋转和平移变换
scale_factor = np.sqrt(S[0] * S[1])  # 取对角线元素的平方根作为缩放因子
translation = target_points.mean(axis=0) - R @ mean_point

# 刚体变换矩阵 T
transform_matrix = np.eye(3)
transform_matrix[:2, :2] = R
transform_matrix[:2, 2] = translation

# 相关问题:
1. 这段代码适用于哪些类型的点对？
2. 如何处理更复杂的情况，比如需要考虑缩放？
3. 如果数据噪声较大，如何改进求解过程？