多传感器融合SLAM --- 14.LIO-SAM地图优化代码分析 mapOptmization.cpp（闭环检测部分）

目录

0 流程图

1 闭环检测线程分析

1.1 闭环检测主进程

1.2 执行回环检测

1.2.1 检测有没有外部通知的回环信息detectLoopClosureExternal

1.2.2 检测内部回环 detectLoopClosureDistance

1.2.3 取出回环候选帧的点云  loopFindNearKeyframes

1.2.4 执行ICP计算位姿变换  ICP solver

1.3 发布可视化回环信息 /lio_sam/mapping/loop_closure_constraints

---

0 流程图

1 闭环检测线程分析

1.1 闭环检测主进程

        程序通过这行代码进入闭环检测主线程：

// 回环检测线程
std::thread loopthread(&mapOptimization::loopClosureThread, &MO);

        回环检测主要分为两个部分：检测回环、回环可视化。如果只是作为一个里程计则不需要回环。

    void loopClosureThread()
    {
        // 如果只作为一个独立的里程计 （如果不进行回环检测 则退出这个线程）
        if (loopClosureEnableFlag == false)
            return;

        // 设置回环检测的速率 1HZ
        ros::Rate rate(loopClosureFrequency);
        while (ros::ok())
        {
            // 执行完一段sleep一段时间 否则该线程占用CPU特别高
            rate.sleep();
            // 执行回环检测
            performLoopClosure();
            // 可视化回环检测
            visualizeLoopClosure();
        }
    }

1.2 执行回环检测

1.2.1 检测有没有外部通知的回环信息detectLoopClosureExternal

    bool detectLoopClosureExternal(int *latestID, int *closestID)
    {
        // this function is not used yet, please ignore it
        int loopKeyCur = -1;
        int loopKeyPre = -1;

        std::lock_guard<std::mutex> lock(mtxLoopInfo);
        // 外部的先验回环信息队列
        if (loopInfoVec.empty())
            return false;

        // 取出回环信息 这里把时间戳作为回环信息
        double loopTimeCur = loopInfoVec.front().data[0];
        double loopTimePre = loopInfoVec.front().data[1];
        loopInfoVec.pop_front();

        // 如过两个回环帧之间的时间差小于30s就算了
        if (abs(loopTimeCur - loopTimePre) < historyKeyframeSearchTimeDiff)
            return false;

        int cloudSize = copy_cloudKeyPoses6D->size();
        // 如果现存的关键帧小于2那也没有意义
        if (cloudSize < 2)
            return false;

        // latest key
        loopKeyCur = cloudSize - 1;

        // 遍历所有的关键帧 找到离后面一个时间戳更接近关键帧的ID
        for (int i = cloudSize - 1; i >= 0; --i)
        {
            if (copy_cloudKeyPoses6D->points[i].time >= loopTimeCur)
                loopKeyCur = round(copy_cloudKeyPoses6D->points[i].intensity);
            else
                break;
        }

        // previous key
        loopKeyPre = 0;
        // 找到距离前时间戳更近的关键帧ID
        for (int i = 0; i < cloudSize; ++i)
        {
            if (copy_cloudKeyPoses6D->points[i].time <= loopTimePre)
                loopKeyPre = round(copy_cloudKeyPoses6D->points[i].intensity);
            else
                break;
        }

        // 如果两个是同一关键帧 就没什么意思了
        if (loopKeyCur == loopKeyPre)
            return false;

        // 检查一下较晚的这个帧有没有被其他时候检测到了回环约束 如果已经和别的帧形成了回环约束
        auto it = loopIndexContainer.find(loopKeyCur);
        if (it != loopIndexContainer.end())
            return false;

        // 两个帧的索引输出
        *latestID = loopKeyCur;
        *closestID = loopKeyPre;

        return true;
    }

        检测是否有外部通知回环detectLoopClosureExternal步骤如下：

1.检测外部输入的回环”先验“队列loopInfoVec是否为空（其有两个时间戳信息）

2.检测时间戳是否小于30s 或者 现存的关键帧是否小于2

3.我们遍历先有的所有的关键帧，找出最接近先验队列的时间戳 loopKeyCur、loopKeyPre

4.检验时间戳是否对应同一帧：检查一下较晚的这个帧有没有被其他时候检测到了回环约束（loopIndexContainer中检查） 如果已经和别的帧形成了回环约束则返回false

5.否则形成回环 返回true 。回环信息赋值给latestID、closestID

1.2.2 检测内部回环 detectLoopClosureDistance

    bool detectLoopClosureDistance(int *latestID, int *closestID)
    {
        // 检测最新帧是否和其他帧形成回环 所以后面一帧的ID是最后一个关键帧
        // copy_cloudKeyPoses3D 拷贝的 cloudKeyPoses3D
        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;
        // 把只包含关键帧位姿XYZ信息的点云填充kdtree
        kdtreeHistoryKeyPoses->setInputCloud(copy_cloudKeyPoses3D);
        // 根据最后一个关键帧的平移信息 寻找距离它一定距离内的其他关键帧 historyKeyframeSearchRadius = 15
        kdtreeHistoryKeyPoses->radiusSearch(copy_cloudKeyPoses3D->back(), historyKeyframeSearchRadius, pointSearchIndLoop, pointSearchSqDisLoop, 0);

        // 遍历当前候选帧
        for (int i = 0; i < (int)pointSearchIndLoop.size(); ++i)
        {
            int id = pointSearchIndLoop[i];
            // 必须满足时间上超过一定阈值 才认为是一个有效的回环  30s
            // timeLaserInfoCur是当前点云帧的时间戳
            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;
    }

        检测是否有正常回环detectLoopClosureDistance：

1.根据距离寻找回环关键帧

2.判断是否满足时间要求

3.是否已经形成回环loopIndexContainer.find(loopKeyCur)loopKeyCur是当前帧的ID

3.当前帧信息赋值latestID，回环帧赋值loopKeyPre

1.2.3 取出回环候选帧的点云  loopFindNearKeyframes

        找出当前关键帧key的前后searchNum的关键帧 将其角点和面点全部取出并做下采样后续进行ICP工作。

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

        // 关键帧数量
        int cloudSize = copy_cloudKeyPoses6D->size();
        // searchNum为搜索范围
        for (int i = -searchNum; i <= searchNum; ++i)
        {
            // 找到这个idx
            int keyNear = key + i;
            if (keyNear < 0 || keyNear >= cloudSize )
                continue;
            // 否则把对应角点和面点的点云转换到世界坐标系下面去
            // cornerCloudKeyFrames 在saveKeyFramesAndFactor中赋值 是vector类型 存储着所有关键帧的角点信息
            *nearKeyframes += *transformPointCloud(cornerCloudKeyFrames[keyNear], &copy_cloudKeyPoses6D->points[keyNear]);
            *nearKeyframes += *tr ansformPointCloud(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;
    }

        取出的点云存放在nearKeyframes中。

        // extract cloud
        // 检测出回环之后开始计算两帧的位姿变换
        pcl::PointCloud<PointType>::Ptr cureKeyframeCloud(new pcl::PointCloud<PointType>());
        pcl::PointCloud<PointType>::Ptr prevKeyframeCloud(new pcl::PointCloud<PointType>());
        {
            // 当前帧就把自己取了出来
            loopFindNearKeyframes(cureKeyframeCloud, loopKeyCur, 0);
            // 稍早一点的就把自己和周围一些点云取出来 也就是构成一个帧到局部地图的一个匹配信息
            loopFindNearKeyframes(prevKeyframeCloud, loopKeyPre, historyKeyframeSearchNum);
            // 如果点云数目太少就算了
            if (cureKeyframeCloud->size() < 300 || prevKeyframeCloud->size() < 1000)
                return;

             lio_sam/mapping/icp_loop_closure_history_cloud  prevKeyframeCloud存放的是当前帧回环候选帧及周围的点云
            if (pubHistoryKeyFrames.getNumSubscribers() != 0)
                // 把局部地图发布出去的点云供rviz可视化使用
                publishCloud(pubHistoryKeyFrames, prevKeyframeCloud, timeLaserInfoStamp, odometryFrame);

        检查点是否足够进行ICP计算，如果回环候选点prevKeyframeCloud足够的话将其发布 ：lio_sam/mapping/icp_loop_closure_history_cloud

1.2.4 执行ICP计算位姿变换  ICP solver

        这段代码使用了点云库（Point Cloud Library，PCL）中的IterativeClosestPoint类来执行点云配准。下面是对各个函数的解释：
        setMaxCorrespondenceDistance函数用于设置最大对应距离，即两个点之间允许的最大距离。在这里，距离设置为historyKeyframeSearchRadius的两倍。
        setMaximumIterations函数设置迭代的最大次数，这里设置为100次。
        setTransformationEpsilon函数设置变换的收敛阈值。当两次变换之间的差异小于该阈值时，认为算法已经收敛。这里的阈值设置为1e-6。
        setEuclideanFitnessEpsilon函数设置欧式适配度的收敛阈值。欧式适配度是指点云配准前后的均方根误差（Root Mean Square Error，RMSE），当误差小于该阈值时，认为算法已经收敛。这里的阈值设置为1e-6。
        setRANSACIterations函数设置RANSAC算法的迭代次数。RANSAC算法用于估计初始变换矩阵，将其用作点云配准的初始猜测。在这里，迭代次数设置为0，即禁用RANSAC算法。
        通过这些设置，代码为ICP配准算法提供了一些参数，以控制算法的收敛性和准确性。
        创建一个名为icp的静态对象，类型为pcl::IterativeClosestPoint<PointType, PointType>

        // 这段代码使用了点云库（Point Cloud Library，PCL）中的IterativeClosestPoint类来执行点云配准。下面是对各个函数的解释：
        // setMaxCorrespondenceDistance函数用于设置最大对应距离，即两个点之间允许的最大距离。在这里，距离设置为historyKeyframeSearchRadius的两倍。
        // setMaximumIterations函数设置迭代的最大次数，这里设置为100次。
        // setTransformationEpsilon函数设置变换的收敛阈值。当两次变换之间的差异小于该阈值时，认为算法已经收敛。这里的阈值设置为1e-6。
        // setEuclideanFitnessEpsilon函数设置欧式适配度的收敛阈值。欧式适配度是指点云配准前后的均方根误差（Root Mean Square Error，RMSE），当误差小于该阈值时，认为算法已经收敛。这里的阈值设置为1e-6。
        // setRANSACIterations函数设置RANSAC算法的迭代次数。RANSAC算法用于估计初始变换矩阵，将其用作点云配准的初始猜测。在这里，迭代次数设置为0，即禁用RANSAC算法。
        // 通过这些设置，代码为ICP配准算法提供了一些参数，以控制算法的收敛性和准确性。
        // 创建一个名为icp的静态对象，类型为pcl::IterativeClosestPoint<PointType, PointType>
        static pcl::IterativeClosestPoint<PointType, PointType> icp;

        // 设置最大对应距离为historyKeyframeSearchRadius的两倍
        icp.setMaxCorrespondenceDistance(historyKeyframeSearchRadius*2);

        // 设置最大迭代次数为100
        icp.setMaximumIterations(100);

        // 设置变换的收敛阈值为1e-6
        icp.setTransformationEpsilon(1e-6);

        // 设置欧式适配度的收敛阈值为1e-6
        icp.setEuclideanFitnessEpsilon(1e-6);

        // 设置RANSAC算法的迭代次数为0（禁用RANSAC算法）
        icp.setRANSACIterations(0);

        // 将cureKeyframeCloud设置为ICP算法的源点云输入
        icp.setInputSource(cureKeyframeCloud);

        // 将prevKeyframeCloud设置为ICP算法的目标点云输入
        icp.setInputTarget(prevKeyframeCloud);

        // 创建一个名为unused_result的指针，指向一个PointCloud对象
        pcl::PointCloud<PointType>::Ptr unused_result(new pcl::PointCloud<PointType>());

        // 调用icp.align函数，将ICP算法应用于输入的源点云和目标点云，并将结果存储在unused_result中
        icp.align(*unused_result);

        // 检查ICP是否收敛且得分是否满足要求
        if (icp.hasConverged() == false || icp.getFitnessScore() > historyKeyframeFitnessScore)
            return;

        // 把修正后的当前点云发布出去
        // lio_sam/mapping/icp_loop_closure_corrected_cloud 发布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);
        }

        // Get pose transformation
        float x, y, z, roll, pitch, yaw;
        Eigen::Affine3f correctionLidarFrame;

        // 获得两个点云的变换矩阵结果
        // cureKeyframeCloud 向 prevKeyframeCloud 变换的矩阵
        correctionLidarFrame = icp.getFinalTransformation();

        // transform from world origin to wrong pose
        // 找到当前帧的位姿结果
        Eigen::Affine3f tWrong = pclPointToAffine3f(copy_cloudKeyPoses6D->points[loopKeyCur]);
        // transform from world origin to corrected pose
        // 将ICP的结果补偿回去 就是稍晚帧的更为准确的位姿结果
        Eigen::Affine3f tCorrect = correctionLidarFrame * tWrong;// pre-multiplying -> successive rotation about a fixed frame

        // 将当前帧的位姿转换成平移 + 欧拉角
        pcl::getTranslationAndEulerAngles (tCorrect, x, y, z, roll, pitch, yaw);

        // from是修正后的当前帧的点云位姿
        gtsam::Pose3 poseFrom = Pose3(Rot3::RzRyRx(roll, pitch, yaw), Point3(x, y, z));

        // to是修正前的稍早的点云位姿 不做修正
        gtsam::Pose3 poseTo = pclPointTogtsamPose3(copy_cloudKeyPoses6D->points[loopKeyPre]);
        gtsam::Vector Vector6(6);

        // 使用ICP的得分作为噪声的约束项
        float noiseScore = icp.getFitnessScore();
        Vector6 << noiseScore, noiseScore, noiseScore, noiseScore, noiseScore, noiseScore;

        // 转换成gtsam的格式
        noiseModel::Diagonal::shared_ptr constraintNoise = noiseModel::Diagonal::Variances(Vector6);

        // Add pose constraint
        // 将两帧索引 两帧相对位姿和噪声作为回环的约束送入队列
        mtx.lock();

        // 回环帧的队列 回环帧 -- 匹配的回环帧
        loopIndexQueue.push_back(make_pair(loopKeyCur, loopKeyPre));
        // poseFrom 变换到 poseTo 的相对位姿变换存储在队列中 供因子图优化使用
        loopPoseQueue.push_back(poseFrom.between(poseTo));
        // 存储协方差矩阵
        loopNoiseQueue.push_back(constraintNoise);
        mtx.unlock();

        // add loop constriant
        // 保存已存在的约束对
        loopIndexContainer[loopKeyCur] = loopKeyPre;

        执行ICP运算：

1. 发布当前帧的点云矫正后的点云 closed_cloud ： lio_sam/mapping/icp_loop_closure_corrected_cloud

2.添加回环队列容器 loopIndexQueue ： 存储的是 make_pair(loopKeyCur, loopKeyPre)型变量 为当前帧-匹配回环帧 在因子图优化中使用

3.添加回环队列容器 loopPoseQueue： 存储当前帧 -> 回环帧的相对位姿变换 供因子图优化使用

4.存储协方差矩阵 loopNoiseQueue ： 供因子图优化时使用

5.保存已经存在的约束对 loopIndexContainer ： 防止同一帧反复寻找回环

1.3 发布可视化回环信息 /lio_sam/mapping/loop_closure_constraints

    void visualizeLoopClosure()
    {
        // 如果没有回环约束就算了
        if (loopIndexContainer.empty())
            return;


        visualization_msgs::MarkerArray markerArray;
        // loop nodes
        // 回环约束的两帧作为node添加
        // 先定义下node的信息
        visualization_msgs::Marker markerNode;
        markerNode.header.frame_id = odometryFrame;
        markerNode.header.stamp = timeLaserInfoStamp;
        markerNode.action = visualization_msgs::Marker::ADD;
        markerNode.type = visualization_msgs::Marker::SPHERE_LIST;
        markerNode.ns = "loop_nodes";
        markerNode.id = 0;
        markerNode.pose.orientation.w = 1;
        markerNode.scale.x = 0.3; markerNode.scale.y = 0.3; markerNode.scale.z = 0.3; 
        markerNode.color.r = 0; markerNode.color.g = 0.8; markerNode.color.b = 1;
        markerNode.color.a = 1;
        // loop edges
        // 两帧之间约束作为edge添加
        visualization_msgs::Marker markerEdge;
        markerEdge.header.frame_id = odometryFrame;
        markerEdge.header.stamp = timeLaserInfoStamp;
        markerEdge.action = visualization_msgs::Marker::ADD;
        markerEdge.type = visualization_msgs::Marker::LINE_LIST;
        markerEdge.ns = "loop_edges";
        markerEdge.id = 1;
        markerEdge.pose.orientation.w = 1;
        markerEdge.scale.x = 0.1;
        markerEdge.color.r = 0.9; markerEdge.color.g = 0.9; markerEdge.color.b = 0;
        markerEdge.color.a = 1;

        // 遍历所有回环约束
        for (auto it = loopIndexContainer.begin(); it != loopIndexContainer.end(); ++it)
        {
            int key_cur = it->first;
            int key_pre = it->second;
            geometry_msgs::Point p;
            p.x = copy_cloudKeyPoses6D->points[key_cur].x;
            p.y = copy_cloudKeyPoses6D->points[key_cur].y;
            p.z = copy_cloudKeyPoses6D->points[key_cur].z;
            // 添加node和edge
            markerNode.points.push_back(p);
            markerEdge.points.push_back(p);
            p.x = copy_cloudKeyPoses6D->points[key_pre].x;
            p.y = copy_cloudKeyPoses6D->points[key_pre].y;
            p.z = copy_cloudKeyPoses6D->points[key_pre].z;
            markerNode.points.push_back(p);
            markerEdge.points.push_back(p);
        }

        markerArray.markers.push_back(markerNode);
        markerArray.markers.push_back(markerEdge);
        // /lio_sam/mapping/loop_closure_constraints  回环约束信息
        pubLoopConstraintEdge.publish(markerArray);
    }

        发布回环约束信息 /lio_sam/mapping/loop_closure_constraints。