非线性最小二乘-高斯牛顿法

Jiqiang_z

已于 2024-06-04 14:50:21 修改

阅读量1.1k

点赞数 27

分类专栏： LOAM系列阅读笔记 SLAM学习笔记文章标签：算法机器人 c++

于 2024-02-28 22:28:54 首次发布

本文链接：https://blog.csdn.net/jjjqqq123321/article/details/136357395

版权

SLAM学习笔记同时被 2 个专栏收录

33 篇文章 7 订阅

订阅专栏

LOAM系列阅读笔记

15 篇文章 3 订阅

订阅专栏

前言

在SLAM领域，状态估计问题多采用基于非线性优化的方法，其中高斯牛顿法的使用频率很高。

求解状态估计问题可以转化成非线性最小二乘问题，例如ICP的求解。下面先考虑一个简单的最小二乘问题：

其中F(x)是目标函数，f(x)是目标函数中的非线性函数。如果f(x)为简单的线性函数，那么F(x)的最小值求解就可以通过令目标函数F(x)的导数为零，然后求解x的最优值即可，如下公式所示：

但是f(x)并不一定是简单的线性函数，所以其导函数形式比较复杂，使得该方程不易求解。对于不方便直接求解的最小二乘问题，可以采用迭代的方式，从一个初始值出发，不断地更新当前的优化变量，使目标函数下降。

因此求解F(x)最小值问题就变成了，每次迭代寻找 $\Delta x$ 值来最小化 $F(x+\Delta x)$ 的问题。

现在考虑第 k次迭代，假设我们在 $X_{k}$ 处，想要寻找增量 Δx，那么最直观的方式就是将目标函数在 $X_{k}$ 附近进行泰勒展开：(泰勒展开其实就是用多项式函数来近似原函数的操作)

高斯牛顿法推理

由于H矩阵求解比较困难，因此我们引入了高斯牛顿法。

高斯牛顿法是通过对f(x)在 $X_{k}$ 附近进行一阶泰勒展开，即高斯牛顿是分解目标函数F为f*f后对f的一次偏导的迭代，最够效果等同于目标函数的二次偏导。

将高斯牛顿中的目标函数展开后为：

对于上式，求关于 Δx的导数，并令其为零，得到：

从上面公式可以看出，高斯牛顿法是每一次迭代后，都会在新的迭代点 $X_{k}$ 附近重新进行一阶泰勒展开进行线性化近似，并根据最新迭代点求解新的雅可比矩阵的值。

求解高斯牛顿法的核心公式：

其中 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的特征值就会出现很小的情况(由于长廊中每个位置的差异性很小，雷达帧点云和长廊的局部地图的点云配准的残差都比较小，其非线性优化的曲率就会很小)，就容易无法得到一个很好的优化结果。