★ FEATURED ARTICLE 点云配准是三维视觉和机器人领域绕不开的基础问题而 ICPIterative Closest Point算法又是配准的入门必修课。我最早接触ICP时以为它就是“迭代找最近点然后解SVD”直到后来在某个项目里需要同时估计六自由度还要处理大量外点噪声纯 SVD 方案开始失灵我才真正把 ICP 背后的高斯牛顿法吃透。今天这篇东西不打算再重复“什么是ICP”那种科普套话重点把 ICP 目标函数、高斯牛顿法推导、雅可比矩阵组装、LM阻尼、退化场景处理这些实操中必须弄明白的细节一次性讲清楚。无论你是刚接触点云配准的初学者还是已经用开源库调过很多次参但总感觉隔着一层纸的开发这篇内容应该都能让你对“ICP到底在优化什么”有一种豁然开朗的感觉。1. 问题建模ICP到底在优化什么1.1 刚体变换与点对应关系ICP 解决的是这样一个问题有两片点云源点云 P 和目标点云 Q它们可能是同一物体从不同角度扫描得到的也可能是不同时刻拍摄的同一场景。我们希望找到一个刚体变换 T把源点云变换到目标点云的坐标系下让两者尽可能重叠。刚体变换包含旋转矩阵 R 和平移向量 t写成齐次形式就是T(p) R * p t因为扫描过程中物体不会形变所以这个变换必须保持点间距离也就是 R 必须满足 R^T R I且 det(R)1。我们要找的本质上就是这 6 个参数3 个旋转自由度加 3 个平移自由度。难点在于我们并不知道源点云里的每一个点具体对应目标点云里的哪个点。点云不像图像那样有规则的像素网格也没有天然的语义对应。所以 ICP 采用了一个非常朴素的策略在每次迭代中对源点云的每个点在目标点云里找最近邻作为它的“假想对应点”。然后基于这批对应关系求解一个新的变换。因为对应关系是估计出来的所以需要反复迭代交替优化对应关系和变换参数。整体目标函数可以写成F(R, t) 1/2 * Σ || R p_i t - q_i ||^2其中 q_i 是 p_i 在当前估计下的最近邻对应点。这个目标函数看起来简单但 R 是旋转矩阵自带非线性约束所以这不是一个普通的线性最小二乘问题而是一个典型的非线性最小二乘问题。高斯牛顿法正是用来求解这种问题的标准工具。1.2 经典SVD解法与高斯牛顿法的取舍很多讲ICP的教程都会告诉你在得到对应关系之后可以用 SVD奇异值分解一步解出 R 和 t步骤是先计算两组点的质心然后做去质心的协方差矩阵最后对协方差矩阵做 SVD得到 R V U^T。这套闭式解法在小规模标准点对点 ICP 中确实高效稳定我自己做实验时也经常用。但 SVD 解法有一个天然的边界它只能处理“点对点距离的 L2 范数”这种简单误差度量。一旦目标函数变了比如你想给误差加一个鲁棒核函数来抑制外点或者使用“点对面”的距离误差或者同时优化尺度因子SVD 就不再适用因为闭式解只存在于非常特定的函数形式里。高斯牛顿法不一样它是一种通用的数值优化框架只要误差函数对参数可导就可以用统一的流程迭代求解。这带来的实际好处是你可以非常自然地扩展 ICP 的功能而不用为每种误差度量都重新推导闭式解。有人可能会问既然高斯牛顿法通用为什么还要保留 SVD 方案我的看法是标准 ICP 场景下 SVD 更快更稳适合做那些“干净数据”的小规模配准但如果你要处理带有大量噪声、动态物体、或者需要引入鲁棒约束的真实工业场景高斯牛顿法会是一个更灵活的底座。理解了高斯牛顿法再看很多开源库里的变种 ICP你会发现自己能看到骨子里去。2. 高斯牛顿法求解ICP的数学推导2.1 参数化旋转扰动模型用高斯牛顿法求解 ICP第一步是把待优化变量变成一个向量。旋转矩阵 R 本身是 3x3 矩阵有 9 个元素但只有 3 个自由度不能直接把这 9 个元素当作优化变量否则会引入矩阵冗余破坏正交约束。所以通常的做法是使用旋转向量 / 李代数扰动模型。我这里采用左扰动模型。设当前估计的变换为 (R, t)真实变换与当前估计之间有一个小的增量变换增量的旋转部分用一个微小旋转向量 δω 表示平移部分用 δt 表示。于是新的变换可以近似为T_new ΔT * T其中 ΔT 对应的旋转矩阵是 exp(δω^)这里 δω^ 是 δω 的反对称矩阵。在 δω 很小时指数映射可以一阶展开exp(δω^) ≈ I δω^利用这个近似我们可以把残差函数写成当前残差加上一阶项。对源点云中的点 p_i先算当前变换下的点p_i R * p_i t残差为r_i p_i - q_i现在加入一个小扰动新的变换会把 p_i 变到p_i δt δω × p_i因此新的残差可以写成r_i_new r_i δt δω × p_i注意这里 δω × p_i 是叉积可以改写成反对称矩阵和向量的乘积。叉积的反对称形式是δω × p_i - [p_i]_× * δω这里的 [p_i]_× 是一个反对称矩阵[p]_× [ 0, -p_z, p_y; p_z, 0, -p_x; -p_y, p_x, 0]于是我们可以得到残差关于增量的雅可比矩阵 J_i∂r_i / ∂δt I ∂r_i / ∂δω - [p_i]_×如果把优化变量按 [δt; δω] 的顺序排列那么雅可比矩阵是J_i [ I, -[p_i]_× ]这个矩阵的维度是 3x6每一行对应残差向量的一个分量每一列对应一个待优化参数。有了这个 J_i高斯牛顿法就能顺利展开了。注意符号问题有些文献用的是右扰动或不同的参数排列顺序最终雅可比可能差一个符号但只要和更新公式一致结果都是一样的。2.2 高斯牛顿增量方程与H矩阵高斯牛顿法的核心思想是在当前点附近把残差函数做一阶泰勒展开然后求解一个线性最小二乘子问题。对每一个对应点我们有r_i(δx) ≈ r_i J_i δx其中 δx [δt; δω]。那么目标函数变为C(δx) 1/2 * Σ || r_i J_i δx ||^2对 δx 求导并令其为零得到正规方程( Σ J_i^T J_i ) δx - Σ J_i^T r_i通常我们把左边的矩阵记作 H右边的向量记作 bH Σ J_i^T J_i b - Σ J_i^T r_i然后求解线性方程组H δx b在实际工程中H 是 6x6 的稠密矩阵直接调用线性代数库求解即可。这个方程组很小计算量可以忽略不计真正的计算瓶颈在最近邻搜索上。我们还可以把 H 和 b 的分块结构展开来看这样能更直观地理解问题。以左扰动模型为例令 A_i -[p_i]_×则H [ Σ I, Σ A_i^T; Σ A_i, Σ A_i^T A_i ]b - [ Σ r_i; Σ A_i^T r_i ]可以看到平移部分的 H 左上块基本就是对应点数量 n 乘以单位阵这说明只要对应点数量足够多平移方向通常是有约束的。而旋转部分的 H 右下块是 Σ A_i^T A_i它取决于 p_i 的空间分布。如果所有 p_i 共线或者共面这个矩阵就可能出现奇异或近奇异这也是后面要讲的退化问题的根源。求解出 δx 之后需要更新变换参数。注意这里用的是左扰动模型更新方式为t_new t δt R_new exp(δω^) * R其中 exp(δω^) 是旋转向量到旋转矩阵的指数映射可以通过罗德里格斯公式计算。实际代码里如果 δω 很小也可以直接用 I δω^ 近似但为了保证多次迭代后的正交性建议用完整指数映射。2.3 核心代码骨架Eigen实现理论说完了直接看代码实现。下面是用 C 和 Eigen 写的高斯牛顿 ICP 核心迭代部分。这个骨架省略了最近邻搜索的具体实现假设你已经有了一个能返回最近邻点的函数findNearestPoint。#include Eigen/Core #include Eigen/Geometry #include Eigen/Dense // 输入源点云N×3目标点云M×3初始R,t // 输出优化后的R,t void gaussNewtonICP(const std::vectorEigen::Vector3d src, const std::vectorEigen::Vector3d dst, Eigen::Matrix3d R, Eigen::Vector3d t) { // 参数 int maxIter 50; double lambda 1e-3; // LM阻尼初始值 double eps 1e-6; // 收敛阈值 double maxDist 0.05; // 最近邻距离阈值 for (int iter 0; iter maxIter; iter) { // 组装 H, b Eigen::MatrixXd H Eigen::MatrixXd::Zero(6, 6); Eigen::VectorXd b Eigen::VectorXd::Zero(6); double cost 0.0; int valid 0; for (size_t i 0; i src.size(); i) { Eigen::Vector3d p R * src[i] t; int idx -1; double dist2 0.0; // 调用KD-tree查找最近点 bool found findNearestPoint(p, dst, idx, dist2); if (!found || dist2 maxDist * maxDist) continue; Eigen::Vector3d q dst[idx]; Eigen::Vector3d r p - q; // 残差 cost r.squaredNorm(); // 雅可比 Eigen::Matrixdouble, 3, 6 J; Eigen::Vector3d pcross; pcross -p.z(), 0.0, p.x(); // 注意需要构造反对称矩阵 // 直接使用 -[p]_× 作为旋转雅可比 J.block3,3(0,0) Eigen::Matrix3d::Identity(); J.block3,3(0,3) - skew(p); // skew(p) 返回 [p]_× H J.transpose() * J; b -J.transpose() * r; valid; } if (valid 10) { // 有效对应点太少直接退出 break; } // 加上LM阻尼 Eigen::MatrixXd HL H lambda * Eigen::MatrixXd::Identity(6, 6); // 求解增量 Eigen::VectorXd dx HL.ldlt().solve(b); // 检查增量大小 if (dx.norm() eps) break; // 更新变换左扰动模型 Eigen::Vector3d delta_t dx.head3(); Eigen::Vector3d delta_omega dx.tail3(); Eigen::Matrix3d delta_R Eigen::AngleAxisd(delta_omega.norm(), delta_omega.normalized()).toRotationMatrix(); R delta_R * R; t delta_t t; // 这里可以加一个简单的线搜索或自适应lambda } }上面代码里的skew函数可以用 Eigen 的Eigen::Matrix3d hat Eigen::Matrix3d::Zero(); hat 0, -p.z(), p.y(), p.z(), 0, -p.x(), -p.y(), p.x(), 0;来构造。注意我在雅可比中旋转部分取的是-skew(p)对应前面推导的- [p]_×。这段代码只是一个最小骨架没有做鲁棒核、动态调整 lambda 等工程优化。但如果你自己把它跑起来配合 KD-tree 搜索已经能得到一个可用的 ICP 配准器。我强烈建议你在自己的数据集上跑一遍然后手动打印每次迭代的 H 矩阵特征值这样能非常直观地感受到退化问题的存在。3. 实操细节与调参经验3.1 对应点匹配与数据预处理高斯牛顿 ICP 的迭代质量严重依赖最近邻对应点的准确性。很多初学者直接在原始点上逐点找最近邻结果发现耗时长、配准效果差。原因很简单原始点云往往包含大量无关点、噪声和密度不均。所以实际项目中我一般会在跑 ICP 之前做几件事。第一件是降采样。点云密度过高时最近邻搜索的耗时呈指数级上升而且密集区域的重复点会让 H 矩阵的统计产生偏向。我常用体素滤波把空间划分成小格子每个格子保留一个代表点。格子的尺度要根据场景大小和精度要求来定通常取配准目标精度的 2 到 3 倍。比如要求毫米级配准体素可以设 2mm 到 5mm如果只是粗略对齐50mm 也没问题。第二件是剔除无效点。深度相机或者激光雷达扫出来的点云经常有 NaN、Inf、以及距离过远的无效值。这些点参与最近邻搜索轻则拖慢速度重则完全破坏配准。也有时候传感器在物体边缘会产生“飞点”由深度不连续导致的虚假点这类点在当前帧和下一帧之间几乎没有一致性需要直接滤除。第三件是设置对应点距离阈值。真实场景中前后两帧点云只有部分重叠如果源点云某个点变换后离目标点云最近邻还有 10cm那这个对应关系大概率是错误匹配。设置maxDist阈值可以只保留距离较近的对应点。这个阈值很难一上来就拍脑袋定我一般先用一个较大值跑几轮然后根据当前残差的中位数动态缩小。还有一种比较实用的做法是“互相最近邻检查”如果 A 的最近点是 B但 B 的最近点不是 A那就认为这对对应关系不可靠直接丢弃。这个方法能砍掉大量错误匹配虽然会增加一次反向最近邻搜索但收益非常明显。除了距离还可以利用法向量一致性。每个点云的局部表面都有法向量如果对应点对的法向量夹角大于某个角度比如 45 度说明局部几何方向差异太大也大概率是错误对应。这类几何约束在结构复杂的环境中尤其有用但对平整表面或者法向量估计本身就有噪声的场景要谨慎使用否则会过滤掉很多本来正确的对应点。3.2 LM阻尼与收敛控制高斯牛顿法直接求解正规方程时有一个隐藏风险如果 H 矩阵是奇异的增量 δx 会变得非常大甚至导致迭代发散。解决这个问题最常用的手段就是 Levenberg-MarquardtLM算法说白了就是在 H 上加一个阻尼项H H λI当 λ 比较大的时候增量方向趋向于最速下降方向步长变小算法更保守当 λ 比较小的时候算法退化为高斯牛顿法收敛速度快。实践中我会维护一个动态 λ如果一次迭代后代价函数下降了就接受这次更新并减小 λ比如乘以 0.3如果代价函数上升了就拒绝更新并增大 λ比如乘以 10重新求解增量。这里需要注意代价函数必须是用当前的对应关系重新计算后的代价。因为每次更新变换后对应关系也会变所以如果拿旧的对应关系算代价哪怕变换更新了代价也可能不变或者上升。我见过很多新手在实现 LM 时在这个地方踩坑他们先更新变换再重新找最近邻结果发现代价没有下降就误以为 LM 没起作用。正确的做法是在候选更新状态下重新计算一遍对应点或者至少重算残差再判断是否接受这个更新。当然这样做会增加计算开销。实际工程里还有一种折中方案用旧的对应关系计算变换增量但更新后再做一次最近邻搜索然后比较配准误差。这个方案虽然不是严格意义的 LM但实践中效果也不错。收敛条件方面我一般看三个指标增量 δx 的范数小于某个阈值说明变换基本不动了前后两次迭代的均方根误差RMSE变化量小于阈值说明代价函数已经进入平台期达到了最大迭代次数。这三个条件满足任意一个都可以停止迭代。需要提醒的是不要把收敛阈值设得太小。点云本身有噪声即使真值配准RMSE 也不可能降到零。过小的阈值只会让算法在平台期反复空转白白浪费时间。3.3 退化问题平面点云与弱约束方向高斯牛顿 ICP 在实际落地时最容易翻车的地方就是退化场景。典型例子是一架机器人在地面空旷的停车场里激光雷达扫描到的环境几乎是一个平面只有少量柱子提供水平方向约束。这时候 H 矩阵的最小特征值会非常小说明某个方向上的约束非常弱。直接求解正规方程得到的 δx 在这个弱方向上可能是一个非常大的数导致算法瞬间发散。检测退化的方法很简单对 H 矩阵做特征值分解看特征值分布。如果最大特征值和最小特征值之间的比值超过一定阈值比如 1000就认为存在退化。对应最小特征值的特征向量方向就是弱约束方向。处理方式有两种一是直接把弱方向上的增量置零只更新强约束方向上的分量二是给弱方向加一个很大的正则化权重让增量在弱方向上趋近于零。我个人更倾向于前者因为更直白可控性好。退化问题的根源在于 H 矩阵的构造。从分块结构可以看到旋转部分的 H_ωω 与所有经过变换的点的空间分布有关。如果这些点全都位于一个平面内那么平面法线方向上的旋转约束就会很弱。这就是为什么很多重型设备在铺设轨道或者沿墙行走时ICP 会突然“漂移”。要解决这种问题光靠 ICP 本身是不够的通常需要融合其他传感器信息比如 IMU、轮式里程计或者 GPS但那是另一个话题了。至少在纯 ICP 范围内我们至少要能检测到退化而不是等算法发散了才一脸懵。4. 实战验证从仿真到真实点云的调试心得4.1 仿真实验高斯牛顿ICP的收敛表现为了验证高斯牛顿 ICP 的收敛行为我建议你可以在仿真数据上先做实验。比如生成一个随机点云给它施加一个已知的刚体变换然后加一点点高斯噪声。把初始估计故意偏移一部分接着用高斯牛顿 ICP 去迭代恢复真实变换。这样的好处是你有真值可以评估误差。我在某次实验中设置的仿真条件大致如下生成 5000 个均匀分布在 1m x 1m x 1m 空间内的点真实平移是 (0.1, -0.2, 0.05) 米真实旋转是绕 Z 轴转 5 度。初始值给 (0, 0, 0) 平移旋转 0 度。高斯噪声标准差 0.005 米。下面是某次运行的迭代记录数据来自我本地跑的代码具体数值略有浮动迭代次数代价函数值旋转误差(度)平移误差(mm)00.08425.0231.810.01470.8238.220.00130.116.430.00020.021.240.00010.0040.3可以看到高斯牛顿法收敛非常快基本 4 到 5 次迭代就达到了稳定值。这也是它的一个优点在初始位姿不太差的情况下收敛速度远快于通用优化算法。但前提是初始位姿不能离真值太远。如果初始平移误差达到 1 米以上或者旋转误差超过 30 度最近邻匹配很容易陷入错误对应高斯牛顿法可能直接发散。因此我强烈建议在实际使用中做一个“粗配准 精配准”两步走。粗配准可以用特征描述子如 FPFH和 RANSAC 来估计初始变换也可以用 NDT 这类基于体素分布的方法。只要把初始位姿拉到收敛域内高斯牛顿 ICP 的精度和效率都非常可观。4.2 真实数据场景的坑位与对应策略仿真是理想情况真实点云数据往往比仿真数据脏得多。传感器噪声分布不均匀、物体表面的反光会产生大量离群点、动态行人和车辆会引入局部不一致。这些都会让最近邻对应关系变差。我在实际数据处理中遇到过几个高频问题总结成一个速查表。常见问题典型表现排查方向与解决手段完全不收敛迭代过程中代价函数震荡或增大检查初始位姿是否在收敛域内检查点云是否做了去质心或降采样检查最近邻搜索是否有 bug精度不足最终变换有偏差重叠区域有缝隙增加迭代次数减小距离阈值检查是否退化考虑使用点面ICP或加鲁棒核发生漂移某些方向上误差持续累积对 H 矩阵做特征值分解检测退化限制平坦方向更新增加其他传感器约束速度太慢单帧迭代耗时超过需求使用体素降采样建立 KD-tree对初配后的点云裁剪感兴趣区域动态物体干扰配准结果被行人/车辆带偏用鲁棒核函数降低大残差权重设置距离阈值剔除动态物体区域其中鲁棒核函数是我特别想强调的一点。在高斯牛顿框架里加入鲁棒核非常自然。比如使用 Huber 核只需要给每个残差项乘上一个权重 w_i。残差越大的点权重越小残差小于某个阈值的点权重保持为 1。这样偶尔出现的错误匹配就不会对 H 和 b 产生过大的影响。我经常见到有人为了用一个开源 ICP 库内置的鲁棒核选项而翻文档实际上自己实现一个 Huber 核在高斯牛顿 ICP 里只需要十几行代码而且可以完全控制阈值和衰减曲线。4.3 高斯牛顿ICP和SVD-ICP的长期观察最后说一点个人体会。如果只追求标准点对点 ICP我承认 SVD 方案在大多数情况下是更好的选择速度快、数值稳定、实现简单秘密就在于它利用了正交 Procrustes 问题的闭式解。但真实项目往往不会停留在标准点对点模型。我在实际项目中曾经遇到过需要同时估计尺度因子、需要把点到面的距离写进误差、需要给不同传感器数据设置不同权重的情况。这些需求在 SVD 框架下一次次把我推到数学推导的尽头而转到高斯牛顿框架之后一切都变得有章可循。还有一点高斯牛顿 ICP 在初始位姿差的时候同样容易陷入局部极小值。局部极小值是所有迭代配准算法绕不开的问题。很多人以为换了 LM、加了鲁棒核就万事大吉但经验告诉我多尺度策略往往比花哨的优化器更有效。比如先对点云做大幅降采样用粗分辨率快速逼近全局最优再逐步提高点云分辨率让精细几何结构修正最终变换。这种从粗到精的思路和高斯牛顿法配合得很好因为粗分辨率下对应关系更稳定局部极小值的风险更低。在我自己长期使用 ICP 的经验里真正让一个配准系统稳定的是几个环节的合力合理的初始对齐、干净的对应点筛选、可靠的退化检测、以及一个稳健的数值优化核心。高斯牛顿法在其中承担了“优化核心”的角色。当你理解了它的雅可比从哪来、正规方程为什么是那样、退化又是如何反映在 H 矩阵特征值上的你会发现后续再看任何基于 ICP 的改进算法比如 NICP、G-ICP、点面 ICP都只是在这个核心上替换误差函数和雅可比的技巧而已。建议找一个自己的数据集把代码骨架跑通然后故意制造退化场景打印 H 矩阵的特征值观察变化这个过程会比看十篇论文都更有收获。
阅读完成 · 觉得有帮助？
