本文共享自华为云社区《lio-sam框架：回环检测及位姿核算》，作者：月照银海似蛟龙。

前言

图优化自身有成形的开源的库例如

g2o
ceres
gtsam

lio-sam 中便是经过 gtsam 库进行图优化的，其间束缚因子就包括回环检测因子

本篇首要解析lio-sam框架下，是如何进行回环检测及位姿核算的。

Pose Graph的概念

用一个图(Graph 图论)来表明SLAM问题

图中的节点来表明机器人的位姿二维的话即为 (x,y,yaw)

两个节点之间的边表明两个位姿的空间束缚(相对位姿关系以及对应方差或线性矩阵)

边分为了两种边

帧间边:衔接的前后,时刻上是连续的
回环边:衔接的前后,时刻上是不连续的,可是直接也是两个位姿的空间束缚

构建了回环边才会有差错呈现,没有回环边是没有差错的

图优化的基本思想:
呈现回环边,有了差错之后.构建图,而且找到一个最优的配置(各节点的位姿),让猜测与观测的差错最小

一旦构成回环即可进行优化消除差错

里程积分的相对位姿视为猜测值 图上的各个节点便是经过里程(激光里程计\轮速里程计)积分得到的
回环核算的相对位姿视为观测值 图上便是说经过 X2和X8的帧间匹配作为观测值

图优化要干的事:
构建图并调整各节点的位姿,让猜测与观测的差错最小

回环检测及位姿核算

在点云匹配之后，可以来看回环检测部分的代码了

这部分的代码进口在 main函数中

std::thread loopthread(&mapOptimization::loopClosureThread, &MO);

独自开了一个回环检测的线程

下面来看loopClosureThread这个函数

    void loopClosureThread()
    {
        if (loopClosureEnableFlag == false)
            return;

假如不需要进行回环检测，那么就退出这个线程

ros::Rate rate(loopClosureFrequency);

设置回环检测的频率 loopClosureFrequency默以为 1hz
没有必要太频频

        while (ros::ok())
        {
            rate.sleep();
            performLoopClosure();
            visualizeLoopClosure();
        }

设置完频率后，进行一个while的死循环。
履行完一次就必须sleep一段时刻，否则该线程的cpu占用会十分高
经过performLoopClosure visualizeLoopClosure 履行回环检测

下面来看performLoopClosure 函数的具体内容

    void performLoopClosure()
    {
        if (cloudKeyPoses3D->points.empty() == true)
            return;

假如没有关键帧，就没法进行回环检测了
就直接退出

        mtx.lock();
        *copy_cloudKeyPoses3D = *cloudKeyPoses3D;
        *copy_cloudKeyPoses6D = *cloudKeyPoses6D;
        mtx.unlock();

把存储关键帧额位姿的点云copy出来，避免线程抵触 cloudKeyPoses3D便是关键帧的位置 cloudKeyPoses6D便是关键帧的位姿

if (detectLoopClosureExternal(&loopKeyCur, &loopKeyPre) == false)

首先看一下外部告诉的回环信息

            if (detectLoopClosureDistance(&loopKeyCur, &loopKeyPre) == false)
                return;

然后依据里程计的间隔来检测回环
假如还没有则直接返回

来看detectLoopClosureDistance 函数的具体内容

        int loopKeyCur = copy_cloudKeyPoses3D->size() - 1;
        int loopKeyPre = -1;

检测最新帧是否和其它帧构成回环
取出最新帧的索引

        auto it = loopIndexContainer.find(loopKeyCur);
        if (it != loopIndexContainer.end())
            return false;

检查一下较晚帧是否和其他构成了回环，假如有就算了
由于当时帧刚刚呈现，不会和其它帧构成回环，所以基本不会触发

kdtreeHistoryKeyPoses->setInputCloud(copy_cloudKeyPoses3D);

把只包括关键帧位移信息的点云填充kdtree

        kdtreeHistoryKeyPoses->radiusSearch(copy_cloudKeyPoses3D->back(), historyKeyframeSearchRadius, pointSearchIndLoop, pointSearchSqDisLoop, 0);

依据最后一个关键帧的平移信息，寻觅离他必定间隔内的其它关键帧

historyKeyframeSearchRadius 搜索规模 15m

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

遍历找到的候选关键帧

            int id = pointSearchIndLoop[i];
            if (abs(copy_cloudKeyPoses6D->points[id].time - timeLaserInfoCur) > historyKeyframeSearchTimeDiff)
            {  
                loopKeyPre = id;
                break;
            }

前史帧，必须比当时帧间隔30s以上
必须满意时刻上超过必定阈值，才以为是一个有用的回环

historyKeyframeSearchTimeDiff 时刻阈值 30s

假如时刻上满意要做就找到了前史回环帧
那么赋值id 而且 break
一次找一个回环帧就行了

        if (loopKeyPre == -1 || loopKeyCur == loopKeyPre)
            return false;

假如没有找到回环或者回环找到自己身上去了，就以为是本次回环寻觅失败

        *latestID = loopKeyCur;
        *closestID = loopKeyPre;
        return true;
    }

至此则找到了当真关键帧和前史回环帧
赋值当时帧和前史回环帧的id

假如在一个当地静止不动的时分，那么按照这个逻辑也会构成关键帧
可以经过以关键帧序列号的方法加以改进

假如检测回环存在了，那么则可以进行下面内容，便是核算检测出这两帧的位姿改换

        pcl::PointCloud<PointType>::Ptr cureKeyframeCloud(new pcl::PointCloud<PointType>());
        pcl::PointCloud<PointType>::Ptr prevKeyframeCloud(new pcl::PointCloud<PointType>());

声明当时关键帧的点云
声明前史回环帧周围的点云（部分地图）

loopFindNearKeyframes(cureKeyframeCloud, loopKeyCur, 0);

当时关键帧把自己取了出来

来看 loopFindNearKeyframes 这个函数

    void loopFindNearKeyframes(pcl::PointCloud<PointType>::Ptr& nearKeyframes, const int& key, const int& searchNum)
    {
        for (int i = -searchNum; i <= searchNum; ++i)
        {

searchNum 是搜索规模，遍历帧的规模

int keyNear = key + i;

找到这个 idx

            if (keyNear < 0 || keyNear >= cloudSize )
                continue;

假如超出规模了就算了

            *nearKeyframes += *transformPointCloud(cornerCloudKeyFrames[keyNear], &copy_cloudKeyPoses6D->points[keyNear]);
            *nearKeyframes += *transformPointCloud(surfCloudKeyFrames[keyNear],   &copy_cloudKeyPoses6D->points[keyNear]);

否则吧对应角点和面点的点云转到世界坐标系下去

        if (nearKeyframes->empty())
            return;

假如没有有用的点云就算了

        pcl::PointCloud<PointType>::Ptr cloud_temp(new pcl::PointCloud<PointType>());
        downSizeFilterICP.setInputCloud(nearKeyframes);
        downSizeFilterICP.filter(*cloud_temp);
        *nearKeyframes = *cloud_temp;

吧点云下采样

然后会到之前的当地：

loopFindNearKeyframes(prevKeyframeCloud, loopKeyPre, historyKeyframeSearchNum);

回环帧把自己周围一些点云取出来，也便是构成一个帧部分地图的一个匹配问题

historyKeyframeSearchNum 25帧

        if (cureKeyframeCloud->size() < 300 || prevKeyframeCloud->size() < 1000)
            return;

假如点云数目太少就算了

            if (pubHistoryKeyFrames.getNumSubscribers() != 0)
                publishCloud(&pubHistoryKeyFrames, prevKeyframeCloud, timeLaserInfoStamp, odometryFrame);

把部分地图发布出来供rviz可视化运用

现在有了当时关键帧投到地图坐标系下的点云和前史回环帧投到地图坐标系下的部分地图，那么接下来就可以进行两者的icp位姿改换求解

 static pcl::IterativeClosestPoint<PointType, PointType> icp;

运用简单的icp来进行帧到部分地图的配准

icp.setMaxCorrespondenceDistance(historyKeyframeSearchRadius*2);

设置最大相关间隔

historyKeyframeSearchRadius 15m

icp.setMaximumIterations(100);

最大优化次数

icp.setTransformationEpsilon(1e-6);

单次改换规模

icp.setEuclideanFitnessEpsilon(1e-6);
icp.setRANSACIterations(0);

残差设置

        icp.setInputSource(cureKeyframeCloud);
        icp.setInputTarget(prevKeyframeCloud);

设置两个点云

        pcl::PointCloud<PointType>::Ptr unused_result(new pcl::PointCloud<PointType>());
        icp.align(*unused_result);

履行配准

        if (icp.hasConverged() == false || icp.getFitnessScore() > historyKeyframeFitnessScore)
            return;

检测icp是否收敛且得分是否满意要求

        if (pubIcpKeyFrames.getNumSubscribers() != 0)
        {
            pcl::PointCloud<PointType>::Ptr closed_cloud(new pcl::PointCloud<PointType>());
            pcl::transformPointCloud(*cureKeyframeCloud, *closed_cloud, icp.getFinalTransformation());
            publishCloud(&pubIcpKeyFrames, closed_cloud, timeLaserInfoStamp, odometryFrame);
        }

把修正后的当时点云发布供可视化运用

        correctionLidarFrame = icp.getFinalTransformation();

取得两个点云的改换矩阵成果

 Eigen::Affine3f tWrong = pclPointToAffine3f(copy_cloudKeyPoses6D->points[loopKeyCur]);

取出当时帧的位姿

 Eigen::Affine3f tCorrect = correctionLidarFrame * tWrong;

将icp成果补偿曩昔，便是当时帧的更为精确的位姿成果

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

将当时帧补偿后的位姿转换成平移和旋转

gtsam::Pose3 poseFrom = Pose3(Rot3::RzRyRx(roll, pitch, yaw), Point3(x, y, z));
gtsam::Pose3 poseTo = pclPointTogtsamPose3(copy_cloudKeyPoses6D->points[loopKeyPre]);

将当时帧补偿后的位姿转换成 gtsam的方式
From 和 To相当于帧间束缚的因子
To是前史回环帧的位姿

gtsam::Vector Vector6(6);
float noiseScore = icp.getFitnessScore();
noiseModel::Diagonal::shared_ptr constraintNoise = noiseModel::Diagonal::Variances(Vector6);

运用icp的得分作为他们的束缚噪声项

        loopIndexQueue.push_back(make_pair(loopKeyCur, loopKeyPre));//两帧索引
        loopPoseQueue.push_back(poseFrom.between(poseTo));//当时帧与前史回环帧相对位姿
        loopNoiseQueue.push_back(constraintNoise);//噪声

将两帧索引，两帧相对位姿和噪声作为回环束缚送入对列

loopIndexContainer[loopKeyCur] = loopKeyPre;

保存已经存在的束缚对

总结

lio-sam回环检测的方法

构建关键帧，将关键帧的位姿存储。以固定频率进行回环检测。每次处理最新的关键帧，经过kdtree寻觅前史关键帧中间隔和时刻满意条件的一个关键帧。然后就以为构成了回环。
构成回环后，前史帧周围25帧，构建部分地图，与当时关键帧进行icp匹配求解位姿改换。

lio-sam 以为里程计累计漂移比较小，所以经过间隔与时刻这两个概念进行的关键帧的回环检测

点击关注，第一时刻了解华为云新鲜技术~

基于lio-sam框架，教你如何进行回环检测及位姿计算

前言

Pose Graph的概念

回环检测及位姿核算

总结

作者信息

基于lio-sam框架，教你如何进行回环检测及位姿计算

前言

Pose Graph的概念

回环检测及位姿核算

总结

相关文章

详解 RxJS 创建型可观察对象操作符

Flutter卡片分享功能实现：将你的内容分享给世界

RAG pipeline 优化策略概述

有奖征文 | 2022 云原生编程挑战赛征稿活动开启！

作者信息