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点到面 ICPPoint-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_iq_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 ICPGICP广义 ICPGICP 可以理解成 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_isource 点 │ │ q_itarget 对应点 │ │ C_i^Psource 点邻域协方差 │ │ C_i^Qtarget 点邻域协方差 │ │ R,t待求旋转和平移 │ └──────────────────────────────────────────────────────────────┘这里的核心是(C_i^Q R * C_i^P * R^T)^(-1)它相当于告诉优化器不同方向上的误差权重不一样。比如在墙面上沿墙面方向误差可以稍微大一点但垂直墙面的误差更重要。你可以这样理解Point-to-Point ICP每个点就是一个点。 Point-to-Plane ICP目标点附近是一个平面。 GICPsource 和 target 两边都考虑局部几何分布。GICP 通常比普通 ICP 更稳但计算量更大。4. NDTNormal 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变换后的当前点 │ │ μ_ix_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. CPDCoherent 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 精配准”。版权声明 辛苦码字不易转载请注明原文出处和作者信息谢谢理解欢迎分享与交流但拒绝任何形式的商业转载或洗稿。
点云匹配算法
发布时间：2026/6/6 12:54:53
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点到面 ICPPoint-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_iq_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 ICPGICP广义 ICPGICP 可以理解成 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_isource 点 │ │ q_itarget 对应点 │ │ C_i^Psource 点邻域协方差 │ │ C_i^Qtarget 点邻域协方差 │ │ R,t待求旋转和平移 │ └──────────────────────────────────────────────────────────────┘这里的核心是(C_i^Q R * C_i^P * R^T)^(-1)它相当于告诉优化器不同方向上的误差权重不一样。比如在墙面上沿墙面方向误差可以稍微大一点但垂直墙面的误差更重要。你可以这样理解Point-to-Point ICP每个点就是一个点。 Point-to-Plane ICP目标点附近是一个平面。 GICPsource 和 target 两边都考虑局部几何分布。GICP 通常比普通 ICP 更稳但计算量更大。4. NDTNormal 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变换后的当前点 │ │ μ_ix_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. CPDCoherent 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 精配准”。版权声明 辛苦码字不易转载请注明原文出处和作者信息谢谢理解欢迎分享与交流但拒绝任何形式的商业转载或洗稿。
