icp点云配准python代码

ICP（迭代最近点）是最常用的点云配准方法之一。下面是一个简单的用Python实现的ICP点云配准代码：

import numpy as np
from scipy.spatial import KDTree

def icp(source, target, max_iterations=100, tolerance=0.001):
    iterations = 0
    
    while iterations < max_iterations:
        tree = KDTree(target)
        distances, indices = tree.query(source)
        
        # 计算转换矩阵
        H = np.eye(4)
        for i, index in enumerate(indices):
            H += np.outer(source[i], target[index])
        U, _, V = np.linalg.svd(H[:3, :3])
        R = V.T @ U.T
        
        t = np.mean(target[indices] - R @ source, axis=0)
        
        # 应用转换矩阵
        transformed = R @ source + t
        
        # 计算误差
        error = np.mean(np.linalg.norm(transformed - target[indices], axis=1))
        
        source = transformed
        
        if error < tolerance:
            break
            
        iterations += 1
        
    return source

# 示例用法
source = np.array([[1, 1, 1],
                   [2, 2, 2],
                   [3, 3, 3]])
target = np.array([[2, 2, 2],
                   [3, 3, 3],
                   [4, 4, 4]])

result = icp(source, target)

print(result)