LIO-SAM后端中的回环检测及位姿计算

Rhys___

已于 2024-05-08 15:34:00 修改

阅读量1.8k

点赞数 4

分类专栏：激光SLAM-LOAM系列文章标签：算法 c++ 自动驾驶

于 2022-09-15 19:22:12 首次发布

本文链接：https://blog.csdn.net/zysss_/article/details/126874947

版权

该博客介绍了LIO-SAM中回环检测的实现过程，包括利用里程计判断回环，寻找15米内时间间隔超过30s的关键帧，以及通过ICP匹配计算两帧位姿变换。在回环检出后，构建局部点云地图进行匹配，并通过优化排除错误匹配，获取正确的位姿结果。最后，将相关信息加入回环约束队列，用于全局误差优化。

摘要由CSDN通过智能技术生成

文章目录

performLoopClosure()
- 寻找回环关键帧detectLoopClosureDistance
- 检出回环之后开始计算两帧位姿变换

回环检测函数调用在mapOptmization.cpp 的main中单独开了一个线程运行，1秒运行一遍回环检测

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

回环检测思路描述：主要是通过里程计判断回环，根据最后一个关键帧的平移信息，寻找离他15米内的其它关键帧，并且两帧的时间戳相差要大于30s，上面两个条件都满足则认为是找到了回环帧，开始用icp匹配计算位姿，算完后把两帧索引，两帧相对位姿，噪声（icp得分）放入回环约束队列中，对回环因子的使用，和因子图的更新在函数saveKeyFramesAndFactor()中，后面会详解

流程综述：

如果不需要进行回环检测就退出这个线程，有标志位设置

    // 回环检测线程
    void loopClosureThread()
    {
   
        // 如果不需要进行回环检测，那么就退出这个线程
        if (loopClosureEnableFlag == false)
            return;
        // 设置回环检测的频率
        ros::Rate rate(loopClosureFrequency);
        while (ros::ok())
        {
   
            // 执行完一次就必须sleep一段时间，否则该线程的cpu占用会非常高
            rate.sleep();
            // 执行回环检测
            performLoopClosure();
            visualizeLoopClosure();
        }
    }

执行回环检测

performLoopClosure()

如果没有关键帧就无法退出回环检测

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

把存储关键帧的位姿的点云copy出来，避免线程冲突，这里开了把锁防止数据调用冲突

mtx.lock();
// 把存储关键帧的位姿的点云copy出来，避免线程冲突
*copy_cloudKeyPoses3D = *cloudKeyPoses3D;
*copy_cloudKeyPoses6D = *cloudKeyPoses6D;
mtx.unlock()

寻找回环关键帧detectLoopClosureDistance

主要是通过里程计判断回环，根据最后一个关键帧的平移信息，寻找离他15米内的其它关键帧，并且两帧的时间戳相差要大于30s，上面两个条件都满足则认为是找到了回环帧

    bool detectLoopClosureDistance(int *latestID, int *closestID)
    {
   
        // 检测最新帧是否和其他帧形成回环，所以后面一帧的id就是最后一个关键帧
        int loopKeyCur = copy_cloudKeyPoses3D->size() - 1;
        int loopKeyPre = -1;

        // check loop constraint added before
        // 检查一下较晚帧是否和别的形成了回环，如果有就算了
        auto it = loopIndexContainer.find(loopKeyCur);
        if (it != loopIndexContainer.end())
            return false;

        // find the closest history key frame
        std::vector<int> pointSearchIndLoop;
        std::vector<float> pointSearchSqDisLoop;
        // 把只包含关键帧位移信息的点云填充kdtree
        kdtreeHistoryKeyPoses->setInputCloud(copy_cloudKeyPoses3D);
        // 根据最后一个关键帧的平移信息，寻找离他一定距离内的其他关键帧
        kdtreeHistoryKeyPoses->radiusSearch(copy_cloudKeyPoses3D->back(), historyKeyframeSearchRadius, pointSearchIndLoop, pointSearchSqDisLoop, 0);
        // 遍历找到的候选关键帧
        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;
            }
        }
        // 如果没有找到回环或者回环找到自己身上去了，就认为是此次回环寻找失败
        if (loopKeyPre == -1 || loopKeyCur == loopKeyPre)
            return false;

        *latestID = loopKeyCur;
        *closestID = loopKeyPre;

        return true;
    }

检出回环之后开始计算两帧位姿变换

把回环帧的前后各25帧都取出来，共51帧，构建一个局部点云地图，然后用当前帧与这个局部点云地图进行匹配

构建回环帧的局部地图用loopFindNearKeyframes()

    void loopFindNearKeyframes(pcl::PointCloud<PointType>::Ptr& nearKeyframes, const int& key, const int& searchNum)
    {
   
        // extract near keyframes
        nearKeyframes->clear();
        int cloudSize = copy_cloudKeyPoses6D->size(