3D激光SLAM：LeGO-LOAM---两步优化的帧间里程计及代码分析

前言

LeGO-LOAM的全称是 Lightweight and Ground-Optimized Lidar Odometry and Mapping on Variable Terrain

其中LeGO就是轻量级和利用地面优化，轻量级的实现就是通过两步的优化方式，利用地面优化的部分也在两步优化的第一步中。

和原始LOAM一样，通过前后两帧点云来估计两帧之间的运动，从而累加得到前端里程计的输出，和上述方法使用线面约束同时优化六自由度帧间位姿不同，LeGO-LOAM的前端分成两个步骤，每个步骤估计三自由度的变量。

通过这种方式进行帧间里程计的运算，可以提供运算效率，使得可以在嵌入式平台中运行

在里程计部分，LeGO-LOAM充分利用了前面存储的标签信息，在原本LOAM的里程计部分，其使用的思路更像是ICP，单纯寻找距离最近的点，而并没有考虑其合理性。在这里，LeGO-LOAM通过标签增加了一次筛选，标签相同的点才会用于优化，通过这种方法，能够大幅提高匹配的准确率。

此外，在优化部分，LOAM使用的是直接将所有参数放在一起，进行LM优化，而在LeGO-LOAM中，作者将LM优化分两步进行，每次优化三个参数，在确保准确性的前提下，减小了35%的计算开销。

利用地面点优化

地面点更符合面特征的性质，因此地面点的优化问题就使用点到面的约束来构建

地面点的约束对x，y，yaw这三个自由度是不能估测的。当这个三个自由度的值发生变化，点到面的残差不会发生显著变化，
地面点的优化，只对pitch、roll及z进行约束和优化

白色的为当前帧，绿色的为上一帧，可以看到当前帧发生yaw的旋转，和发生x、y的平移的时候，当前帧的面点到上一帧的距离都没有变，所有不能把这三个自由度的优化放到面点特征上来。

当z轴发生移动，或者发生pitch和roll角的变化，当前帧的面特征点，到上一帧的面特征点的距离即发生变化。

利用角点优化

第一步优化完pitch、roll和z之后，剩下还有三个自由度的变量需要进行估计，
需要提取角点进行优化，由于多线激光雷达提取的角点通常是垂直的边缘特征，因此对，x、y及yaw有着比较好的能观性，通过角点的优化结合上地面点的结果可以得到六自由度的帧间优化结果。

将上一步分优化的z、pitch和roll的三自由度量，和剩下没有估计的三个自由度进行角点的约束优化。

代码部分

LeGO-LOAM的帧间里程计部分在FeatureAssociation.cpp中 。
可以直接找到runFeatureAssociation的函数

        adjustDistortion();//去除畸变

        calculateSmoothness();//计算曲率

        markOccludedPoints();//剔除离散点

        extractFeatures();//提取特征

        publishCloud(); // cloud for visualization
	
        /** 2. Feature Association */
        if (!systemInitedLM) {
    
            checkSystemInitialization();
            return;
        }

        updateInitialGuess();

        updateTransformation();//帧间里程计核心部分

        integrateTransformation();

        publishOdometry();

        publishCloudsLast(); // cloud to mapOptimization

在这个函数中进行了

• 去除畸变
• 计算曲率
• 剔除离散点
• 提取特征
• 帧间里程计核心部分
• 发布里程计
• 发布点云

其中帧间里程计核心部分在 updateTransformation() 函数中

    void updateTransformation(){
    

        if (laserCloudCornerLastNum < 10 || laserCloudSurfLastNum < 100)
            return;

        //地面点帧间里程计估计优化
        for (int iterCount1 = 0; iterCount1 < 25; iterCount1++) {
    
            laserCloudOri->clear();
            coeffSel->clear();

            findCorrespondingSurfFeatures(iterCount1);//通过kdtree 寻找对应的点

            if (laserCloudOri->points.size() < 10)
                continue;
            if (calculateTransformationSurf(iterCount1) == false)//进行优化估计
                break;
        }

        //角点帧间里程计估计优化
        for (int iterCount2 = 0; iterCount2 < 25; iterCount2++) {
    

            laserCloudOri->clear();
            coeffSel->clear();

            findCorrespondingCornerFeatures(iterCount2);//通过kdtree 寻找对应的点

            if (laserCloudOri->points.size() < 10)
                continue;
            if (calculateTransformationCorner(iterCount2) == false)//进行优化估计
                break;
        }
    }

从这个函数中可以明显看出分成了两部分

• 第一个for循环是 地面点帧间里程计估计优化
• 第二个for循环是 角点帧间里程计估计优化

从这里可以看出surfPointsFlat中的点都是地面点

然后在findCorrespondingSurfFeatures的函数中，进行当前帧的地面点与上一帧中的地面点进行匹配。
其过程和ALOAM中的原理一致。

在findCorrespondingCornerFeatures的函数中，进行当前帧的角点与上一帧中的角点进行匹配。
其过程和ALOAM中的原理一致。

当前帧角点i，映射到上一帧点云中，并找到距离最近的点j，在与j相同的scan中，寻找次近邻点m。
利用映射后的点i到j和m的距离，进行约束

通过两步优化的位姿变换通过里程计数据的方式发送出来可以进行可视化的操作
具体代码在 publishOdometry() 函数中

当前帧角点i，映射到上一帧点云中，并找到距离最近的点j，并在相同scan中找到次近邻点l，在相邻的scan中再找到近邻点m。
利用映射后的点i到j、l、m平面的距离，进行约束。

//发布前端里程计
    void publishOdometry(){
    
        geometry_msgs::Quaternion geoQuat = tf::createQuaternionMsgFromRollPitchYaw(transformSum[2], -transformSum[0], -transformSum[1]);

        laserOdometry.header.stamp = cloudHeader.stamp;
        laserOdometry.pose.pose.orientation.x = -geoQuat.y;
        laserOdometry.pose.pose.orientation.y = -geoQuat.z;
        laserOdometry.pose.pose.orientation.z = geoQuat.x;
        laserOdometry.pose.pose.orientation.w = geoQuat.w;
        laserOdometry.pose.pose.position.x = transformSum[3];
        laserOdometry.pose.pose.position.y = transformSum[4];
        laserOdometry.pose.pose.position.z = transformSum[5];
        pubLaserOdometry.publish(laserOdometry);

        laserOdometryTrans.stamp_ = cloudHeader.stamp;
        laserOdometryTrans.setRotation(tf::Quaternion(-geoQuat.y, -geoQuat.z, geoQuat.x, geoQuat.w));
        laserOdometryTrans.setOrigin(tf::Vector3(transformSum[3], transformSum[4], transformSum[5]));
        tfBroadcaster.sendTransform(laserOdometryTrans);
    }

其topic的名称为：

gazebo测试

前端的里程计在Z轴方向已经发生了漂移。
但是地图没有飘，这个是后端进行优化的结果，将前端漂移的部分优化了回来