我正在参与「启航计划」

前语

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

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

LIO-SAM 提出了一个运用GT-SAM的紧耦合激光雷达惯导里程计的结构。完成了高精度、实时的移动机器人的轨道估量和建图。

其间建图优化节点全体如下图

在之前的博客中，咱们分部完成了

帧间点云匹配
回环检测及位姿核算

本篇咱们来看在lio-sam结构中，是如何将三者（雷达里程计、回环检测、gps）进行交融，来完成大局位姿优化的。

代码解析

    void saveKeyFramesAndFactor()
    {

保存关键帧增加 factor

        if (saveFrame() == false)
            return;

通过旋转和平移增量，判断是否是关键帧假如不是关键帧则不往因子图里加factor

来看saveFrame()函数

    bool saveFrame()
    {
        if (cloudKeyPoses3D->points.empty())
            return true;

假如没有关键帧，就直接认为是关键帧

Eigen::Affine3f transStart = pclPointToAffine3f(cloudKeyPoses6D->back());

取出上一个关键帧的位姿

        Eigen::Affine3f transFinal = pcl::getTransformation(transformTobeMapped[3], transformTobeMapped[4], transformTobeMapped[5],
                                                            transformTobeMapped[0], transformTobeMapped[1], transformTobeMapped[2]);

当时帧的位姿转成eigen形式

Eigen::Affine3f transBetween = transStart.inverse() * transFinal;

核算两个位姿之间的 delta pose

        float x, y, z, roll, pitch, yaw;
        pcl::getTranslationAndEulerAngles(transBetween, x, y, z, roll, pitch, yaw);

转成平移 + 旋转（欧拉角）的形式

        if (abs(roll)  < surroundingkeyframeAddingAngleThreshold &&
            abs(pitch) < surroundingkeyframeAddingAngleThreshold && 
            abs(yaw)   < surroundingkeyframeAddingAngleThreshold &&
            sqrt(x*x + y*y + z*z) < surroundingkeyframeAddingDistThreshold)
            return false;
        return true;

任何一个旋转大于给定阈值或许平移大于给定阈值就认为是关键帧

阈值的设置在 params.yaml 文件中

视点阈值是 0.2 弧度平移阈值是 1 米

增加激光雷达帧间里程计因子

回到 saveKeyFramesAndFactor 函数中假如是关键帧的话就给isam增加因子

        addOdomFactor();

增加odom 的因子来看具体内容

    void addOdomFactor()
    {

        if (cloudKeyPoses3D->points.empty())
        {

假如是榜首帧关键帧

noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Variances((Vector(6) << 1e-2, 1e-2, M_PI*M_PI, 1e8, 1e8, 1e8).finished());

置信度就设置差一点，尤其是不可观的平移和yaw角

gtSAMgraph.add(PriorFactor<Pose3>(0, trans2gtsamPose(transformTobeMapped), priorNoise));

增加先验束缚，对第 0 个节点增加束缚

initialEstimate.insert(0, trans2gtsamPose(transformTobeMapped));

参加节点信息初始值

}else{

假如不是榜首帧，就增加帧间束缚

noiseModel::Diagonal::shared_ptr odometryNoise = noiseModel::Diagonal::Variances((Vector(6) << 1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4).finished());

这时帧间束缚置信度就设置高一些

gtsam::Pose3 poseFrom = pclPointTogtsamPose3(cloudKeyPoses6D->points.back());

上一关键帧位姿转成 gtsam的格式

gtsam::Pose3 poseTo   = trans2gtsamPose(transformTobeMapped);

当时关键帧位姿转成 gtsam的格式

gtSAMgraph.add(BetweenFactor<Pose3>(cloudKeyPoses3D->size()-1, cloudKeyPoses3D->size(), poseFrom.between(poseTo), odometryNoise));

这是一个帧间束缚，分别输入两个节点的 id，帧间束缚大小以及置信度

 initialEstimate.insert(cloudKeyPoses3D->size(), poseTo);

参加节点信息先验位姿

增加GPS因子

回到 saveKeyFramesAndFactor 函数中

addGPSFactor();

gps 的因子

    void addGPSFactor()
    {
        if (gpsQueue.empty())
            return;

假如没有gps信息就算了

        if (cloudKeyPoses3D->points.empty())
            return;

假如有gps可是没有关键帧信息也算了由于gps 是给关键帧供给束缚的

        else
        {
            if (pointDistance(cloudKeyPoses3D->front(), cloudKeyPoses3D->back()) < 5.0)
                return;
        }

榜首个关键帧和最终一个关键帧相差很近，也就算了，要么刚起步，要么会触发回环

        if (poseCovariance(3,3) < poseCovThreshold && poseCovariance(4,4) < poseCovThreshold)
            return;

gtsam 反馈的当时 x、y 的置信度，假如置信度比较高也不需求 gps来进行优化

        while (!gpsQueue.empty())
        {

            if (gpsQueue.front().header.stamp.toSec() < timeLaserInfoCur - 0.2)
            {
                gpsQueue.pop_front();
            }

把间隔当时帧比较早的帧都扔掉

            else if (gpsQueue.front().header.stamp.toSec() > timeLaserInfoCur + 0.2)
            {
                break;
            }`                float noise_x = thisGPS.pose.covariance[0];
                float noise_y = thisGPS.pose.covariance[7];
                float noise_z = thisGPS.pose.covariance[14];
                // 假如gps的置信度也不高，也没有必要运用了
                if (noise_x > gpsCovThreshold || noise_y > gpsCovThreshold)
                    continue;`

比较晚就索性再等等lidar核算

            {
                // 寿命这个gps 时刻上间隔当时帧现已比较近了，那就把这个数据取出来
                nav_msgs::Odometry thisGPS = gpsQueue.front();
                gpsQueue.pop_front();

说明这个gps 时刻上间隔当时帧现已比较近了，那就把这个数据取出来

                float noise_x = thisGPS.pose.covariance[0];
                float noise_y = thisGPS.pose.covariance[7];
                float noise_z = thisGPS.pose.covariance[14];
                if (noise_x > gpsCovThreshold || noise_y > gpsCovThreshold)
                    continue;

假如gps的置信度也不高，也没有必要运用了

                float gps_x = thisGPS.pose.pose.position.x;
                float gps_y = thisGPS.pose.pose.position.y;
                float gps_z = thisGPS.pose.pose.position.z;

取出gps 的位置

                if (!useGpsElevation)
                {
                    gps_z = transformTobeMapped[5];
                    noise_z = 0.01;
                }

通常gps 的z 没有 x y准，因而这儿可以不运用z值 useGpsElevation 默认为0 直接拿里程计的z 并设置高置信度

                if (abs(gps_x) < 1e-6 && abs(gps_y) < 1e-6)
                    continue;

假如gps 的 x 或许 y 太小，说明还没有初始化好

                PointType curGPSPoint;
                curGPSPoint.x = gps_x;
                curGPSPoint.y = gps_y;
                curGPSPoint.z = gps_z;
                if (pointDistance(curGPSPoint, lastGPSPoint) < 5.0)
                    continue;
                else
                    lastGPSPoint = curGPSPoint;

参加gps观测不宜太频繁，相邻不能超过5m

                gtsam::Vector Vector3(3);
                Vector3 << max(noise_x, 1.0f), max(noise_y, 1.0f), max(noise_z, 1.0f);

gps 的置信度，标准差设置成最小1m，也便是不会特别信赖gps信息

noiseModel::Diagonal::shared_ptr gps_noise = noiseModel::Diagonal::Variances(Vector3);

设置节点置信度

 gtsam::GPSFactor gps_factor(cloudKeyPoses3D->size(), gtsam::Point3(gps_x, gps_y, gps_z), gps_noise);
 gtSAMgraph.add(gps_factor);

调用gtsam 中集成的gps 束缚

                aLoopIsClosed = true;
                break;

参加gps 之后等同于回环，需求触发较多的isam update

增加回环检测因子

回到 saveKeyFramesAndFactor 函数中

    void addLoopFactor()
    {

        if (loopIndexQueue.empty())
            return;

有一个专门的回环检测线程会检测回环，检测到就会给这个行列塞入回环束缚

        for (int i = 0; i < (int)loopIndexQueue.size(); ++i)

把行列里边一切的回环束缚增加进行

            int indexFrom = loopIndexQueue[i].first;//当时帧
            int indexTo = loopIndexQueue[i].second;// 回环帧

当时帧回环帧索引

gtsam::Pose3 poseBetween = loopPoseQueue[i];

帧间束缚

gtsam::noiseModel::Diagonal::shared_ptr noiseBetween = loopNoiseQueue[i];

回环的置信度便是icp的得分

gtSAMgraph.add(BetweenFactor<Pose3>(indexFrom, indexTo, poseBetween, noiseBetween));

参加束缚

        loopIndexQueue.clear();
        loopPoseQueue.clear();
        loopNoiseQueue.clear();

清空回环相关行列

        aLoopIsClosed = true;
    }

标志位至 true

isam 更新

回到 saveKeyFramesAndFactor 函数中

        isam->update(gtSAMgraph, initialEstimate);
        isam->update();

一切因子加完了，就调用isam 接口，更新图模型

        if (aLoopIsClosed == true)
        {
            isam->update();
            isam->update();
            isam->update();
            isam->update();
            isam->update();
        }

假如参加了gps束缚或许回环束缚，isam需求进行更多次的优化

        gtSAMgraph.resize(0);
        initialEstimate.clear();

将束缚和节点信息清空，他们现已呗参加到isam中去了，因而这儿清空不会影响整个优化

isamCurrentEstimate = isam->calculateEstimate();

通过接口获得所以变量的状况

latestEstimate = isamCurrentEstimate.at<Pose3>(isamCurrentEstimate.size()-1);

取出优化后的最新关键帧位姿

        thisPose3D.x = latestEstimate.translation().x();
        thisPose3D.y = latestEstimate.translation().y();
        thisPose3D.z = latestEstimate.translation().z();
        thisPose3D.intensity = cloudKeyPoses3D->size(); 
        cloudKeyPoses3D->push_back(thisPose3D);

平移信息取出来保存进clouKeyPoses 3D这个结构中，其间索引作为 intensity

        thisPose6D.x = thisPose3D.x;
        thisPose6D.y = thisPose3D.y;
        thisPose6D.z = thisPose3D.z;
        thisPose6D.intensity = thisPose3D.intensity ;
        thisPose6D.roll  = latestEstimate.rotation().roll();
        thisPose6D.pitch = latestEstimate.rotation().pitch();
        thisPose6D.yaw   = latestEstimate.rotation().yaw();
        thisPose6D.time = timeLaserInfoCur;
        cloudKeyPoses6D->push_back(thisPose6D);

6D姿势相同保存下来

 poseCovariance = isam->marginalCovariance(isamCurrentEstimate.size()-1);

保存当时位姿的置信度用于是否运用gps的判断

        transformTobeMapped[0] = latestEstimate.rotation().roll();
        transformTobeMapped[1] = latestEstimate.rotation().pitch();
        transformTobeMapped[2] = latestEstimate.rotation().yaw();
        transformTobeMapped[3] = latestEstimate.translation().x();
        transformTobeMapped[4] = latestEstimate.translation().y();
        transformTobeMapped[5] = latestEstimate.translation().z();

将优化后的位姿更新到transformTobeMapped数组中，作为当时最佳估量值

        pcl::copyPointCloud(*laserCloudCornerLastDS,  *thisCornerKeyFrame);
        pcl::copyPointCloud(*laserCloudSurfLastDS,    *thisSurfKeyFrame);

当时帧的点云的角点和面点分别拷贝一下

        cornerCloudKeyFrames.push_back(thisCornerKeyFrame);
        surfCloudKeyFrames.push_back(thisSurfKeyFrame);

关键帧的点云保存下来

 updatePath(thisPose6D);

根据当时最新位姿更新rviz可视化

调整大局轨道

下面来看调整大局轨道

    void correctPoses()
    {

        if (cloudKeyPoses3D->points.empty())
            return;

没有关键帧，天然也没有什么含义

        if (aLoopIsClosed == true)
        {

只有回环以及gps信息这些会触发大局调整信息才会触发

laserCloudMapContainer.clear();

很多位姿会变化，因子之前的容器内转到世界坐标系下的很多点云就需求调整，因而这儿索性清空 laserCloudMapContainer 这儿存放关键帧的位姿和点云

globalPath.poses.clear();

清空path

            int numPoses = isamCurrentEstimate.size();
            for (int i = 0; i < numPoses; ++i)
            {   
                // 更新一切关键帧的位姿
                cloudKeyPoses3D->points[i].x = isamCurrentEstimate.at<Pose3>(i).translation().x();
                cloudKeyPoses3D->points[i].y = isamCurrentEstimate.at<Pose3>(i).translation().y();
                cloudKeyPoses3D->points[i].z = isamCurrentEstimate.at<Pose3>(i).translation().z();
                cloudKeyPoses6D->points[i].x = cloudKeyPoses3D->points[i].x;
                cloudKeyPoses6D->points[i].y = cloudKeyPoses3D->points[i].y;
                cloudKeyPoses6D->points[i].z = cloudKeyPoses3D->points[i].z;
                cloudKeyPoses6D->points[i].roll  = isamCurrentEstimate.at<Pose3>(i).rotation().roll();
                cloudKeyPoses6D->points[i].pitch = isamCurrentEstimate.at<Pose3>(i).rotation().pitch();
                cloudKeyPoses6D->points[i].yaw   = isamCurrentEstimate.at<Pose3>(i).rotation().yaw();
                // 一起更新path
                updatePath(cloudKeyPoses6D->points[i]);
            }

然后更新一切的位姿

            aLoopIsClosed = false;
        }

标志位至 0

lio-sam框架：后端里程计、回环、gps融合

前语

代码解析

增加激光雷达帧间里程计因子

增加GPS因子

增加回环检测因子

isam 更新

调整大局轨道

总结

作者信息

lio-sam框架：后端里程计、回环、gps融合

前语

代码解析

增加激光雷达帧间里程计因子

增加GPS因子

增加回环检测因子

isam 更新

调整大局轨道

总结

相关文章

SwiftUI：做一个好看的评分控件

一种新型的系统设计解决方案：模块树驱动设计

Android使用Koin依赖项注入 – 第一弹

Handler 源码解析（一）—— Handler 的工作流程

作者信息