点云配准ICP算法python

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


def translate(points, t):
    return points + t

def rotate(points, R):
    return np.dot(points, R.T)

def nearest_neighbor(src, dst):
    tree = KDTree(dst)
    distances, indices = tree.query(src, k=1)
    return indices

def compute_transform(src, dst):
    src_mean = np.mean(src, axis=0)
    dst_mean = np.mean(dst, axis=0)
    src_centered = src - src_mean
    dst_centered = dst - dst_mean
    H = np.dot(src_centered.T, dst_centered)
    U, _, Vt = np.linalg.svd(H)
    R = np.dot(Vt.T, U.T)
    t = dst_mean - np.dot(R, src_mean)
    return R, t

def icp(src, dst, max_iterations=100, tolerance=1e-6):
    src_points = np.copy(src)
    dst_points = np.copy(dst)
    R_total = np.eye(src.shape[1])
    t_total = np.zeros(src.shape[1])
    prev_error = 0
    for i in range(max_iterations):
        indices = nearest_neighbor(src_points, dst_points)
        corresponding_points = dst_points[indices]
        R, t = compute_transform(src_points, corresponding_points)
        src_points = rotate(src_points, R)
        src_points = translate(src_points, t)
        R_total = np.dot(R, R_total)
        t_total = np.dot(R, t_total) + t
        mean_error = np.mean(np.linalg.norm(corresponding_points - src_points, axis=1))
        print(f'Iteration {i+1}: Mean Error = {mean_error}')
        if abs(prev_error - mean_error) < tolerance:
            break
        prev_error = mean_error
    return R_total, t_total


if __name__ == "__main__":
    source = o3d.io.read_point_cloud(r'C:\Users\1900\Desktop\source-Cloud.pcd')
    target = o3d.io.read_point_cloud(r'C:\Users\1900\Desktop\target-Cloud.pcd')
    source_points = np.array(source.points)
    target_points = np.array(target.points)
    R_est, t_est = icp(source_points, target_points)
    icp_points = rotate(source_points, R_est)
    icp_points = translate(icp_points, t_est)
    print("\nEstimated Rotation Matrix:", R_est)
    print("\nEstimated Translation Vector:", t_est)
    result = o3d.geometry.PointCloud()
    result.points = o3d.utility.Vector3dVector(icp_points)
    o3d.visualization.draw_geometries([source, result, target])
    o3d.io.write_point_cloud(r"C:\Users\1900\Desktop\result_icp.pcd", result)

不是我写的 不知道从哪个地方抠出来  改改发现能用 感觉效果有点奇怪 视情况而定 也算是配的准