lio-sam框架：点云匹配之手写高斯牛顿下降优化求状态量更新

前言

LIO-SAM的全称是：Tightly-coupled Lidar Inertial Odometry via Smoothing and Mapping

从全称上可以看出，该算法是一个紧耦合的雷达惯导里程计（Tightly-coupled Lidar Inertial Odometry），借助的手段就是利用GT-SAM库中的方法。

LIO-SAM 提出了一个利用GT-SAM的紧耦合激光雷达惯导里程计的框架。
实现了高精度、实时的移动机器人的轨迹估计和建图。

其中建图优化节点整体如下图

在这篇博客中 https://www.guyuehome.com/39723 分析了 点云匹配前戏之初值计算及局部地图构建

在这篇博客中 https://www.guyuehome.com/39767 分析了 点云配准之角点面点的残差及梯度构建

经过之前两片博客，

• 激光雷达当前帧有了初值的估计，那么则可以利用初值投到地图坐标系下
• 构建了局部地图
• 构建了角点和面点的残差及梯度方向

下面可以通过高斯牛顿下降算法来求解状态更新量了

本篇主要分析：如何通过高斯牛顿下降算法来求解状态更新量

代码分

在上一篇分析了 角点和面点的残差及梯度方向，就是

                cornerOptimization();
                surfOptimization();
1
2

这两个函数

下面是通过这个函数将角点和面点的残差及梯度统一到一起

combineOptimizationCoeffs();
1

来看具体内容：

    void combineOptimizationCoeffs()
    {
1
2

        for (int i = 0; i < laserCloudCornerLastDSNum; ++i){
            // 
            if (laserCloudOriCornerFlag[i] == true){
                laserCloudOri->push_back(laserCloudOriCornerVec[i]);//点
                coeffSel->push_back(coeffSelCornerVec[i]);//残差及梯度
            }
        }
1
2
3
4
5
6
7

只有标记为true的时候才是有效约束
点和残差及梯度加入到 laserCloudOri 和coeffSel 中

        for (int i = 0; i < laserCloudSurfLastDSNum; ++i){
            if (laserCloudOriSurfFlag[i] == true){
                laserCloudOri->push_back(laserCloudOriSurfVec[i]);
                coeffSel->push_back(coeffSelSurfVec[i]);
            }
        }
1
2
3
4
5
6

同理处理面点

        std::fill(laserCloudOriCornerFlag.begin(), laserCloudOriCornerFlag.end(), false);
        std::fill(laserCloudOriSurfFlag.begin(), laserCloudOriSurfFlag.end(), false);
1
2

标志位清零

当数据准备好之后，即可开始下面的优化

                if (LMOptimization(iterCount) == true)
                    break;  
1
2

执行优化求解
虽然函数名写的是LM优化算法，但是函数内部是高斯牛顿下降优化算法

来看具体内容：

    bool LMOptimization(int iterCount)
    {
1
2

        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]);
1
2
3
4
5
6

首先求一些 x y z 的三角函数
注意这里有个坐标变换

camera <- lidar 就是雷达到相机坐标系的变换是这样的：

x = y
y = z
z = x
roll = pitch
pitch = yaw
yaw = roll

lidar <- camera 相机到雷达坐标系的变换是这样的：

x = z
y = x
z = y
roll = yaw
pitch = roll
yaw = pitch

transformTobeMapped 是一个6维的数组 分别存放：roll 、pitch、yaw、x、y、z
代码在这里已经做了 雷达到相机的坐标转化

        int laserCloudSelNum = laserCloudOri->size();
        if (laserCloudSelNum < 50) {
            return false;
        }
1
2
3
4

检测有多少个约束，如果约束小于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));
1
2
3
4
5
6

用cv Mat做了些变量定义

        for (int i = 0; i < laserCloudSelNum; i++) {
1

迭代每一个约束点进行优化求解

            pointOri.x = laserCloudOri->points[i].y;
            pointOri.y = laserCloudOri->points[i].z;
            pointOri.z = laserCloudOri->points[i].x;
1
2
3

首先将当前点转到相机系 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;
1
2
3
4

将当前点到线（面）的单位向量转到相机系

            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;
1
2
3
4
5
6
7
8
9
10
11
12

求 雅克比矩阵 旋转部分
相机系下的旋转顺序是 Y - X - Z 对应 lidar 系下的 Z-Y-X

            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;
1
2
3
4
5
6
7

这里就是把camera转到lidar了
赋值 雅克比矩阵

下面 构造 JTJ 以及 -JTe 矩阵

cv::transpose(matA, matAt);
1

matA 是 雅克比矩阵 J
matAt 是 JT

matAtA = matAt * matA;
1

JTJ

matAtB = matAt * matB;
1

-JTe

cv::solve(matAtA, matAtB, matX, cv::DECOMP_QR);
1

利用CV的方法求解增量
JTJX=-JTe

        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);
1
2
3
4
5
6

检测一下 是否有退化得情况
对JTJ 进行特征值分解

            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;
                }
            }
1
2
3
4
5
6
7
8
9
10

特征值从小到大遍历，如果小于阈值就认为退化
对应的特征向量全部置0
退化标志位 为 true

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

如果发生退化，就对增量进行修改，退化方向不更新

        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);
1
2
3
4
5
6

增量更新

        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));
1
2
3
4
5
6
7
8

计算更新的旋转和平移大小

        if (deltaR < 0.05 && deltaT < 0.05) {
            return true; 
        }
1
2
3

如果更新的旋转和平移过小，怎么认为达到最优解

        return false; 
    }
1
2

否则继续优化

求解出来优化结果后 把结果和imu进行一次加权融合

transformUpdate();
1

        if (cloudInfo.imuAvailable == true)
        {
1
2

判断 可以获取九轴imu的世界坐标系下的姿态

            if (std::abs(cloudInfo.imuPitchInit) < 1.4)
            {
1
2

角度没有很大

                transformQuaternion.setRPY(transformTobeMapped[0], 0, 0);
                imuQuaternion.setRPY(cloudInfo.imuRollInit, 0, 0);
                tf::Matrix3x3(transformQuaternion.slerp(imuQuaternion, imuWeight)).getRPY(rollMid, pitchMid, yawMid);
                transformTobeMapped[0] = rollMid;
1
2
3
4

lidar 匹配获得的roll角转成四元数
imu 获得的roll角转成四元数
使用四元的球面插值
插值的结果作为roll的最终结果

                transformQuaternion.setRPY(0, transformTobeMapped[1], 0);
                imuQuaternion.setRPY(0, cloudInfo.imuPitchInit, 0);
                tf::Matrix3x3(transformQuaternion.slerp(imuQuaternion, imuWeight)).getRPY(rollMid, pitchMid, yawMid);
                transformTobeMapped[1] = pitchMid;
1
2
3
4

同理pitch 角度

        transformTobeMapped[0] = constraintTransformation(transformTobeMapped[0], rotation_tollerance);
        transformTobeMapped[1] = constraintTransformation(transformTobeMapped[1], rotation_tollerance);
        transformTobeMapped[5] = constraintTransformation(transformTobeMapped[5], z_tollerance);
1
2
3

对roll pitch 和z进行一些约束，主要针对室内2d场景下，已知2d先验可以加上这些约束

 incrementalOdometryAffineBack = trans2Affine3f(transformTobeMapped);
1

最终的结果也可以转成eigen的结构