激光雷达点云配准：ICP 与 NDT 算法深度解析
在自动驾驶和机器人 SLAM 系统中，激光雷达采集的点云数据往往来自不同时刻或不同视角。如何将它们对齐到同一坐标系下，就是点云配准（Point Cloud Registration）要解决的问题。今天我们来聊聊最经典的两种算法：ICP 和 NDT。
一、点云配准基础概念
简单来说，点云配准就是通过寻找一个刚体变换（旋转和平移），使得源点云和目标点云在空间上尽可能重合。评价配准好坏，通常看两个指标：一是残差距离越小越好，二是计算时间越短越好。
二、ICP 算法详解
ICP（Iterative Closest Point）算是老生常谈了，但依然是工业界最常用的基准。
1. 基本原理
核心思想很简单：找到对应点，算变换，重复直到收敛。每次迭代里，先找源点云中每个点最近的邻居作为目标点，然后计算最小化这些点对距离的变换矩阵。
2. 流程步骤
实际跑起来大概是这样：
- 初始化变换矩阵（通常是单位矩阵）。
- 遍历源点云，在目标点云中找最近邻。
- 用找到的点对计算最优变换（常用 SVD 分解）。
- 应用变换更新源点云。
- 检查误差是否小于阈值，没满足就回到第 2 步。
3. 代码实战
下面是一个基于 Open3D 的简单实现，注意这里用了法向量来加速匹配：
import open3d as o3d
import numpy as np
def icp_registration(source, target, threshold=0.02):
# 确保有法向量
source.compute_vertex_normals()
target.compute_vertex_normals()
# 执行 ICP
trans_init = np.eye(4)
reg_p2p = o3d.pipelines.registration.registration_icp(
source, target, threshold, trans_init,
o3d.pipelines.registration.TransformationEstimationPointToPlane(),
o3d.pipelines.registration.ICPConvergenceCriteria(max_iteration=50)
)
return reg_p2p.transformation
# 使用示例
# source_cloud = o3d.io.read_point_cloud("source.pcd")
# target_cloud = o3d.io.read_point_cloud("target.pcd")
# T = icp_registration(source_cloud, target_cloud)
4. 优缺点分析
优点很直观：精度高，数学理论成熟。缺点也很明显：容易陷入局部最优，非常依赖初始位置。如果两张图相差太大，ICP 基本会失效。
三、NDT 算法详解
NDT（Normal Distributions Transform）是为了解决 ICP 在大位移下不稳定的问题而生的。
1. 基本原理
它不把点看作孤立的个体，而是把空间划分成网格（体素），在每个网格里假设点服从高斯分布。通过最大化概率密度来寻找最佳变换。
