LIO-SAM 的 scan-to-map 不是直接拿当前整帧点云跑普通 ICP而是先用当前预测位姿把当前帧角点、面点投到地图坐标系然后分别在局部地图中寻找对应的线特征和平面特征。cornerOptimization()负责为当前角点建立点到线约束surfOptimization()负责为当前面点建立点到面约束最后LMOptimization()将这些约束统一写成线性方程求出当前帧六自由度位姿的修正量。for (int iterCount 0; iterCount 30; iterCount) { laserCloudOri-clear(); // 清空本轮参与优化的当前帧特征点 coeffSel-clear(); // 清空这些点对应的残差与梯度信息 cornerOptimization(); // 当前帧角点 - 局部地图线特征 surfOptimization(); // 当前帧面点 - 局部地图平面特征 combineOptimizationCoeffs(); // 合并所有有效的点到线、点到面约束 if (LMOptimization(iterCount)) break; // 位姿增量足够小认为当前帧已收敛 } transformUpdate(); // 融合 IMU 姿态辅助约束等整个流程是“预测位姿 → 建立线面对应 → 求位姿增量 → 更新位姿 → 重新匹配”。当前帧一开始只有 IMU / odom 给出的粗预测值经过多轮 scan-to-map 迭代后才得到较准确的地图系位姿。1.LMOptimization()优化的是什么它优化的是当前帧在地图系下的六自由度位姿也就是三个旋转量和三个平移量。代码中这六个变量保存在transformTobeMapped数组里。transformTobeMapped[0] // roll绕 x 轴旋转 transformTobeMapped[1] // pitch绕 y 轴旋转 transformTobeMapped[2] // yaw绕 z 轴旋转 transformTobeMapped[3] // x 平移 transformTobeMapped[4] // y 平移 transformTobeMapped[5] // z 平移x [ r p y tₓ tᵧ t_z ]ᵀ这里的x表示当前帧待优化的完整位姿状态r、p、y分别表示 roll、pitch、yaw后面的三个量表示当前雷达在地图系下的位置。当前帧的一个 LiDAR 点先按当前位姿估计投到地图系之后才能去局部地图中寻找附近的线或平面。p_M R(r,p,y) · p_L t其中p_L是当前点在 LiDAR 坐标系下的位置p_M是变换到地图系后的点R(r,p,y)是由当前 roll、pitch、yaw 构造的旋转矩阵t是当前位置平移量。2. 进入LMOptimization()前前面已经准备好了什么cornerOptimization()和surfOptimization()已经分别完成了特征匹配和残差计算。进入LMOptimization()时最关键的是下面两个容器。laserCloudOri-points[i] // 当前帧中第 i 个参与优化的特征点 coeffSel-points[i] // 该点对应的残差梯度方向 加权残差laserCloudOri保存的是当前帧参与优化的角点或面点coeffSel则保存“这个点当前偏了多少、应该向哪个方向修正才能减小误差”。coeffSel-points[i].x // 残差在地图 x 方向上的梯度分量 coeffSel-points[i].y // 残差在地图 y 方向上的梯度分量 coeffSel-points[i].z // 残差在地图 z 方向上的梯度分量 coeffSel-points[i].intensity // 加权后的点到线 / 点到面残差这里的intensity已经不是激光反射强度而是被复用成当前点的残差值。也就是说LMOptimization()不再关心当前点最初是角点还是面点它只关心这个点对应的“残差”和“残差对位姿的影响”。3. 点到线、点到面的残差是怎样进入优化的对于角点LIO-SAM 会在局部地图角点集合里找到邻域点并判断这些邻域点是否近似共线。若满足条件就把它们当成一条局部地图线计算当前点到这条线的距离。r_edge ‖ (p_M − c) × v ‖其中c是地图线上的一点v是地图线的单位方向向量p_M是当前帧点投到地图系后的坐标。当前点偏离这条线越远点到线残差就越大。对于面点LIO-SAM 会在局部地图面点集合中找邻域点并拟合一个局部平面。a·x b·y c·z d 0平面法向量会先归一化因此点代入平面方程后的结果可以直接反映点到平面的有符号距离。a² b² c² 1r_surf a·x b·y c·z d点到线约束主要约束墙角、立柱、货柜棱边等结构点到面约束主要约束地面、墙面、集装箱侧面等平面结构。两类约束一起使用才能对六自由度位姿形成更完整的限制。4. 残差如何被线性化当前位姿附近任何一个点到线或点到面的残差都可以用一阶近似描述。这个过程就是把原本非线性的位姿问题转成当前迭代附近的线性问题。rᵢ(x Δx) ≈ rᵢ(x) JᵢΔx其中rᵢ(x)第i个点在当前位姿下的残差Δx本轮要求解的六维位姿增量Jᵢ第i个点残差对六自由度位姿的雅可比行向量。Jᵢ [ ∂rᵢ/∂r ∂rᵢ/∂p ∂rᵢ/∂y ∂rᵢ/∂tₓ ∂rᵢ/∂tᵧ ∂rᵢ/∂t_z ]这个雅可比表达的是当前点的误差对 roll、pitch、yaw、x、y、z 分别有多敏感。例如若 yaw 微调一点能让点更接近地图中的墙角那么该点对 yaw 的偏导就会较明显若沿某方向移动对残差几乎没有影响则该方向的偏导就很小。5. 为什么代码里有 LiDAR 到旧 LOAM 坐标约定的转换这段代码继承自原始loam_velodyne。原始 LOAM 的优化雅可比采用了一套 camera 风格的变量顺序因此 LIO-SAM 为复用原公式先把 LiDAR 坐标轴和残差梯度做一次重排。// lidar - camera pointOri.x laserCloudOri-points[i].y; pointOri.y laserCloudOri-points[i].z; pointOri.z laserCloudOri-points[i].x; coeff.x coeffSel-points[i].y; coeff.y coeffSel-points[i].z; coeff.z coeffSel-points[i].x; coeff.intensity coeffSel-points[i].intensity;这里的camera不代表真的使用了摄像头而是旧 LOAM 内部的变量约定。后面得到的旋转偏导还会再映射回 LIO-SAM 自己的 LiDAR 坐标和 roll、pitch、yaw 顺序。为了计算旋转雅可比代码先计算当前姿态角的正弦、余弦值。float srx sin(transformTobeMapped[1]); float crx cos(transformTobeMapped[1]); float sry sin(transformTobeMapped[2]); float cry cos(transformTobeMapped[2]); float srz sin(transformTobeMapped[0]); float crz cos(transformTobeMapped[0]);这些三角函数后面会进入旋转矩阵求导结果。因为点经过旋转矩阵后的位置依赖于 roll、pitch、yaw所以残差对旋转角求导时自然会出现这些正弦、余弦组合。6.arx、ary、arz到底是什么下面三部分是旋转雅可比的解析计算。它们虽然很长但本质是在计算“当前点残差对三个旋转自由度的变化率”。float arx (crx*sry*srz*pointOri.x crx*crz*sry*pointOri.y - srx*sry*pointOri.z) * coeff.x (-srx*srz*pointOri.x - crz*srx*pointOri.y - crx*pointOri.z) * coeff.y (crx*cry*srz*pointOri.x crx*cry*crz*pointOri.y - cry*srx*pointOri.z) * coeff.z; float ary ((cry*srx*srz - crz*sry)*pointOri.x (sry*srz cry*crz*srx)*pointOri.y crx*cry*pointOri.z) * coeff.x ((-cry*crz - srx*sry*srz)*pointOri.x (cry*srz - crz*srx*sry)*pointOri.y - crx*sry*pointOri.z) * coeff.z; float arz ((crz*srx*sry - cry*srz)*pointOri.x (-cry*crz - srx*sry*srz)*pointOri.y) * coeff.x (crx*crz*pointOri.x - crx*srz*pointOri.y) * coeff.y ((sry*srz cry*crz*srx)*pointOri.x (crz*sry - cry*srx*srz)*pointOri.y) * coeff.z;它们对应的核心含义如下。∂rᵢ/∂θ gᵢᵀ · ∂(R(θ)·p_L)/∂θ其中θ表示三个旋转角p_L是当前帧原始点R(θ)·p_L是该点经过旋转后的坐标gᵢ是当前残差的梯度方向也就是coeff.x、coeff.y、coeff.z。简单说arx、ary、arz回答的是当前帧姿态绕某个轴轻微转动时这个点到线或点到面的误差会变大还是变小以及变化速度有多快。7.matA和matB分别是什么每一个有效特征点都会贡献一行雅可比和一个残差项。代码将它们写入matA和matB。// camera - lidar matA.atfloat(i, 0) arz; matA.atfloat(i, 1) arx; matA.atfloat(i, 2) ary; matA.atfloat(i, 3) coeff.z; matA.atfloat(i, 4) coeff.x; matA.atfloat(i, 5) coeff.y; matB.atfloat(i, 0) -coeff.intensity;matA的第i行就是当前第i个点残差对六自由度位姿的偏导。Aᵢ [ ∂rᵢ/∂roll ∂rᵢ/∂pitch ∂rᵢ/∂yaw ∂rᵢ/∂x ∂rᵢ/∂y ∂rᵢ/∂z ]所有点堆叠起来就形成完整雅可比矩阵。A [ J₁ ; J₂ ; ··· ; J_N ]对应的右端项是所有点残差的负值。b [ −r₁ ; −r₂ ; ··· ; −r_N ]ᵀ这里取负号是因为优化希望找到一个位姿增量将当前误差向零方向抵消。8. 正规方程真正求当前帧位姿增量的地方当matA和matB构建完成后代码开始构造正规方程并求解本轮的六维位姿增量。cv::transpose(matA, matAt); matAtA matAt * matA; matAtB matAt * matB; cv::solve(matAtA, matAtB, matX, cv::DECOMP_QR);这段代码对应的公式如下。AᵀA Δx Aᵀb等价写法如下JᵀJ Δx −Jᵀr最终要求解的六维位姿增量是Δx [ Δr Δp Δy Δtₓ Δtᵧ Δt_z ]ᵀ其中A或J所有残差的雅可比矩阵b或负残差所有点当前的误差AᵀA近似 Hessian 矩阵描述局部环境对六自由度的约束强度matX最终求得的当前帧位姿修正量。matX的六个元素分别对应当前帧 roll、pitch、yaw、x、y、z 应该在本轮迭代中修正多少。9. 为什么函数名叫 LM但实际更接近高斯牛顿函数名称是LMOptimization()但从实际代码求解的矩阵形式来看它没有显式引入标准 Levenberg-Marquardt 的阻尼项。LIO-SAM 当前代码JᵀJ Δx −Jᵀr标准 LM 应该是标准 LM(JᵀJ λI) Δx −Jᵀr其中λ是阻尼系数I是单位矩阵。标准 LM 会根据优化过程调整阻尼系数在初值较差或矩阵病态时更稳定。你贴出的 LIO-SAM 代码中没有显式的λI也没有动态调整阻尼系数的逻辑。因此更准确的说法是它保留了 LOAM 中的LMOptimization()函数名但当前 scan-to-map 的位姿增量求解形式本质上更接近高斯牛顿型最小二乘优化。10. 退化检测为什么要分析matAtA并不是每个环境都能稳定约束六自由度。例如只有地面时部分水平方向位移和 yaw 的约束可能较弱只有一面墙时沿墙方向移动时点到面残差变化不明显长走廊中沿走廊方向的位移也容易不稳定。因此LIO-SAM 只在第一轮优化时对matAtA做一次特征值分解。if (iterCount 0) { cv::Mat matE(1, 6, CV_32F, cv::Scalar::all(0)); cv::Mat matV(6, 6, CV_32F, cv::Scalar::all(0)); cv::Mat matV2(6, 6, CV_32F, cv::Scalar::all(0)); cv::eigen(matAtA, matE, matV); matV.copyTo(matV2); isDegenerate false; float eignThre[6] {100, 100, 100, 100, 100, 100}; for (int i 5; i 0; i--) { if (matE.atfloat(0, i) eignThre[i]) { for (int j 0; j 6; j) matV2.atfloat(i, j) 0; isDegenerate true; } else { break; } } matP matV.inv() * matV2; }这里分析的是近似 Hessian。H AᵀA特征值分解写成H VΛVᵀ其中特征值代表某个姿态方向上的约束强度。若某个特征值很小说明沿对应方向改变位姿时点到线、点到面的残差几乎不变环境无法可靠限制这个方向。λᵢ τ ⇒ 第 i 个方向约束不足属于退化方向代码中的eignThre是经验阈值。若某个方向退化系统就不会完全相信最小二乘求出的该方向增量而是将其过滤掉。11. 退化方向如何被压制若检测到退化代码会将原始增量投影到可观测子空间中。if (isDegenerate) { cv::Mat matX2(6, 1, CV_32F, cv::Scalar::all(0)); matX.copyTo(matX2); matX matP * matX2; }对应含义如下。Δx_valid P · Δx其中P是退化投影矩阵Δx是原始最小二乘解Δx_valid是过滤后的可靠位姿增量。这样做可以防止机器人在大平面、空旷区域、长走廊等弱特征场景中沿不可观方向发生不可靠的大幅更新也就是减少位姿“乱跳”。12. 更新当前帧位姿并判断是否收敛经过退化处理后matX就是当前帧本轮真正使用的六自由度增量。代码直接累加到transformTobeMapped中。transformTobeMapped[0] matX.atfloat(0, 0); // roll transformTobeMapped[1] matX.atfloat(1, 0); // pitch transformTobeMapped[2] matX.atfloat(2, 0); // yaw transformTobeMapped[3] matX.atfloat(3, 0); // x transformTobeMapped[4] matX.atfloat(4, 0); // y transformTobeMapped[5] matX.atfloat(5, 0); // z对应的位姿更新方式是xₖ₊₁ xₖ Δx随后代码统计本轮角度增量和平移增量是否足够小。float deltaR sqrt( pow(pcl::rad2deg(matX.atfloat(0, 0)), 2) pow(pcl::rad2deg(matX.atfloat(1, 0)), 2) pow(pcl::rad2deg(matX.atfloat(2, 0)), 2)); float deltaT sqrt( pow(matX.atfloat(3, 0) * 100, 2) pow(matX.atfloat(4, 0) * 100, 2) pow(matX.atfloat(5, 0) * 100, 2)); if (deltaR 0.05 deltaT 0.05) { return true; }旋转增量的计算方式是ΔR √[(rad2deg(Δroll))² (rad2deg(Δpitch))² (rad2deg(Δyaw))²]平移增量的计算方式是ΔT √[(100·Δx)² (100·Δy)² (100·Δz)²]收敛判断条件是ΔR 0.05° 且 ΔT 0.05 cm这里表示的是“本轮优化带来的修正已经足够小”而不是说当前帧的绝对定位误差已经小于零点零五厘米。它只是说明在当前局部地图、当前对应关系和当前观测条件下继续迭代已经很难带来明显改善。总结LIO-SAM 的 scan-to-map 可以理解为先利用 IMU / odom 给当前帧一个粗预测位姿然后让当前帧角点去匹配局部地图中的线让当前帧面点去匹配局部地图中的平面之后把所有点到线、点到面的残差统一送进LMOptimization()。LMOptimization()的核心工作是根据每个点残差对 roll、pitch、yaw、x、y、z 的变化规律构造雅可比矩阵和残差向量再求解正规方程得到当前帧本轮应修正的六自由度位姿增量若环境特征不足还会通过特征值分解检测退化方向并压制不可观方向上的不可靠更新。所以前面的cornerOptimization()、surfOptimization()负责“建立当前点该贴近哪条线、哪个面”的约束而LMOptimization()负责“根据这些约束真正算出当前帧位姿应该改多少”。