LIO-SAM中scan2map优化问题的求解具体由LMOptimization函数实现。该函数在一个scan2map的配准中会被循环调用多次（最多30次）。它的目标是求解出一个微小的 6-DOF 位姿增量，即 matX，使得当前帧的点云投影到局部地图后，点到线、点到面的距离残差平方和最小。该函数内容如下：

// 输入参数 iterCount：当前优化迭代的次数
// 返回值：如果本次迭代算出的位姿增量非常小（收敛），则返回 true；否则返回 false
bool LMOptimization(int iterCount)
{
    // ==========================================================
    // 1. 坐标系转换的预处理 (Lidar -> Camera 坐标系映射)
    // ==========================================================
    // 这部分代码继承自 LOAM。由于Ji Zhang最初推导雅可比矩阵公式时使用的是相机坐标系
    // 相机坐标系：Z轴向前，X轴向左，Y轴向上
    // 雷达坐标系：X轴向前，Y轴向左，Z轴向上 (LIO-SAM 采用的标准)
    // 为了直接复用那套复杂的解析求导公式，这里将雷达的欧拉角映射为相机的欧拉角：
    // roll = pitch, pitch = yaw, yaw = roll
    
    // transformTobeMapped 存储的是当前优化预测的位姿：[roll, pitch, yaw, x, y, z]
    // 这里提取映射后的欧拉角的 sin 和 cos 值，用于后面雅可比矩阵的计算
    float srx = sin(transformTobeMapped[1]); // x 轴对应雷达的 pitch（y）
    float crx = cos(transformTobeMapped[1]);
    float sry = sin(transformTobeMapped[2]); // y 轴对应雷达的 yaw（z）
    float cry = cos(transformTobeMapped[2]);
    float srz = sin(transformTobeMapped[0]); // z 轴对应雷达的 roll（x）
    float crz = cos(transformTobeMapped[0]);

    // 获取本次参与优化的有效特征点数量（这些点都已经成功在局部地图中找到了匹配的线/面）
    int laserCloudSelNum = laserCloudOri->size();
    
    // 如果约束点太少（< 50），系统极不稳定，直接放弃本次优化
    if (laserCloudSelNum < 50) {
        return false;
    }

    // ==========================================================
    // 2. 初始化高斯-牛顿法 (Gauss-Newton) 所需的矩阵
    // ==========================================================
    // 目标是求解正规方程: (J^T * J) * Δx = -J^T * e
    // matA: 雅可比矩阵 J (N * 6)，存储残差对 6DOF 的偏导数
    // matAt: J的转置 (6 * N)
    // matAtA: 近似海森/信息矩阵 H = J^T * J (6 * 6)
    // matB: 负残差向量 -e (N * 1)
    // matAtB: -J^T * e (6 * 1)
    // matX: 解出的位姿增量 Δx (6 * 1)
    cv::Mat matA(laserCloudSelNum, 6, CV_32F, cv::Scalar::all(0));
    cv::Mat matAt(6, laserCloudSelNum, CV_32F, cv::Scalar::all(0));
    cv::Mat matAtA(6, 6, CV_32F, cv::Scalar::all(0));
    cv::Mat matB(laserCloudSelNum, 1, CV_32F, cv::Scalar::all(0));
    cv::Mat matAtB(6, 1, CV_32F, cv::Scalar::all(0));
    cv::Mat matX(6, 1, CV_32F, cv::Scalar::all(0));

    // ==========================================================
    // 3. 构建雅可比矩阵 matA 和 残差向量 matB
    // ==========================================================
    PointType pointOri, coeff;

    for (int i = 0; i < laserCloudSelNum; i++) {
        // laserCloudOri 存的是当前帧原始特征点的坐标
        // lidar -> camera 坐标转换
        pointOri.x = laserCloudOri->points[i].y;
        pointOri.y = laserCloudOri->points[i].z;
        pointOri.z = laserCloudOri->points[i].x;
        
        // coeff 存的是前面步骤算出来的【几何约束】：
        // coeff.x, coeff.y, coeff.z 是点到直线/平面的距离对平移的偏导数
        // coeff.intensity 存的是点到直线/平面的【距离残差】
        // 同理，进行 lidar -> camera 坐标转换
        coeff.x = coeffSel->points[i].y;
        coeff.y = coeffSel->points[i].z;
        coeff.z = coeffSel->points[i].x;
        coeff.intensity = coeffSel->points[i].intensity;

        // ---- 核心求导公式 (解析导数) ----
        // 下面这三坨公式是点到线/面的距离对三个旋转角 (roll, pitch, yaw) 的偏导数
        // arx 是对 camera 坐标系 x 轴旋转的偏导数
        // ary 是对 camera 坐标系 y 轴旋转的偏导数
        // arz 是对 camera 坐标系 z 轴旋转的偏导数
        // (推导过程利用了旋转矩阵的链式求导法则，这里直接套用 LOAM 的结论)
        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;

        // 填充雅可比矩阵 A 的第 i 行 (顺序与欧拉角映射对应)
        // 对应雷达坐标系的 [roll, pitch, yaw, x, y, z] 的偏导数
        matA.at<float>(i, 0) = arz; // 雷达 roll -> 相机 z 轴旋转
        matA.at<float>(i, 1) = arx; // 雷达 pitch -> 相机 x 轴旋转
        matA.at<float>(i, 2) = ary; // 雷达 yaw -> 相机 y 轴旋转
        matA.at<float>(i, 3) = coeff.z; // 平移部分的偏导就是法向量的分量
        matA.at<float>(i, 4) = coeff.x;
        matA.at<float>(i, 5) = coeff.y;
        
        // 填充残差向量 B (负的距离残差 -e)
        matB.at<float>(i, 0) = -coeff.intensity;
    }

    // ==========================================================
    // 4. 求解正规方程，并进行退化(Degeneracy)检测
    // ==========================================================
    cv::transpose(matA, matAt);  // 求 J^T
    matAtA = matAt * matA;       // 算 H = J^T * J  (信息矩阵)
    matAtB = matAt * matB;       // 算 -J^T * e

    // 用 QR 分解法求解线性方程组 H * Δx = -J^T * e，求出位姿增量 matX
    cv::solve(matAtA, matAtB, matX, cv::DECOMP_QR);

    // ---- 退化检测与处理 (仅在第一次迭代时计算) ----
    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));

        // 对信息矩阵 matAtA 进行特征值分解
        // matE 返回从大到小排列的 6 个特征值，matV 每行对应一个特征向量
        cv::eigen(matAtA, matE, matV);
        matV.copyTo(matV2);

        isDegenerate = false;
        // 设置经验阈值：如果特征值小于100，认为该特征方向约束不足（发生退化）
        float eignThre[6] = {10, 10, 10, 10, 10, 10}; 
        
        // 从最小的特征值开始遍历
        for (int i = 5; i >= 0; i--) {
            if (matE.at<float>(0, i) < eignThre[i]) {
                // 如果该方向退化了，把 matV2 中对应的特征向量清零
                // 意思是：在这个不确定的方向上，我们不相信雷达算出来的位姿增量
                for (int j = 0; j < 6; j++) {
                    matV2.at<float>(i, j) = 0;
                }
                isDegenerate = true;
            } else {
                break; // 因为特征值是降序排列的，遇到一个大于阈值的，前面的肯定也大于
            }
        }
        // 计算投影矩阵 P = V^-1 * V2
        // 其作用是：把求出的 Δx 转换到特征空间，将退化方向上的增量置0，再转换回来
        matP = matV.inv() * matV2;
    }

    // ==========================================================
    // 5. 应用位姿更新与收敛判断
    // ==========================================================
    if (isDegenerate) {
        // 如果发生了退化，将原始算出的增量 matX 乘上投影矩阵 matP
        // 剥离掉退化维度上的更新量
        cv::Mat matX2(6, 1, CV_32F, cv::Scalar::all(0));
        matX.copyTo(matX2);
        matX = matP * matX2;
    }

    // 将解出的增量 Δx (平移和旋转) 累加到当前的预测位姿上
    transformTobeMapped[0] += matX.at<float>(0, 0); // roll
    transformTobeMapped[1] += matX.at<float>(1, 0); // pitch
    transformTobeMapped[2] += matX.at<float>(2, 0); // yaw
    transformTobeMapped[3] += matX.at<float>(3, 0); // x
    transformTobeMapped[4] += matX.at<float>(4, 0); // y
    transformTobeMapped[5] += matX.at<float>(5, 0); // z

    // 计算本次迭代实际更新的平移幅度(deltaT)和旋转幅度(deltaR)
    float deltaR = sqrt(
                        pow(pcl::rad2deg(matX.at<float>(0, 0)), 2) +
                        pow(pcl::rad2deg(matX.at<float>(1, 0)), 2) +
                        pow(pcl::rad2deg(matX.at<float>(2, 0)), 2));
    float deltaT = sqrt(
                        pow(matX.at<float>(3, 0) * 100, 2) +
                        pow(matX.at<float>(4, 0) * 100, 2) +
                        pow(matX.at<float>(5, 0) * 100, 2));

    // 如果平移增量小于 0.05 厘米，且旋转增量小于 0.05 度
    // 说明高斯牛顿法已经找到了极小值，再迭代也没意义了，判定为成功收敛
    if (deltaR < 0.05 && deltaT < 0.05) {
        return true; // 告诉上层函数：我算好了，不用再循环迭代了
    }
    
    // 返回 false 表示还没有达到极致收敛，上层会继续进行下一次 iterCount 迭代
    return false;
}

        主要看其中的退化检测部分：

1. 什么是退化？

        在 SLAM 中，“退化”意味着传感器在当前环境中收集不到足够的信息来约束全部的 6 个自由度（x、y、z轴方向上的平移和绕三轴的旋转）。按照右手系的定义，以x轴作为机器人前进方向。

• 场景A（长直走廊）：雷达在一个超长的长廊里前进无法，激光束无法打到长廊的尽头，两边和上下全是平坦的墙。雷达可以完美知道自己有没有左右动、上下动、有没有旋转，但无法判断自己往前走了多远（前后方向退化）。
• 场景B（开阔广场+单面墙）：雷达面前只有一堵巨大的墙。它能知道自己离墙有多远，但在这个平行于墙面的二维平面上滑动时，观测到的点云是一模一样的（该平面内的x、y两个平移自由度退化）。
• 场景C（开阔广场）：无论雷达怎样运动，只能照到一个地面。雷达只能知道帧间离地面有多远，并且是否绕x轴、y轴进行了旋转。此时，对在x、y轴的平移，以及绕z轴的旋转感知退化。

        如果强行优化，噪声会导致状态估计在退化方向上疯狂漂移。

2.退化检测与处理

        在高斯-牛顿法中，被称为信息矩阵或近似海森矩阵。信息矩阵的特征向量代表了优化状态空间中的主方向。信息矩阵的特征值代表了系统在对应特征向量方向上受到的约束强度（信息量的大小）。如果某个特征值极小，说明系统在这个对应的方向上几乎没有约束，这就是退化方向。

        由于环境退化大多具有持续性，LOAM 规定只在第一次迭代 (iterCount == 0) 时进行退化检测并计算投影矩阵 matP，以节省算力。

if (iterCount == 0) {
    // 1. 特征值分解
    cv::eigen(matAtA, matE, matV);
    matV.copyTo(matV2);

    isDegenerate = false;
    float eignThre[6] = {100, 100, 100, 100, 100, 100};
    
    // 2. 检查最小特征值
    for (int i = 5; i >= 0; i--) {
        if (matE.at<float>(0, i) < eignThre[i]) {
            for (int j = 0; j < 6; j++) {
                // 3. 将退化方向的特征向量清零
                matV2.at<float>(i, j) = 0; 
            }
            isDegenerate = true;
        } else {
            break;
        }
    }
    // 4. 计算投影矩阵
    matP = matV.inv() * matV2; 
}

（1）OpenCV返回的 matE 是降序排列的特征值，matV 是按行排列的特征向量矩阵。

（2）从后往前遍历（即从小到大检查特征值）。如果最小的特征值小于设定的经验阈值（100），说明该特征向量所指向的方向退化了。

（3）把 matV2 中对应的特征向量（那一行）全部置为 0。这意味着：“在这个不确定的方向上，我们不信任雷达的匹配结果，干脆就不更新了！”

（4）计算投影矩阵matP：

1. 假设原始状态增量是（matX）。
2. 通过将其转换到特征空间（独立的主方向）。
3. 在特征空间里，我们把退化的那个维度置为 0，这等价于乘上那个修改过的特征向量矩阵。也就是实际执行。
4. 最后，需要把修剪后的更新量转回物理坐标系，需要左乘。
5. 综合起来，修正后的更新量。
6. 所以，投影矩阵（matP = matV.inv() * matV2）。

（5）实施投影约束：

if (isDegenerate)
{
    cv::Mat matX2(6, 1, CV_32F, cv::Scalar::all(0));
    matX.copyTo(matX2);
    matX = matP * matX2;
}

        如果在第一次迭代发现了退化，在后续的每一次迭代中，解算出来的原始步长 matX 都会被强行乘上投影矩阵 matP。

        物理意义： 虽然高斯牛顿法在所有 6 个维度上都算出了一个步长，但通过 matP 投影，系统去除了退化维度上的更新量（保持为0，即相信预测的先验位姿），只保留了那些约束强烈、置信度高的维度上的更新量。