2d 点云配准

import numpy as np
def kabsch_2d(P, Q):
    """
    二维点云配准算法，使用Kabsch算法。
    inputs: P  N x 2 numpy矩阵，表示点集P中的二维坐标点
            Q  N x 2 numpy矩阵，表示点集Q中的二维坐标点
    return: A 3 x 2矩阵，前两行为旋转矩阵，最后一行为平移向量
    """
    if (P.size == 0 or Q.size == 0):
        raise ValueError("Empty matrices sent to kabsch")
    centroid_P = np.mean(P, axis=0)
    centroid_Q = np.mean(Q, axis=0)
    P_centered = P - centroid_P
    Q_centered = Q - centroid_Q
    H = P_centered.T.dot(Q_centered)
    U, S, V = np.linalg.svd(H)
    R = U.dot(V).T
    if np.linalg.det(R) < 0:
        V[1, :] *= -1
        R = U.dot(V).T
    t = centroid_Q - R.dot(centroid_P)
    return np.vstack((R, t))
if __name__ == '__main__':
    np.random.seed(0)
    num_points = 5
    P = np.random.rand(num_points, 2)  # 5个二维点的集合P
    # 对Q进行旋转和平移来模拟配准之后的点云
    rotation_angle = np.deg2rad(30)  # 旋转角度为30度
    translation_vector = np.array([0.2, 0.3])  # 平移向量为(0.2, 0.3)
    rotation_matrix = np.array([[np.cos(rotation_angle), -np.sin(rotation_angle)],
                                [np.sin(rotation_angle), np.cos(rotation_angle)]])
    Q = P.dot(rotation_matrix.T) + translation_vector  # 对P进行旋转和平移得到Q
    print("Q",Q)
    # 调用二维点云配准函数
    transform_matrix = kabsch_2d(P, Q)
    print("Transform Matrix (Rotation + Translation):\n", transform_matrix)

    # 将P进行变换，应该与Q尽可能地接近
    transformed_P = P.dot(transform_matrix[:2, :2].T) + transform_matrix[2, :]
    print("Transformed P:\n", transformed_P)