在SLAM领域，后端多采用基于非线性优化的方法，来优化位姿和地图点，其中高斯牛顿法的使用频率很高。
求解高斯牛顿法的核心公式：
其中 f 是误差函数，J是误差关于待优化变量的雅可比矩阵。
其中H为海森矩阵（error相对于变量的二阶导数的近似 ），海森矩阵的特征向量表示了优化问题中的不同方向，海森矩阵的特征值可以提供关于优化问题在不同方向上的曲率大小的信息，在高斯-牛顿算法中，特征值可以用作步长（步幅）的参考。
但是在实际的计算中，JTJ却只有半正定性，因此可能会出现奇异矩阵或者病态的情况。在SLAM的非线性优化中使用高斯牛顿法求解时，会通过近似的H矩阵的特征值大小判断是否发生退化情况。
特征值和特征向量的几何意义：
矩阵特征值，奇异值分解_奇异值分解的特征矩阵-CSDN博客
LIO-SAM中scan-to-map的非线性优化问题采用了高斯牛顿法：
/**
* scan-to-map优化
* 对匹配特征点计算Jacobian矩阵，观测值为特征点到直线、平面的距离，构建高斯牛顿方程，迭代优化当前位姿，存transformTobeMapped
* 公式推导：todo
*/
bool LMOptimization(int iterCount)
{
// This optimization is from the original loam_velodyne by Ji Zhang, need to cope with coordinate transformation
// lidar <- camera --- camera <- lidar
// x = z --- x = y
// y = x --- y = z
// z = y --- z = x
// roll = yaw --- roll = pitch
// pitch = roll --- pitch = yaw
// yaw = pitch --- yaw = roll
// lidar -> camera
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]);
// 当前帧匹配特征点数太少
int laserCloudSelNum = laserCloudOri->size();
if (laserCloudSelNum < 50) {
return false;
}
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));
PointType pointOri, coeff;
// 遍历匹配特征点，构建Jacobian矩阵
for (int i = 0; i < laserCloudSelNum; i++) {
// lidar -> camera todo
// 首先将当前点以及点到线（面）的单位向量转到相机系
pointOri.x = laserCloudOri->points[i].y;
pointOri.y = laserCloudOri->points[i].z;
pointOri.z = laserCloudOri->points[i].x;
// 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;
// in camera
// 对Roll角的求导
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;
// lidar -> camera
// 雅可比矩阵，【6，1】的矩阵，前3个是误差关于旋转的导数，后3个是误差关于平移的导数。
matA.at<float>(i, 0) = arz;
matA.at<float>(i, 1) = arx;
matA.at<float>(i, 2) = ary;
matA.at<float>(i, 3) = coeff.z;
matA.at<float>(i, 4) = coeff.x;
matA.at<float>(i, 5) = coeff.y;
// 点到直线距离、平面距离，作为观测值
matB.at<float>(i, 0) = -coeff.intensity;
}
// 构造JTJ（H）以及-JTe(b)矩阵，matAt是转置
cv::transpose(matA, matAt);
matAtA = matAt * matA;
matAtB = matAt * matB;
// J^T·J·delta_x = -J^T·f 高斯牛顿，matX是增量待求解量。
cv::solve(matAtA, matAtB, matX, cv::DECOMP_QR);
// 首次迭代，检查近似Hessian矩阵（J^T·J）是否退化，或者称为奇异，行列式值=0 todo
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));
// 对H进行特征值分解
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.at<float>(0, i) < eignThre[i]) {
for (int j = 0; j < 6; j++) {
matV2.at<float>(i, j) = 0;
}
isDegenerate = true;
} else {
break;
}
}
matP = matV.inv() * matV2;
}
// 如果发生退化，就对增量进行修改，退化方向不更新
if (isDegenerate)
{
cv::Mat matX2(6, 1, CV_32F, cv::Scalar::all(0));
matX.copyTo(matX2);
matX = matP * matX2;
}
// 更新当前位姿 x = x + delta_x
transformTobeMapped[0] += matX.at<float>(0, 0);
transformTobeMapped[1] += matX.at<float>(1, 0);
transformTobeMapped[2] += matX.at<float>(2, 0);
transformTobeMapped[3] += matX.at<float>(3, 0);
transformTobeMapped[4] += matX.at<float>(4, 0);
transformTobeMapped[5] += matX.at<float>(5, 0);
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));
// delta_x很小，认为收敛
if (deltaR < 0.05 && deltaT < 0.05) {
return true;
}
// 否则继续优化
return false;
}其中一步对H矩阵做了特征值分解，若特征值太小，则说明优化方向的曲率过小，在SLAM中认为这种情况下为退化环境，非线性优化无法得到一个最优的结果。
例如长廊下的退化环境，进行scan-to-map的非线性优化时，此时求解H的特征值就会出现很小的情况(由于长廊中每个位置的差异性很小，雷达帧点云和长廊的局部地图的点云配准的残差都比较小，其非线性优化的曲率就会很小)，就容易无法得到一个很好的优化结果。
