点云匹配算法

0. 点云匹配到底在匹配什么？

假设有两帧点云：

P = {p1, p2, p3, ...} // source，当前帧点云 Q = {q1, q2, q3, ...} // target，目标点云 / 地图点云

点云匹配要找一个刚体变换：

T = [R, t]

其中：

R：旋转矩阵，表示转多少角度 t：平移向量，表示移动多少距离

把当前点云里的点p_i变到目标坐标系：

p_i' = R * p_i + t

如果匹配正确，变换后的p_i'应该和目标点云Q对齐。

总目标可以写成：

┌──────────────────────────────────────────────┐ │ 点云匹配总目标 │ │ │ │ T* = argmin Σ error(T * p_i, Q) │ │ │ │ T*：最优位姿变换 │ │ p_i：当前点云中的点 │ │ Q：目标点云 / 地图点云 │ │ error：当前点变换后和目标点云之间的误差 │ └──────────────────────────────────────────────┘

一句话：点云匹配就是不断调整 R 和 t，让当前点云和目标点云之间的误差最小。

1. Point-to-Point ICP：点到点 ICP

这是最经典、最基础的点云匹配算法。ICP 全称是Iterative Closest Point，迭代最近点算法。

它的思路很简单：

1. 当前点云 P 里取一个点 p_i； 2. 在目标点云 Q 里找离它最近的点 q_i； 3. 认为 p_i 和 q_i 是一对匹配点； 4. 求一个 R,t，让所有 p_i 尽量靠近对应的 q_i； 5. 更新位姿后，重新找最近点； 6. 重复迭代，直到误差不怎么变了。

公式：

┌──────────────────────────────────────────────┐ │ Point-to-Point ICP 目标函数 │ │ │ │ T* = argmin Σ || R * p_i + t - q_i ||² │ │ │ │ p_i：当前点云 source 中的点 │ │ q_i：目标点云 target 中离 p_i 最近的点 │ │ R：旋转矩阵 │ │ t：平移向量 │ │ || ||²：欧氏距离平方 │ └──────────────────────────────────────────────┘

这里的误差就是：

R * p_i + t - q_i

意思是：当前点p_i经过旋转和平移后，和目标点q_i之间还差多少。

举个例子：当前点云是一把椅子，目标点云也是同一把椅子，但是当前点云偏了 0.5 米、转了 10°。ICP 就不断找最近点，然后估计一个旋转和平移，把当前椅子点云慢慢对齐到目标椅子点云。

优点是简单、经典、容易实现。缺点是非常依赖初值。如果初始位姿差太大，最近点可能找错，最后就会匹配到错误位置。

2. Point-to-Plane ICP：点到面 ICP

Point-to-Point ICP 是让点靠近点，但点云很多时候来自墙面、地面、桌面。对于这些平面区域，“点到点”不如“点到面”稳定。

Point-to-Plane ICP 的思想是：

当前点 p_i 变换后，不一定要贴到目标点 q_i 上； 它只要落在 q_i 所在的局部平面上就可以。

公式：

┌────────────────────────────────────────────────────────────┐ │ Point-to-Plane ICP 目标函数 │ │ │ │ T* = argmin Σ [ n_i^T * (R * p_i + t - q_i) ]² │ │ │ │ p_i：当前点云中的点 │ │ q_i：目标点云中对应的最近点 │ │ n_i：q_i 所在局部平面的法向量 │ │ R：旋转矩阵 │ │ t：平移向量 │ │ n_i^T * (...)：点到平面的有符号距离 │ └────────────────────────────────────────────────────────────┘

这里的n_i是法向量。比如一面墙的法向量垂直于墙面。如果当前点变换后落在墙面上，那么它到墙面的垂直距离应该接近 0。

点到面距离可以这样理解

d = n_i^T * (p_i' - q_i)

其中：

p_i' = R * p_i + t

所以：

d = n_i^T * (R * p_i + t - q_i)

如果d = 0，说明当前点刚好落在目标平面上。

Point-to-Plane ICP 通常比 Point-to-Point ICP 收敛更快，尤其适合室内墙面、地面、走廊这种平面结构明显的场景。但它需要目标点云有法向量，法向量一般通过邻域点 PCA 计算。

法向量怎么来？取目标点q_i周围的一些点，拟合一个局部平面：

ax + by + cz + d = 0

其中：

[a, b, c]

就是平面法向量n_i。

3. Generalized ICP：GICP，广义 ICP

GICP 可以理解成 Point-to-Point 和 Point-to-Plane 的升级版。普通 ICP 认为每个点只是一个点，但 GICP 认为每个点附近都有一个局部形状，比如是平面、边缘，还是比较散。

它给每个点建一个协方差矩阵：

C_i

协方差可以理解为这个点附近的形状分布。如果点在平面上，那么点沿平面方向变化大，沿法向方向变化小；如果点在边缘上，分布又不一样。

GICP 目标函数大概是：

┌──────────────────────────────────────────────────────────────┐ │ GICP 目标函数 │ │ │ │ T* = argmin Σ d_i^T * (C_i^Q + R * C_i^P * R^T)^(-1) * d_i │ │ │ │ d_i = q_i - (R * p_i + t) │ │ │ │ p_i：source 点 │ │ q_i：target 对应点 │ │ C_i^P：source 点邻域协方差 │ │ C_i^Q：target 点邻域协方差 │ │ R,t：待求旋转和平移 │ └──────────────────────────────────────────────────────────────┘

这里的核心是：

(C_i^Q + R * C_i^P * R^T)^(-1)

它相当于告诉优化器：不同方向上的误差权重不一样。比如在墙面上，沿墙面方向误差可以稍微大一点，但垂直墙面的误差更重要。

你可以这样理解：

Point-to-Point ICP：每个点就是一个点。 Point-to-Plane ICP：目标点附近是一个平面。 GICP：source 和 target 两边都考虑局部几何分布。

GICP 通常比普通 ICP 更稳，但计算量更大。

4. NDT：Normal Distributions Transform，正态分布变换

NDT 是自动驾驶和激光定位里非常常见的点云匹配算法。它和 ICP 最大区别是：ICP 直接找点到点的对应关系，NDT 不直接存所有点，而是把目标点云划分成很多格子，每个格子用一个高斯分布表示。

先把目标点云分成很多体素格子：

voxel 1, voxel 2, voxel 3, ...

每个格子里面有一些点，对这些点计算均值和协方差：

μ：均值，表示这个格子里点云中心 Σ：协方差，表示这个格子里点云分布形状

然后当前点云经过变换后，如果落到某个格子里，就看它在这个格子的高斯分布概率高不高。

高斯概率大概是：

┌────────────────────────────────────────────────────────────┐ │ NDT 单点概率模型 │ │ │ │ P(x) ∝ exp( -1/2 * (x - μ)^T * Σ^(-1) * (x - μ) ) │ │ │ │ x：当前点变换后的坐标 │ │ μ：目标体素格子的均值 │ │ Σ：目标体素格子的协方差 │ │ Σ^(-1)：协方差逆矩阵，表示不同方向上的权重 │ └────────────────────────────────────────────────────────────┘

匹配目标是让当前点云变换后，在目标 NDT 地图里的概率最大。通常写成最小化负对数概率：

┌────────────────────────────────────────────────────────────┐ │ NDT 优化目标 │ │ │ │ T* = argmin Σ (x_i - μ_i)^T * Σ_i^(-1) * (x_i - μ_i) │ │ │ │ x_i = R * p_i + t │ │ │ │ p_i：当前点云点 │ │ x_i：变换后的当前点 │ │ μ_i：x_i 所在体素格子的均值 │ │ Σ_i：该体素格子的协方差 │ └────────────────────────────────────────────────────────────┘

NDT 的特点是不用每个点都去找最近邻，而是匹配到概率分布上，所以在大规模地图定位中很常用。缺点是体素大小很关键。体素太大，地图细节丢失；体素太小，每个格子点太少，高斯分布不稳定。

NDT 适合：

大范围激光定位； 自动驾驶已有地图定位； 初值还可以但点云数量较大的场景。

5. Correlative Scan Matching：相关性扫描匹配

这个算法在 2D LiDAR、栅格地图定位里非常常见，Cartographer 里也有类似思想。它的核心不是最近邻，而是：在一堆候选位姿里搜索，看哪个位姿下当前点云压到地图障碍物上的得分最高。

假设有一个栅格地图：

M(x, y)

每个格子有一个占用概率：

M(x, y) 越大，说明这个地方越可能是障碍物

当前激光点是：

p_i

给定一个候选位姿：

T = [x, y, θ]

把当前点云变到地图上，计算得分：

score(T) = Σ M(T * p_i)

也就是：

当前点云经过 T 变换后，落在地图高概率障碍物上的点越多，score 越高。

公式：

┌────────────────────────────────────────────────────────────┐ │ Correlative Scan Matching 目标 │ │ │ │ T* = argmax Σ M(T * p_i) │ │ │ │ T：候选位姿 [x, y, θ] │ │ p_i：当前激光点 │ │ M：栅格地图，占用概率地图 │ │ M(T * p_i)：当前点变换到地图后对应格子的占用概率 │ └────────────────────────────────────────────────────────────┘

它通常会在一个搜索窗口里枚举：

x ∈ [-0.5m, 0.5m] y ∈ [-0.5m, 0.5m] θ ∈ [-10°, 10°]

找得分最高的位姿。

优点是对初值比 ICP 更宽容，因为它可以在一定范围内全局搜索。缺点是搜索范围越大，计算量越大。为了加速，常用多分辨率、branch-and-bound 分支定界。

这种方法适合：

2D LiDAR 定位； 栅格地图匹配； 有较大初始误差时的粗匹配； Cartographer 这类框架。

6. LOAM / LIO-SAM 特征点匹配：点到线 + 点到面

LOAM、LeGO-LOAM、LIO-SAM 这类框架常用特征点匹配。它们不是拿全部点做 ICP，而是先提取两类点：

corner feature：角点 / 边缘点 surface feature：平面点

然后做两种残差：

角点 -> 点到线残差 平面点 -> 点到面残差

6.1 角点：点到线残差

当前帧角点变换到地图系后是：

p

在地图角点云中找最近的若干点，用 PCA 判断它们是不是一条线。如果是，构造线上的两个点：

a, b

点到线距离是：

┌──────────────────────────────────────────────┐ │ 点到线残差 │ │ │ │ d_line = || (p - a) × (p - b) || / || a - b ||│ │ │ │ p：当前角点变换到地图系后的坐标 │ │ a,b：地图线上的两个点 │ │ ×：叉乘 │ │ d_line：点到线的垂直距离 │ └──────────────────────────────────────────────┘

优化目标：

min Σ d_line²

含义是：当前帧角点应该贴近地图里的边缘线。

6.2 平面点：点到面残差

当前帧平面点变换到地图系后是：

p = [x, y, z]

在地图平面点云中找最近若干点，拟合平面：

ax + by + cz + d = 0

点到平面距离：

┌──────────────────────────────────────────────┐ │ 点到平面残差 │ │ │ │ d_plane = a*x + b*y + c*z + d │ │ │ │ [a,b,c]：平面法向量 │ │ d：平面偏移 │ │ [x,y,z]：当前点坐标 │ │ d_plane：点到平面的有符号距离 │ └──────────────────────────────────────────────┘

优化目标：

min Σ d_plane²

总目标：

┌──────────────────────────────────────────────┐ │ LOAM / LIO-SAM scan-to-map 目标 │ │ │ │ T* = argmin Σ d_line_i² + Σ d_plane_j² │ │ │ │ d_line_i：第 i 个角点到地图线的距离 │ │ d_plane_j：第 j 个平面点到地图面的距离 │ └──────────────────────────────────────────────┘

这种方法的优点是效率高，不用所有点都参与优化，而且边缘和平面有明确几何意义。缺点是特征提取质量很关键，环境退化时，比如长走廊、空旷场景、重复结构，会影响匹配稳定性。

7. RANSAC + 特征描述子匹配

这类方法通常用于初始粗配准。它不依赖初始位姿太准，而是先从点云中提取关键点和描述子。

常见描述子有：

FPFH SHOT PFH

流程：

1. 从 source 和 target 点云中提取关键点； 2. 为每个关键点计算描述子； 3. 根据描述子相似度建立匹配对； 4. 用 RANSAC 剔除错误匹配； 5. 估计一个粗位姿； 6. 再用 ICP / NDT 精配准。

RANSAC 的思想是随机选少量匹配点，估计一个变换，然后看有多少点支持这个变换。

目标可以理解成：

┌──────────────────────────────────────────────┐ │ RANSAC 配准目标 │ │ │ │ 找 T，使满足 || R*p_i + t - q_i || < threshold │ │ 的匹配点数量最多 │ │ │ │ p_i, q_i：一对候选匹配点 │ │ threshold：内点距离阈值 │ │ 内点：符合当前变换的匹配点 │ └──────────────────────────────────────────────┘

RANSAC 不是追求所有点误差最小，而是先找一个“被最多匹配点支持”的粗变换。

适合：

初值很差； 两帧点云差异大； 需要全局粗配准； 给 ICP/NDT 提供初始位姿。

缺点是描述子计算比较慢，而且点云稀疏或重复结构多时，描述子容易误匹配。

8. CPD：Coherent Point Drift，概率点云配准

CPD 是一种概率配准方法。它把一组点云看成高斯混合模型，然后让另一组点云去拟合这个概率模型。

小白理解：不是硬找最近点，而是认为目标点云是一些概率中心，当前点云点属于这些概率中心的可能性不同。

CPD 常见目标可以理解为最大化概率，或者最小化负对数似然：

┌──────────────────────────────────────────────┐ │ CPD 思想 │ │ │ │ target 点云形成一堆高斯分布中心； │ │ source 点云通过变换后，应该更可能来自这些高斯分布。 │ │ │ │ T* = argmax P(T(P) | Q) │ └──────────────────────────────────────────────┘

CPD 可以做刚体配准，也可以做非刚体配准。SLAM 里刚体配准更常见，医学图像、形变物体配准里 CPD 更常见。

优点是概率模型比较稳，对噪声有一定鲁棒性。缺点是计算量大，实时 SLAM 里没有 ICP/NDT 那么常用。

9. 基于距离场 / TSDF / ESDF 的匹配

这种方法常见于 RGB-D、三维重建、机器人地图中。它不是直接点对点，而是把地图构造成距离场。

比如地图中每个位置存一个值：

D(x)

表示这个位置到最近表面的距离。当前点变换到地图后，希望落在表面上，也就是距离接近 0。

目标函数：

┌──────────────────────────────────────────────┐ │ 距离场匹配目标 │ │ │ │ T* = argmin Σ D(R*p_i + t)² │ │ │ │ D(x)：位置 x 到最近地图表面的距离 │ │ p_i：当前点 │ │ R,t：待求位姿 │ └──────────────────────────────────────────────┘

如果当前点变换后正好落在地图表面上：

D(R*p_i + t) ≈ 0

这种方法适合稠密地图、深度相机地图、TSDF 地图。优点是匹配目标平滑，不用显式找最近点；缺点是要提前构建距离场，内存和计算代价较高。

10. 常见算法对比表

┌───────────────────────┬───────────────────────┬────────────────────────┬──────────────────────┐ │ 算法 │ 核心思想 │ 优点 │ 缺点 │ ├───────────────────────┼───────────────────────┼────────────────────────┼──────────────────────┤ │ Point-to-Point ICP │ 点找最近点 │ 简单经典，容易实现 │ 依赖初值，易局部最优 │ │ Point-to-Plane ICP │ 点贴近平面 │ 收敛快，适合墙面地面 │ 需要法向量 │ │ GICP │ 点带协方差 │ 更稳，考虑局部几何 │ 计算量更大 │ │ NDT │ 点匹配高斯体素分布 │ 适合大地图定位 │ 体素大小敏感 │ │ Correlative Matching │ 搜索最高栅格得分 │ 初值容忍度较高 │ 搜索范围大时慢 │ │ LOAM / LIO-SAM 特征法 │ 点到线 + 点到面 │ 高效，几何意义明确 │ 特征质量影响大 │ │ RANSAC + 描述子 │ 先找粗匹配 │ 可处理大初始误差 │ 描述子慢，易误匹配 │ │ CPD │ 概率模型配准 │ 鲁棒，可扩展非刚体 │ 实时性较差 │ │ 距离场匹配 │ 点落到距离场零面 │ 目标函数平滑 │ 需要构建距离场 │ └───────────────────────┴───────────────────────┴────────────────────────┴──────────────────────┘

11. 实际 SLAM 里怎么组合使用？

实际工程里很少只用一种算法，常见组合是：

粗匹配 + 精匹配

比如：

RANSAC / Scan Context / Correlative Matching 先给粗位姿； ICP / NDT / Point-to-Plane 再做精配准。

或者：

IMU / Odom 给初值； LiDAR scan-to-map 做精匹配； 因子图做全局优化。

LIO-SAM 就是这种思想：

IMU 预积分给初值和去畸变； featureExtraction 提取角点和平面点； mapOptimization 用点到线、点到面做 scan-to-map； 后端因子图再融合 GPS、回环和 IMU。

12. 总结

ICP是“当前点找目标最近点，然后让点靠点”。
Point-to-Plane ICP是“当前点不要贴某个点，而是贴目标平面”。
GICP是“每个点附近都有形状，用协方差描述这个形状”。
NDT是“目标地图不是点，而是一堆高斯分布格子”。
Correlative Scan Matching是“枚举很多候选位姿，看哪个位姿让点云压到地图障碍物上得分最高”。
LOAM / LIO-SAM是“角点贴地图线，平面点贴地图面”。
RANSAC + 描述子是“先靠特征找粗位姿，再交给 ICP/NDT 精配准”。

版权声明： 辛苦码字不易，转载请注明原文出处和作者信息，谢谢理解！

欢迎分享与交流，但拒绝任何形式的商业转载或洗稿。