Python版的icp

采用Python编写的最近点迭代算法：

import numpy as np
from scipy.spatial import KDTree
import open3d as o3d

# 读取点云数据
def read_point_cloud(file_path):
    pcd = o3d.io.read_point_cloud(file_path)
    return np.asarray(pcd.points)

# 将点从3维转换为4维齐次坐标
def points_to_homogeneous(points):
    return np.hstack([points, np.ones((points.shape[0], 1))])

# 将点从4维齐次坐标转换为3维
def points_from_homogeneous(points_homogeneous):
    return points_homogeneous[:, :3]

# 求解最优刚体变换矩阵
def compute_rigid_transform(A, B):
    assert len(A) == len(B)

    N = A.shape[0]  # 点数
    centroid_A = np.mean(A, axis=0)
    centroid_B = np.mean(B, axis=0)

    AA = A - centroid_A
    BB = B - centroid_B

    H = AA.T @ BB
    U, S, Vt = np.li