做激光SLAM、三维重建或者工业视觉检测的朋友，对“点云配准”这四个字应该不陌生。把不同时刻、不同视角扫到的两片点云拼到同一个坐标系下，精度能做到毫米级甚至更高，这事听着简单，真正上手就知道坑有多少。而一旦配准问题被包装成非线性最小二乘，Levenberg-Marquardt（简称LM）几乎是绕不开的求解器。这篇我打算完全从一个工程实现者的角度，把LM用在点云配准里的原理、代码、调参经验一次讲透，适合正在写配准模块的开发者、准备改进ICP/NDT性能的同学，以及被各种优化库参数折磨过的人。
先说明一点：我这里聊的不是PCL或者Open3D里封装好的黑盒接口，而是把LM当作一个能自己控制的优化内核，塞进自己的配准流程里。理解了这个内核，你才能解释为什么某些场景下LM比SVD闭式解更稳，为什么调一个λ值能让发散的程序起死回生，以及什么是配准中真正决定精度的关键环节。
1. 配准问题到底在优化什么：为什么LM在这里有立足之地
1.1 点云配准的目标函数长什么样
假设现在有源点云 P = {p₁, p₂, ..., pₙ} 和目标点云 Q = {q₁, q₂, ..., qₘ}，我们要求一个刚体变换 T ∈ SE(3)，让 P 变换后与 Q 尽可能重合。最经典的point-to-point目标函数是：
min_{R,t} ∑ᵢ ‖ R·pᵢ + t - qᵢ ‖²
这里 R 是旋转矩阵，t 是平移向量。看起来就是一个普通最小二乘，但有两个地方让问题变得不平凡：第一，旋转矩阵 R 不是任意9个实数，它必须满足 RᵀR = I 且 det(R)=1，本质上是非线性约束；第二，点 pᵢ 和 qᵢ 的对应关系不是天然给定的，我们通常是找最近邻作为对应，而最近邻关系会随着当前位姿估计变化，所以整个问题是一个带隐式变量（对应关系）的非线性优化问题。
配准领域最广为人知的算法是ICP，它的做法是交替两个步骤：先根据当前位姿建立对应关系，再用SVD求一个最优刚体变换闭式解。ICP之所以流行，是因为SVD在“点到点”距离下能直接给出最优解，迭代几次就收敛，实现起来非常干净。但工程场景不会一直这么理想——当你换成point-to-plane距离，或者想引入鲁棒核函数抑制离群点，SVD就没那么顺手了。
1.2 为什么不用ICP那套SVD，或者为什么还要LM
很多人会问：ICP已经这么成熟，直接用SVD不香吗？我的看法是：SVD和LM并不互斥，它们是不同层面的工具。
ICP框架里的内层优化可以看作是在“固定对应关系”的前提下求位姿。如果距离度量是点到点欧氏距离，SVD闭式解又快又稳，完全没必要用迭代优化。但一旦距离度量变成点到平面距离，目标函数自带法向量点乘，闭式解不再是简单SVD；还有，当你需要给每个残差加权重、加鲁棒核，把配准统一成一个带先验或带约束的非线性最小二乘时，SVD就很难统一处理。这些场景下，LM几乎就是标准答案。
另外还有个理解上的关键点：ICP用交替优化，对应关系更新和位姿更新轮流进行。LM则是在固定对应关系后直接对位姿参数做非线性最小二乘迭代，每一次迭代都会重新线性化旋转项，因此它对“旋转更新”这类非线性项处理得更自然。激光SLAM领域常见的point-to-plane ICP，工程实现里大量使用LM或高斯牛顿作为内层求解器，就是这个原因。
1.3 LM适合谁来用、解决什么问题
回到读者视角。如果你正在做以下事情，这篇内容对你最有用：
- 自己实现了配准流程，但发现点到面距离不好用SVD求闭式解；
- 配准对离群点很敏感，想引入Huber/Cauchy等鲁棒核，却不知道权重怎么更新；
- 想深挖NDT、BA（捆集调整）、图优化背后的求解器原理；
- 或者你纯粹好奇：为什么开源库里到处是
LevenbergMarquardtOptimizer这种类名。
LM解决的核心问题是“带非线性最小二乘形式的优化”，配准恰好可以写成这种形式。理解LM之后，你会发现自己对配准的掌控力高了一个档次，至少不会再遇到发散就只会调迭代次数。
2. 从直觉打通LM：阻尼因子是如何融合两种优化思路的
2.1 梯度下降与Gauss-Newton：两个极端
要理解LM，先看两个极端。设残差向量为 f(θ)，θ 是待优化参数，目标函数 F(θ) = ½‖f(θ)‖²。对残差做一阶泰勒展开 f(θ + Δ) ≈ f(θ) + J·Δ，其中 J 是雅可比矩阵。代入目标函数，得到近似二次模型：
F(θ + Δ) ≈ ½‖f + JΔ‖² = F + ΔᵀJᵀf + ½ΔᵀJᵀJΔ
对 Δ 求导并令其为零，就得到Gauss-Newton更新：
(JᵀJ) Δ = -Jᵀf
这个方程看起来非常优美，实际使用中 JᵀJ 经常半正定甚至奇异，尤其在配准退化场景下，某些方向没有约束，直接求解会给出一个离谱的大步长，然后整个优化就飞了。
梯度下降则是另一个极端，每次只沿着负梯度方向走一小步：Δ = -α·Jᵀf。它稳定但慢，而且非常依赖步长 α 的选取。太大会震荡，太小则半天不动。
2.2 LM的折中：一个λ值动态切换
LM的思路很朴素：在Gauss-Newton方程里加一个阻尼项，把方程变成：
(JᵀJ + λ·D) Δ = -Jᵀf
其中 D 可以取单位阵 I，也可以取 diag(JᵀJ)。后者用到了每个参数方向的尺度信息，实际效果往往更好，这也是经典Marquardt方案。
现在看 λ 的作用：
- λ 很小的时候，JᵀJ + λD ≈ JᵀJ，退化成Gauss-Newton，收敛速度快；
- λ 很大的时候，方程主导项变成 λD，更新方向被拉向负梯度方向（因为 Δ ≈ -(1/λ)D⁻¹Jᵀf），步长也变小，等于在做小步长梯度下降。
所以LM本质上是一个“自动切换器”：当迭代顺利、目标函数稳定下降时，它趋向Gauss-Newton高速逼近；当迭代失败、可能走出危险区域时，它增大λ，缩成保守的梯度下降步。这个λ就是传说中的“阻尼因子”，也是LM唯一需要动态调节的超参数。
用生活化类比：Gauss-Newton像是一个对自己定位非常自信的人，闭着眼睛大步往前冲；梯度下降像是一个谨慎的人，每一步都要试探地形；LM则是一个能根据脚下路况自动切换走路姿态的人——路况好就大步快跑，脚下一滑立刻变成小碎步稳住重心。
2.3 算法流程与终止判定
经典的LM单步流程可以写成这样：
- 给定当前参数 θ，计算残差 f(θ) 和雅可比 J；
- 计算 H = JᵀJ，g = Jᵀf；
- 解线性方程 (H + λD) Δ = -g；
- 用 θ + Δ 试算新残差，比较新旧代价 F(θ+Δ) 与 F(θ)；
- 如果新代价更小，接受更新，减小λ（比如 λ × 0.5）；如果更大，拒绝更新，增大λ（比如 λ × 10），回到第3步重新求解；
- 迭代直至增量、代价变化或梯度模长满足阈值。
这里的核心在于 λ 的动态更新节奏。工程上常见做法是维护一个初始 λ，通常从 1e-3 或 1e-6 起步；接受步则乘以一个 0.3~0.5 的缩小因子，拒绝步则乘以 2~10 的放大因子。这个缩放因子本身就是一门调参学问，后面我会专门讲。
LM还有一个很漂亮的性质：它可以被理解为“带约束步长”的信赖域方法。λ 越大，等效信赖域半径越小，当前二次模型越可信的范围就越小。这也解释了为什么 λ 初值不能太大：初值太大，算法一开始就在做龟速梯度下降，白白浪费迭代次数；初值太小，第一步就可能因为JᵀJ奇异而飞出去。
3. 配准场景下LM的工程化细节：残差、雅可比与参数化
3.1 残差到底怎么定义
配准里最常见的两种残差定义：point-to-point和point-to-plane。
point-to-point残差直接写为：
eᵢ = R·pᵢ + t - qᵢ
它简单直观，但有个致命弱点：在平面或缓变曲面上，沿法向的约束强、沿切向的约束弱，导致优化病态。point-to-plane残差则加入目标点法向量 nᵢ：
eᵢ = nᵢᵀ · (R·pᵢ + t - qᵢ)
这个残差是标量，它的几何意义是“源点沿法向方向到目标平面的距离”。在激光SLAM里，点云扫描天然覆盖大量平面结构，point-to-plane的收敛速度和精度通常明显优于point-to-point，而且对点云密度变化更鲁棒。
选择用哪种残差并不只是精度问题，它还直接决定了雅可比矩阵的shape。point-to-point的雅可比是 3×6，point-to-plane的雅可比是 1×6。后面代码里我会体现这个差异。
3.2 旋转参数化：为什么用轴角而不是四元数
配准的待优化参数是6自由度位姿，通常写成 ξ = (t, φ)，φ 是轴角旋转向量，属于 so(3)。用轴角而不是旋转矩阵或四元数，核心原因是优化时我们希望参数空间“无约束”：旋转矩阵有正交性约束，四元数有单位模长约束，直接把这些约束扔进迭代里会非常麻烦。
轴角参数化下，当前旋转矩阵 R 通过指数映射与 φ 关联：R = exp([φ]×)。在迭代时，用扰动模型更实用。假设当前位姿为 (R, t)，对旋转施加一个小的左扰动 δφ，则新旋转矩阵近似为：
R_new ≈ exp([δφ]×) · R
对平移的扰动直接相加：t_new = t + δt。
于是每次迭代求出的增量就是 Δ = (δt, δφ)ᵀ，一个6维向量。这个参数化方式保证了每一步更新后的旋转矩阵仍然合法，不需要额外的归一化或者投影操作。
补充一个细节：左扰动和右扰动在雅可比上只差一个旋转共轭，最终优化结果一致。但如果你把扰动公式写错，雅可比符号随随便便就会反，这也是很多人照着论文推公式却越调越发散的原因之一。
3.3 雅可比矩阵推导
雅可比矩阵是整个LM里最容易写错的地方。以point-to-point为例，残差 eᵢ = R pᵢ + t - qᵢ。对平移 t 求导是单位阵，对左扰动 δφ 求导需要一点李代数知识：
∂eᵢ/∂δφ = -[R pᵢ]×
这里的 [·]× 表示向量的反对称矩阵。直觉理解：当整体旋转一个微小角度，点 Rpᵢ 在空中划过一个微小弧，弧的切线方向正好等于 Rpᵢ 与旋转轴的叉乘。
因此完整雅可比是：
Jᵢ = ( -[R pᵢ]× , I₃ )
它是一个 3×6 矩阵。point-to-plane残差则直接把法向量乘上去：
Jᵢ = nᵢᵀ · ( -[R pᵢ]× , I₃ ) = ( -nᵢᵀ [R pᵢ]× , nᵢᵀ )
得到 1×6 矩阵。代码里只要实现这一个公式，剩下全是矩阵组装功夫。
这里多说一句：雅可比符号错了不会导致立即发散，它可能只是让收敛变慢、或者在某些位姿下表现异常，这类bug非常隐蔽。我排查过不止一次，最后发现就是某个叉乘顺序反了。
3.4 对应关系如何提供
LM只负责优化，对应关系还得自己找。标准流程是：
- 用当前位姿把源点云变换到目标坐标系；
- 对每个变换后的点，在目标点云里搜最近邻（KD-Tree）；
- 选配校验：距离阈值过滤、法向夹角过滤、边缘点剔除；
- 把筛选后的对应点对送入LM。
对应关系质量对LM影响巨大。存在大量错误对应时，即使LM带有鲁棒项也只是缓解，不可能根治。工程上常用体素降采样降低点云密度，同时保留几何特征，再用距离阈值和法向一致性做两级过滤，能把误匹配率压到很低。
4. 手写一个配准用的LM优化器
这一节放一段可以直接跑的Python风格伪代码，核心逻辑完整，不依赖特定点云库，方便理解。真正工程化时可以换成C++和Eigen，结构完全一致。
4.1 核心代码骨架
import numpy as np from scipy.spatial import cKDTree def skew(v): return np.array([ [0, -v[2], v[1]], [v[2], 0, -v[0]], [-v[1], v[0], 0] ]) def apply_transform(points, R, t): return (R @ points.T).T + t def lm_align(src, dst, dst_normals, R0, t0, max_iter=50, lambda_init=1e-3, accept_scale=0.5, reject_scale=10.0, tol_delta=1e-7, tol_cost=1e-7): R, t = R0.copy(), t0.copy() lam = lambda_init tree = cKDTree(dst) # 先算初始代价 cost_prev = float('inf') for it in range(max_iter): # 当前变换后的源点云 src_trans = apply_transform(src, R, t) # 最近邻对应 dist, idx = tree.query(src_trans, k=1) mask = dist < 1.0 # 距离阈值过滤，可配置 src_sub = src[mask] src_trans_sub = src_trans[mask] dst_sub = dst[idx[mask]] # 残差和雅可比组装 J = [] f = [] for i in range(src_sub.shape[0]): Rp = R @ src_sub[i] # point-to-point e = src_trans_sub[i] - dst_sub[i] Ji = np.hstack([-skew(Rp), np.eye(3)]) J.append(Ji) f.append(e) J = np.array(J).reshape(-1, 6) # N*3 x 6 f = np.array(f).reshape(-1) # N*3 H = J.T @ J g = J.T @ f cost = 0.5 * np.sum(f**2) # LM 核心：求解增量，带动态阻尼 accepted = False for _ in range(10): # 拒绝后重试次数 D = np.diag(np.diag(H)) delta = np.linalg.solve(H + lam * D, -g) dR = skew(delta[3:6]) R_new = dR @ R # 左扰动 t_new = t + delta[0:3] # 评估新代价 src_new = apply_transform(src_sub, R_new, t_new) e_new = src_new - dst_sub cost_new = 0.5 * np.sum(e_new**2) if cost_new < cost: R, t = R_new, t_new lam *= accept_scale accepted = True break else: lam *= reject_scale if not accepted: # 连续拒绝，说明模型与真实差距大，保守退出 break # 收敛判断 delta_norm = np.linalg.norm(delta) cost_change = abs(cost_new - cost_prev) / (cost_prev + 1e-12) if delta_norm < tol_delta or cost_change < tol_cost: break cost_prev = cost_new return R, t上面这段代码把LM最重要的几个环节都体现出来了：组装H矩阵和g向量、动态求解带阻尼的线性系统、接受/拒绝机制、收敛判断。麻雀虽小五脏俱全。
4.2 λ动态更新的实现细节
在真实工程里，λ更新不是机械地乘一个固定系数，我习惯的做法是分层处理：
- 首次迭代，λ_init 根据 H 最大对角元动态设定，例如 λ_init = 1e-3 * max(diag(H))，避免点云尺度差异导致λ完全不匹配；
- 接受更新后，λ 乘 0.3~0.5，让算法尽快切换到Gauss-Newton模式；
- 拒绝更新后，λ 乘 5~10，立即缩步，然后重新解方程，不必重新算对应关系和雅可比——因为对应关系没有变，H和g都可以复用；
- 如果连续拒绝超过若干次，说明当前线性化点周围模型严重失真，此时需要重新清理对应关系或直接退出，而不是无限放大λ。
这里有个细节经常被忽略：每次拒绝时，我们只是改变了λ，解出的增量会越来越小，代价评估仍然失败的话，其实说明当前迭代点本身就不太合适。这时候与其死磕，不如重置λ并强制重新找对应。
4.3 鲁棒核与加权
点云里存在离群点是常态。直接把所有残差等权送进LM，一个错误的对应点就可能把结果带偏。工程上在LM里加鲁棒核非常容易：只需要给每个残差乘一个权重 wᵢ，把目标函数从 ½∑‖eᵢ‖² 改成 ½∑ wᵢ‖eᵢ‖²。
Huber核的权重形式是：
wᵢ = 1，当 ‖eᵢ‖ ≤ δ；wᵢ = δ / ‖eᵢ‖，当 ‖eᵢ‖ > δ
δ 一般取配准精度的2~3倍，比如点云噪声为0.02m，δ可以设0.05m。这种权重等价于对每个残差重新缩放，修改后的残差 e'ᵢ = sqrt(wᵢ)·eᵢ，然后照常组装H和g。
实操中还有个细节：权重会影响H矩阵的数值范围，加了鲁棒核之后，原本被大误差支配的对角元会被压下来，λ需要相应重新调整。所以我的建议是，鲁棒核和λ动态调整要一起联调，千万别只加核不调λ。
4.4 收敛条件与输出
收敛条件需要结合实际硬件和精度要求来定。我常用的三件套：
- 增量范数 ‖Δ‖ 小于阈值，说明位姿变化已经小于微米/毫弧度量级；
- 代价相对变化小于阈值，说明优化进入平台期；
- 迭代次数达到上限，防止死循环。
特别提醒：在点云配准里，代价值本身受点数影响很大。同样是1000个点和100000个点的场景，绝对代价值完全不在一个量级，所以相对变化比绝对变化可靠得多。我在代码里用cost_change = |cost_new - cost_prev| / cost_prev就是这个原因。
最终输出除了最优位姿 R、t，我强烈建议把最后一次迭代得到的信息一起返回：内点数量、残差RMS、H矩阵特征值。前两个用来判断配准质量，后者用来判断退化程度，这些信息在工程联调时是无价之宝。
5. 参数调优与实验现象：那些文档里不会写的经验
5.1 初值决定成败
LM和ICP本质上都是局部优化算法，它们只能把你带到“最近”的局部极小点。如果你的初始位姿离真值差个几十厘米或者十度以上，LM再怎么调λ也无济于事，因为它连正确对应关系都建立不起来。
在工程里我一般这么处理初值：
- 激光里程计场景：上一帧位姿叠加运动预测，初值误差通常在厘米级；
- 多帧拼接：用粗略的全局特征匹配或者里程计积分给出初值；
- 静态场景：可以用PCA主轴对齐粗配准，或者用RANSAC找一个粗略位姿；
- 实在没有初值，就先用低分辨率体素降采样跑一遍粗配准，再用全分辨率精配准。
很多新手把LM当作全局优化器用，认为多迭代几次就能从任意初值收敛，这是对LM最大的误解。LM的价值在于“精细对齐”，不在于“全局匹配”。
5.2 λ初值和缩放因子
关于λ的取值，网上资料写1e-3、1e-6的都有，实际上没有绝对答案，它跟残差尺度强相关。点云坐标以米为单位时和以毫米为单位时，H矩阵对角线量级差了几百万倍，同样的λ表现会天差地别。
所以我推荐用相对值：λ_init = 1e-3 * max(diag(H))。这样至少保证初始阻尼与问题尺度一致。实际项目里我把这个值写在配置文件里，不同传感器（机械式激光雷达、固态激光雷达、深度相机）之间切换时不需要反复试。
缩放因子的经验值：
| 参数 | 推荐范围 | 我的常用值 | 备注 |
|---|---|---|---|
| λ_init系数 | 1e-6 ~ 1e-2 | 1e-3 | 乘上H最大对角元 |
| 接受步缩小因子 | 0.3 ~ 0.8 | 0.5 | 越大保留GN风格越多 |
| 拒绝步放大因子 | 2 ~ 15 | 10 | 越小越容易再试 |
| 单次拒绝后重试次数 | 3 ~ 20 | 10 | 超过则整体退出 |
还有一个容易被忽略的点：如果每次迭代对应关系都重新搜索，那么代价变化曲线可能不是单调下降的。对应关系一变，整个残差集合不同，前后代价不可严格比较，此时LM的接受/拒绝机制需要做一点妥协——只比较重叠区域内的点。这也是为什么实际工程的LM配准代码会比教科书复杂一些。
5.3 收敛阈值怎么定
收敛阈值不要一上来就追求极致。阈值设得太小，最后几次迭代完全是在噪声里挣扎，白白消耗时间；阈值设得太大，位姿明显还没稳定就停了，精度不足。
我习惯分两级设定：先跑一个宽松阈值判断整体收敛趋势，如果代价还在明显下降，再切换到严格阈值继续精修。这个策略有点类似从粗配准到精配准的分层思想。举个例子：粗配准阶段，增量阈值1e-4就够；精配准阶段，增量阈值设到1e-8也不过分，但前提是点云质量撑得起这个精度。
5.4 实验对比与现象记录
在我自己的实验里，对比过ICP-SVD和LM内层求解器在相同对应关系下的表现，记录几组典型现象：
- point-to-point + SVD：迭代收敛极快，通常3~5步就稳定，但对离群点敏感，平面场景下收敛精度一般；
- point-to-point + LM：收敛步数和SVD接近，但可以通过权重灵活控制离群点影响；
- point-to-plane + SVD：需要自行推导非线性闭式解，实现复杂，工程里很少见；
- point-to-plane + LM：收敛精度最高，特别是结构化环境，平面约束能有效压制切向漂移，但初值要求更高。
另一个有趣的实验现象是：在点到面配准中，LM的阻尼机制天然地处理了“切线方向约束弱”的问题。因为MLS曲面或平面环境下，H矩阵对应切向的特征值很小，λ的存在让该方向更新幅度被压制，避免一步迈出几米远。这解释了为什么在长廊、大平层这类场景，LM比纯Gauss-Newton稳得多。
6. 常见问题与排查清单
6.1 迭代速度慢
LM每步迭代都要解一个线性方程，如果点云规模大，J矩阵组装和H矩阵计算也会占据大量时间。遇到速度瓶颈，先做这四件事：
- 对源点云做体素降采样，把参与优化的点数从几十万压到几千，精度损失通常很小；
- 限制最大迭代次数，比如20次以内，大多数场景5~10次就够；
- 用Cholesky分解解线性方程，而不是通用求逆；
- 如果同一个配准任务要反复跑，预计算源点的空间哈希结构，避免每轮重建KD-Tree。
我遇到过最夸张的一次，接入降采样后速度从单帧200ms降到15ms，精度只下降了0.3mm，这就是体素降采样的魅力。
6.2 迭代发散
这是最常见的求助问题。发散的原因可能有两个：第一，初值差到对应关系完全混乱；第二，λ初值太小导致第一步直接飞出去。排查顺序建议：
- 打印每一步的代价变化，如果第一步代价就暴增，优先怀疑初值；
- 检查雅可比符号，比如把
-[Rp]×写成+[Rp]×，发散规律非常隐蔽； - 把λ初始系数调大两三个数量级，看看是否立刻变稳，如果稳定了说明原始λ确实过小；
- 检查是否没有过滤异常对应点，比如某个点距离其他点几百米远，会把残差顶到天文数字。
6.3 退化场景：平面、长廊、单面墙
当点云只提供单一方向约束时，LM的H矩阵会存在近零特征值，对应方向估计不可靠。比如一个空旷的房间里，激光点云全是墙和地板，墙面法向基本平行于地面，那么沿着墙面法向的位置约束很强，但沿着墙面切向和绕法向的旋转就缺乏信息。
处理退化场景的手段：
- 计算H矩阵特征值，特征值小于最大特征值的1e-4时，认为对应方向退化；
- 退化的自由度不参与更新，即把增量投影到非退化方向子空间；
- 结合IMU/轮式里程计，为退化方向提供先验约束；
- 或者在LM目标函数里加一个弱先验项，直接把退化方向拽向先验值。
我在一个仓库巡检项目里遇到过长走廊配准发散，最后就是靠 H 矩阵特征值判断退化、冻结长廊轴向自由度解决的，这个技巧非常实用。
6.4 对应关系的坑
LM本身不产生对应关系，但对应关系的质量直接决定LM效果。最常见的问题有两种：一是点云密度不均导致最近邻搜索偏向密集区域；二是运动物体点被当成静态对应点。
对应关系这条线我建议的流程：
- 源点云和目标点云都用体素滤波，密度均匀化；
- 变换后按距离阈值过滤，阈值取体素尺寸的2~3倍；
- 对法向夹角过滤，夹角超过15~20度的对应点直接丢弃；
- 最后再用鲁棒核兜底，专门处理漏网的outlier。
还要提醒一句：如果两帧点云时间间隔长（比如回环检测），必须先用全局描述子粗匹配，再进LM精配准，跳过粗匹配直接精配准是浪费时间。
7. 实际项目中的扩展与体会
把这些内容组合起来，我最后聊聊LM在真实项目中的定位。它从来不是孤零零存在的一个算法，而是和对应关系、初值、鲁棒核、退化检测这些环节组合成一个完整配准系统。我在实际工程里，一个配准模块通常包含三层：粗配准层负责拉回全局位姿，精配准层用LM迭代最小化点到面残差，最后还有一个退化检测层负责告知上层“这次配准到底可不可信”。
人人都在谈ICP、NDT、GICP，但落到代码层面，它们的优化内核几乎都能统一成LM或类似的高斯牛顿变体。学会手写一个LM，意味着你不再被开源库的黑盒参数绑架，出了任何问题都能从原理上定位。我踩过的最大的坑就是早期直接把PCL里的ICP参数拿来用，结果在某个特殊场景下发散到怀疑人生；后来自己实现了LM内核，把每一步的H、λ、代价全部打印出来，才真正看懂算法在干什么。
如果你正准备动手实现，我的建议是：先用500行以内的Python把LM跑通，盯着λ的每步变化理解接受/拒绝机制；然后再考虑性能优化、鲁棒核、特征值退化分析。把基础内核吃透，后续所有的扩展都只是往框架里挂模块而已。
