LIO-SAM回环检测模块代码解析

前端里程计是一个不断递推相邻帧位姿的过程，其必定存在累计漂移。为了解决这一问题，回环检测并进行后端优化是一个重要的环节。下面以典型激光SLAM框架LIO-SAM为例进行解析，学习其进行回环检测的策略。

分配线程

首先，查看mapOptmization.cpp文件中的main函数，可以看到给回环检测处理分配一个线程。

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

查看loopClosureThread函数：

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

        ros::Rate rate(loopClosureFrequency);
        while (ros::ok())
        {
            rate.sleep();

            TicToc t_loopclosure;

            performLoopClosure();
            visualizeLoopClosure();

            if(t_loopclosure.toc() > 1)
            {
                std::cout << "t_loopclosure: " << t_loopclosure.toc() << std::endl;
                loopClosureTime += t_loopclosure.toc();
                frameCount2++;
            }
        }
    }

这里并没有太多复杂的处理逻辑，判断loopClosureEnableFlag是否开启了回环检测。若没有开启则直接返回不处理。开启了回环检测，则使用下述的loopClosureFrequency频率进行处理。

performLoopClosure函数

先进行判断关键帧的数量是否为空：

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

若为空则直接返回，不为空则复制一份关键帧的位置（3维）与位姿（6维）到另外一份变量中，防止冲突。

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

定义两个变量，存储当前关键帧索引与寻找到的最近邻关键帧索引。

        // find keys
        int loopKeyCur;
        int loopKeyPre;

查找最近邻关键帧索引，若两个函数均失败则说明没有检测到配对的关键帧，直接返回。

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

经过上述detectLoopClosureExternal与detectLoopClosureDistance函数处理之后，能够得到loopKeyCur当前帧索引，与其配对的之前最近邻关键帧loopKeyPre索引。

接着，定义cureKeyframeCloud与prevKeyframeCloud两个变量，分别存储当前关键帧点云数据、前述对应关键帧及前后historyKeyframeSearchNum帧点云组成的局部地图。

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

使用loopFindNearKeyframes函数加载对应关键帧，组成上述两个变量，并判断变量中的点云数量是否符合要求。

        {
            // 加载配对成功的当前关键帧，当前关键帧独立作为一个候选，仅加载当前帧点云
            loopFindNearKeyframes(cureKeyframeCloud, loopKeyCur, 0);
            // 加载配准成功的pre关键帧，加载前后historyKeyframeSearchNum的关键帧，组成局部地图
            loopFindNearKeyframes(prevKeyframeCloud, loopKeyPre, historyKeyframeSearchNum);
            if (cureKeyframeCloud->size() < 300 || prevKeyframeCloud->size() < 1000)
                return;
            if (pubHistoryKeyFrames.getNumSubscribers() != 0)
                publishCloud(pubHistoryKeyFrames, prevKeyframeCloud, timeLaserInfoStamp, odometryFrame);
        }

设置ICP配准算法的相关参数：

        // ICP Settings
        static pcl::IterativeClosestPoint<PointType, PointType> icp;
        icp.setMaxCorrespondenceDistance(historyKeyframeSearchRadius*2);
        icp.setMaximumIterations(100);
        icp.setTransformationEpsilon(1e-6);
        icp.setEuclideanFitnessEpsilon(1e-6);
        icp.setRANSACIterations(0);

配准处理cureKeyframeCloud与prevKeyframeCloud两个点云，并判断是否配准成功，若没有成功则直接返回：

        // Align clouds
        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;

若有接听，则发布变换后的点云：

// publish corrected cloud
        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);
        }

获取配准的位姿变换（当前关键帧与找到的回环关键帧之间）：

        // Get pose transformation
        float x, y, z, roll, pitch, yaw;
        Eigen::Affine3f correctionLidarFrame;
        correctionLidarFrame = icp.getFinalTransformation();

获取在没有进行回环改正之前，原始的位姿变换：（pclPointToAffine3f函数为把PCL中的“点”类型变换成位姿Affine3f类型）

// transform from world origin to wrong pose
        Eigen::Affine3f tWrong = pclPointToAffine3f(copy_cloudKeyPoses6D->points[loopKeyCur]);

获取矫正后的位姿：

// transform from world origin to corrected pose
        Eigen::Affine3f tCorrect = correctionLidarFrame * tWrong;// pre-multiplying -> successive rotation about a fixed frame

设置gtsam位姿节点和噪声模型：

 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::Vector Vector6(6);
        float noiseScore = icp.getFitnessScore();
        Vector6 << noiseScore, noiseScore, noiseScore, noiseScore, noiseScore, noiseScore;
        noiseModel::Diagonal::shared_ptr constraintNoise = noiseModel::Diagonal::Variances(Vector6);

在图中添加loop约束：

        // Add pose constraint
        mtx.lock();
        loopIndexQueue.push_back(make_pair(loopKeyCur, loopKeyPre));
        loopPoseQueue.push_back(poseFrom.between(poseTo));
        loopNoiseQueue.push_back(constraintNoise);
        mtx.unlock();

使用map记录两个关键帧成为回环：

        // add loop constriant
        loopIndexContainer[loopKeyCur] = loopKeyPre;

detectLoopClosureExternal函数

其中detectLoopClosureExternal函数中有注释说不再只用，所以就先忽略了。

// this function is not used yet, please ignore it

detectLoopClosureDistance函数

进入函数内，定义两个变量，分别存储loopKeyCur当前帧索引，与其配对的之前最近邻关键帧loopKeyPre索引。

    bool detectLoopClosureDistance(int *latestID, int *closestID)
    {
        int loopKeyCur = copy_cloudKeyPoses3D->size() - 1;
        int loopKeyPre = -1;

检查是否已经添加过当前帧在loopIndexContainer变量中：

        // check loop constraint added before
        auto it = loopIndexContainer.find(loopKeyCur);
        if (it != loopIndexContainer.end())
            return false;

使用pcl自带的kdtree->radiusSearch找到与当前帧最近邻的关键帧位置。

// find the closest history key frame
        std::vector<int> pointSearchIndLoop;
        std::vector<float> pointSearchSqDisLoop;
        kdtreeHistoryKeyPoses->setInputCloud(copy_cloudKeyPoses3D);
        kdtreeHistoryKeyPoses->radiusSearch(copy_cloudKeyPoses3D->back(), historyKeyframeSearchRadius, pointSearchIndLoop, pointSearchSqDisLoop, 0);

判断找到的最近邻关键帧与当前帧之间的时间差异是否大于设置的阈值historyKeyframeSearchTimeDiff，如果满足条件则认为找到了最近邻关键帧，赋值索引，反之索引仍为初值-1：

        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;
            }
        }

判断是否找到了最近邻关键帧索引，若没有找到则返回false。找到了则赋值loopKeyCur与loopKeyPre的值，返回true。

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

        *latestID = loopKeyCur;
        *closestID = loopKeyPre;

        return true;

loopFindNearKeyframes函数

函数作用为输入一个关键帧索引key和选取前后关键帧的数量searchNum，加载关键帧到nearKeyframes变量中：

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

加载选取的关键帧，到变量nearKeyframes中：

        for (int i = -searchNum; i <= searchNum; ++i)
        {
            int keyNear = key + i;
            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;

下采样，抽稀：

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

至此，我们基本了解了LIO-SAM框架中回环检测模块的代码，理解了其总体思路。