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);
1

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

下面来看loopClosureThread这个函数

    void loopClosureThread()
    {
1
2

        if (loopClosureEnableFlag == false)
            return;
1
2

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

ros::Rate rate(loopClosureFrequency);
1

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

        while (ros::ok())
        {
            rate.sleep();
            performLoopClosure();
            visualizeLoopClosure();
        }
1
2
3
4
5
6

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

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

    void performLoopClosure()
    {
1
2

        if (cloudKeyPoses3D->points.empty() == true)
            return;
1
2

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

        mtx.lock();
        *copy_cloudKeyPoses3D = *cloudKeyPoses3D;
        *copy_cloudKeyPoses6D = *cloudKeyPoses6D;
        mtx.unlock();
1
2
3
4

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

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

首先看一下外部通知的回环信息

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

然后根据里程计的距离来检测回环
如果还没有则直接返回

来看detectLoopClosureDistance 函数的具体内容

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

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

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

检查一下较晚帧是否和别的形成了回环，如果有就算了
因为当前帧刚刚出现，不会和其它帧形成回环，所以基本不会触发

kdtreeHistoryKeyPoses->setInputCloud(copy_cloudKeyPoses3D);
1

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

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

根据最后一个关键帧的平移信息，寻找离他一定距离内的其它关键帧

• historyKeyframeSearchRadius 搜索范围 15m

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

遍历找到的候选关键帧

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

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

• historyKeyframeSearchTimeDiff 时间阈值 30s

如果时间上满足要做就找到了历史回环帧
那么赋值id 并且 break
一次找一个回环帧就行了

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

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

        *latestID = loopKeyCur;
        *closestID = loopKeyPre;

        return true;
    }
1
2
3
4
5

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

如果在一个地方静止不动的时候，那么按照这个逻辑也会形成关键帧
可以通过以关键帧序列号的方式加以改进

如果检测回环存在了，那么则可以进行下面内容，就是计算检测出这两帧的位姿变换

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

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

loopFindNearKeyframes(cureKeyframeCloud, loopKeyCur, 0);
1

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

来看 loopFindNearKeyframes 这个函数

    void loopFindNearKeyframes(pcl::PointCloud<PointType>::Ptr& nearKeyframes, const int& key, const int& searchNum)
    {
1
2

        for (int i = -searchNum; i <= searchNum; ++i)
        {
1
2

searchNum 是搜索范围 ，遍历帧的范围

int keyNear = key + i;
1

找到这个 idx

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

如果超出范围了就算了

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

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

        if (nearKeyframes->empty())
            return;
1
2

如果没有有效的点云就算了

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

吧点云下采样

然后会到之前的地方：

loopFindNearKeyframes(prevKeyframeCloud, loopKeyPre, historyKeyframeSearchNum);
1

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

• historyKeyframeSearchNum 25帧

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

如果点云数目太少就算了

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

把局部地图发布出来供rviz可视化使用

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

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

使用简单的icp来进行帧到局部地图的配准

icp.setMaxCorrespondenceDistance(historyKeyframeSearchRadius*2);
1

设置最大相关距离

• historyKeyframeSearchRadius 15m

icp.setMaximumIterations(100);
1

最大优化次数

icp.setTransformationEpsilon(1e-6);
1

单次变换范围

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

残差设置

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

设置两个点云

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

执行配准

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

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

把修正后的当前点云发布供可视化使用

        correctionLidarFrame = icp.getFinalTransformation();
1

获得两个点云的变换矩阵结果

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

取出当前帧的位姿

 Eigen::Affine3f tCorrect = correctionLidarFrame * tWrong;
1

将icp结果补偿过去，就是当前帧的更为准确的位姿结果

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

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

gtsam::Pose3 poseFrom = Pose3(Rot3::RzRyRx(roll, pitch, yaw), Point3(x, y, z));
1

gtsam::Pose3 poseTo = pclPointTogtsamPose3(copy_cloudKeyPoses6D->points[loopKeyPre]);
1

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

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

使用icp的得分作为他们的约束噪声项

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

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

loopIndexContainer[loopKeyCur] = loopKeyPre;
1

保存已经存在的约束对

总结

lio-sam回环检测的方式
构建关键帧，将关键帧的位姿存储。以固定频率进行回环检测。每次处理最新的关键帧，通过kdtree寻找历史关键帧中距离和时间满足条件的一个关键帧。然后就认为形成了回环。
形成回环后，历史帧周围25帧，构建局部地图，与当前关键帧进行icp匹配求解位姿变换。

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