第三章：点云配准——让多视角点云“对齐”的艺术

点云配准，说白了就是把不同位置、不同角度拍到的点云，拼成一个完整的模型。我刚开始接触这个领域时，觉得这跟拼图差不多——把碎片对齐就行了。但真正上手才发现，三维空间里的“对齐”，远比想象中复杂。

这一章，我会带你从最经典的ICP算法讲起，再到更鲁棒的NDT，最后聊聊粗配准怎么给精配准“铺路”。嗯，都是实战中反复用到的技术。

3.1 ICP算法——最经典的迭代最近点

ICP（Iterative Closest Point）是点云配准的“老大哥”。它的思路很直接：对于源点云中的每个点，在目标点云中找到最近邻，然后求解一个刚体变换（旋转+平移），让这些点对的距离之和最小。重复这个过程，直到收敛。

我当年第一次实现ICP时，觉得这算法太简单了——不就是找最近点、算变换、再找最近点嘛。结果跑出来的结果惨不忍睹。后来才明白，ICP对初始位置非常敏感。你想想看，如果两个点云初始偏差太大，最近邻匹配就是错的，迭代再多次也白搭。

3.1.1 ICP的数学本质

ICP的核心是最小化以下目标函数：

E(R, t) = Σ || R·p_i + t - q_i ||²

其中p_i是源点云中的点，q_i是目标点云中对应的最近点。R是旋转矩阵，t是平移向量。

求解这个优化问题，通常用SVD分解。我记得第一次手推SVD求解时，被那个协方差矩阵的构造绕晕了。后来发现，PCL和Open3D都封装好了，直接调用就行。但理解原理还是必要的——至少你知道什么时候该用SVD，什么时候该用其他方法。

3.1.2 ICP的代码实现

用Open3D实现ICP非常简洁。我习惯先做一步降采样，能大幅提升速度：

import open3d as o3d
import numpy as np

# 读取点云
source = o3d.io.read_point_cloud("source.pcd")
target = o3d.io.read_point_cloud("target.pcd")

# 降采样（我一般用0.05m的体素大小）
source_down = source.voxel_down_sample(0.05)
target_down = target.voxel_down_sample(0.05)

# 设置ICP参数
threshold = 0.02  # 对应点距离阈值
trans_init = np.eye(4)  # 初始变换矩阵

# 执行ICP
reg_p2p = o3d.pipelines.registration.registration_icp(
    source_down, target_down, threshold, trans_init,
    o3d.pipelines.registration.TransformationEstimationPointToPoint(),
    o3d.pipelines.registration.ICPConvergenceCriteria(max_iteration=200)
)

print("变换矩阵:\n", reg_p2p.transformation)
print("适应度:", reg_p2p.fitness)
print("RMSE:", reg_p2p.inlier_rmse)

这里有个坑——threshold参数。设得太小，匹配点对太少，算法容易发散；设得太大，又会引入错误匹配。我一般先设0.02m，看结果再调整。

3.2 ICP调优——那些年我踩过的坑

ICP调优是个手艺活。我总结了几个关键点：

• 初始位姿：这是最重要的。没有好的初值，ICP就是瞎子摸象。后面会讲FPFH+RANSAC做粗配准，就是解决这个问题的。
• 点云密度：太密了计算慢，太疏了精度差。我习惯用0.02-0.05m的体素降采样。
• 对应点筛选：除了距离阈值，还可以用法向量一致性过滤。如果两个点的法向量夹角太大，即使距离很近，也不该匹配。
• 收敛判据：别只看迭代次数。我一般同时检查变换矩阵的变化量和RMSE的变化量。

3.3 NDT算法——另一种思路

NDT（Normal Distributions Transform）跟ICP的思路完全不同。ICP是在点层面做匹配，NDT则把空间划分成网格，每个网格用高斯分布建模。匹配时，不是找点对点，而是让源点云的点落在目标网格的概率最大。

我刚开始觉得NDT不如ICP直观，但用了几次后发现，NDT对初始位姿的容忍度更高。而且它不需要显式地找最近邻，计算效率在某些场景下反而更好。

3.3.1 NDT的核心步骤

1. 将目标点云划分成体素网格（比如1m×1m×1m）
2. 对每个网格内的点，计算均值和协方差矩阵
3. 对于源点云中的每个点，找到它落在哪个网格，计算该点的高斯概率密度
4. 最大化所有点的概率密度之和，求解变换矩阵

这里有个细节——网格大小很关键。网格太大，分布太粗糙，精度不够；网格太小，每个网格里的点太少，统计意义不强。我一般根据点云密度来调，通常0.5m到2m之间。

# Open3D中的NDT实现
reg_ndt = o3d.pipelines.registration.registration_ndt(
    source_down, target_down, 0.5, trans_init,
    o3d.pipelines.registration.ICPConvergenceCriteria(max_iteration=50)
)

你看，代码跟ICP很像，只是换了个注册方法。但背后的数学原理完全不同。

3.4 粗配准——FPFH + RANSAC

前面说了，ICP和NDT都需要一个好的初始位姿。那初始位姿从哪来？粗配准就是干这个的。

粗配准的思路是：先提取点云的特征点，然后根据特征描述子找到对应关系，最后用RANSAC剔除错误匹配，估算一个初始变换。

3.4.1 FPFH特征描述子

FPFH（Fast Point Feature Histogram）是一种局部特征描述子。它描述的是点云中每个点周围的几何信息——法向量夹角、距离等。我理解它就像人的指纹，每个点都有独特的FPFH特征。

提取FPFH的步骤：

1. 计算每个点的法向量
2. 对每个点，计算它和邻域点的相对关系（角度、距离）
3. 把这些关系统计成直方图，就是FPFH

# 计算FPFH特征
source_fpfh = o3d.pipelines.registration.compute_fpfh_feature(
    source_down, 
    o3d.geometry.KDTreeSearchParamHybrid(radius=0.25, max_nn=100)
)

target_fpfh = o3d.pipelines.registration.compute_fpfh_feature(
    target_down,
    o3d.geometry.KDTreeSearchParamHybrid(radius=0.25, max_nn=100)
)

这里radius参数要跟点云尺度匹配。我一般取点云平均间距的5-10倍。

3.4.2 RANSAC粗配准

有了FPFH特征，就可以用RANSAC找对应关系了。RANSAC的思路是：随机选3对匹配点，算一个变换，看有多少其他点支持这个变换。重复多次，选支持度最高的那个。

# RANSAC粗配准
result_ransac = o3d.pipelines.registration.registration_ransac_based_on_feature_matching(
    source_down, target_down, source_fpfh, target_fpfh,
    mutual_filter=True,
    max_correspondence_distance=0.075,
    estimation_method=o3d.pipelines.registration.TransformationEstimationPointToPoint(False),
    ransac_n=3,
    checkers=[
        o3d.pipelines.registration.CorrespondenceCheckerBasedOnEdgeLength(0.9),
        o3d.pipelines.registration.CorrespondenceCheckerBasedOnDistance(0.075)
    ],
    criteria=o3d.pipelines.registration.RANSACConvergenceCriteria(100000, 0.999)
)

这段代码里，我特别想提一下mutual_filter参数。设为True时，只有双向匹配的点对才被保留——也就是A的最近点是B，且B的最近点也是A。这能过滤掉大量错误匹配。

3.5 配准质量评估

配准做完了，怎么知道好不好？我一般看三个指标：

指标	含义	好结果	坏结果
Fitness	匹配点对占比	> 0.6	< 0.3
RMSE	匹配点对均方根误差	< 0.02m	> 0.1m
可视化	肉眼观察重叠区域	边缘对齐	有错位

不过说实话，指标只是参考。我见过fitness很高但明显错位的情况——因为匹配点对都集中在某个局部区域。所以，最终还是要靠肉眼确认。嗯，这也是为什么做点云处理，可视化能力很重要。

这一章的内容就到这里。配准是点云处理的核心技术之一，ICP和NDT各有优劣，FPFH+RANSAC则是解决初值问题的利器。多动手试试，你会找到最适合自己场景的方案。