回环检测 Scan Contex 及 扩展（二）

前文介绍了基于Scan Context扩展的SC-LIO-SAM，本文将对FAST_LIO_SLAM进行介绍。

1.先总结SC-LIO-SAM相比原代码增加的内容：

• 保存关键帧的scan context描述子；
• LIO-SAM是基于时间和距离判断回环帧的，SC-LIO-SAM新增通过scan context寻找回环帧的方法(当前帧和回环帧点云局部点云ICP，计算位姿)，加入因子图。

2.为什么加回环：

参考作者在知乎[1]回复：“fast lio 定位是前端，类似 svo，因为我们认为学术工作应当专注在创新点上，而不在于功能是否多。回环检测和回环优化我们都有独立的研究发表了。如果只是简单的将回环加入 lio，这个偏工程应用，不太适合作为学术工作发表。”

3.FAST_LIO_SLAM：

在FAST-LIO2的基础上，添加SC-PGO模块，通过加入ScanContext全局描述子，进行回环修正。

SC-PGO (Loop detection and Pose-graph Optimization): Scan Context-based Loop detection and GTSAM-based Pose-graph optimization。

总结：FAST-LIO本身是一个非常棒的前端（里程计），FAST_LIO_SLAM增加了回环检测和后端优化，变为完整的SLAM。

4.Code:

代码很好的一点是 SC-PGO模块与FAST-LIO2解耦，SC-PGO 从 FAST-LIO2 节点接收里程计和激光点云的话题。

1.run FAST-LIO2 
roslaunch fast_lio mapping_ouster64_mulran.launch

2.run SC-PGO
roslaunch aloam_velodyne fastlio_ouster64.launch

根据launch文件，只关注laserPosegraphOptimization.cpp代码即可

4.1订阅：

• 订阅fast-lio 发布的"/cloud_registered_body"  ，这个点云是去畸变点云，注意是imu坐标系，不是lidar坐标系，回调函数把点云数据放到fullResBuf。
• 订阅fast-lio 发布的  "/Odometry"，位姿是imu到map的变换，回调函数把odom数据放到odometryBuf。

4.2线程：

1.process_pg 构建位姿图（Pose Graph）主线程

主要流程：

• 等待点云+里程计数据到达、对齐；
• 计算关键帧间的相对运动，判断当前帧是否为关键帧（超过一定距离/角度）；
• 是关键帧则保存点云、pose，向pose graph添加先验因子和里程计因子；
• 同步保存当前帧点云为PCD、时间戳等；

void process_pg()
{
    while(1)
    {
		while ( !odometryBuf.empty() && !fullResBuf.empty() )
        {

			mBuf.lock();   
            // (1) 如果 odometry 时间比 fullRes 时间早，则 odometry 过时，丢弃    
            while (!odometryBuf.empty() && odometryBuf.front()->header.stamp.toSec() < fullResBuf.front()->header.stamp.toSec())
                odometryBuf.pop();

            if (odometryBuf.empty())
            {
                mBuf.unlock();
                break;
            }

            // (2) 取当前帧时间戳
            timeLaserOdometry = odometryBuf.front()->header.stamp.toSec();
            timeLaser = fullResBuf.front()->header.stamp.toSec();
            
            // (3) 取出点云数据，转换为pcl格式
            pcl::PointCloud<PointType>::Ptr thisKeyFrame(new pcl::PointCloud<PointType>());
            pcl::fromROSMsg(*fullResBuf.front(), *thisKeyFrame);
            fullResBuf.pop();

            // (4) 取出当前odom位姿
            Pose6D pose_curr = getOdom(odometryBuf.front());
            odometryBuf.pop();

            double eps = 0.1; // find a gps topioc arrived within eps second 
            while (!gpsBuf.empty()) {
                auto thisGPS = gpsBuf.front();
                auto thisGPSTime = thisGPS->header.stamp.toSec();
                if( abs(thisGPSTime - timeLaserOdometry) < eps ) {
                    currGPS = thisGPS;
                    hasGPSforThisKF = true; 
                    break;
                } else {
                    hasGPSforThisKF = false;
                }
                gpsBuf.pop();
            }
            mBuf.unlock(); 

            odom_pose_prev = odom_pose_curr;
            odom_pose_curr = pose_curr;
            Pose6D dtf = diffTransformation(odom_pose_prev, odom_pose_curr); // 位姿增量
            double delta_translation = sqrt(dtf.x*dtf.x + dtf.y*dtf.y + dtf.z*dtf.z); // 平移增量
            translationAccumulated += delta_translation;
            rotaionAccumulated += (dtf.roll + dtf.pitch + dtf.yaw); // 累计欧拉角增量

            // (5) 判断是否为关键帧
            // 第一帧是关键帧
            if( translationAccumulated > keyframeMeterGap || rotaionAccumulated > keyframeRadGap ) {
                // 满足位移或转角要求，标记为关键帧
                isNowKeyFrame = true;
                translationAccumulated = 0.0; // reset 
                rotaionAccumulated = 0.0; // reset 
            } else {
                isNowKeyFrame = false;
            }

            if( ! isNowKeyFrame ) 
                continue; 

            if( !gpsOffsetInitialized ) {
                if(hasGPSforThisKF) { // if the very first frame 
                    gpsAltitudeInitOffset = currGPS->altitude;
                    gpsOffsetInitialized = true;
                } 
            }

            // (6) 记录关键帧点云、位姿、时间，构建scancontext描述子
            pcl::PointCloud<PointType>::Ptr thisKeyFrameDS(new pcl::PointCloud<PointType>());
            downSizeFilterScancontext.setInputCloud(thisKeyFrame);
            downSizeFilterScancontext.filter(*thisKeyFrameDS);

            mKF.lock(); 
            keyframeLaserClouds.push_back(thisKeyFrameDS);  // 保存关键帧点云
            keyframePoses.push_back(pose_curr);             // 保存关键帧位姿
            keyframePosesUpdated.push_back(pose_curr);      // 后端优化后的位姿，初值就是当前
            keyframeTimes.push_back(timeLaserOdometry);     // 时间戳

            // 当前关键帧生成sc 描述子
            scManager.makeAndSaveScancontextAndKeys(*thisKeyFrameDS);

            mKF.unlock(); 

            // (7) 添加因子
            const int prev_node_idx = keyframePoses.size() - 2; 
            const int curr_node_idx = keyframePoses.size() - 1; // becuase cpp starts with 0 (actually this index could be any number, but for simple implementation, we follow sequential indexing)
            if( ! gtSAMgraphMade /* prior node */) {
                const int init_node_idx = 0; 
                gtsam::Pose3 poseOrigin = Pose6DtoGTSAMPose3(keyframePoses.at(init_node_idx));
                
                mtxPosegraph.lock();
                {
                    // 添加先验因子
                    gtSAMgraph.add(gtsam::PriorFactor<gtsam::Pose3>(init_node_idx, poseOrigin, priorNoise));
                    initialEstimate.insert(init_node_idx, poseOrigin);
                }   
                mtxPosegraph.unlock();

                gtSAMgraphMade = true; 
                cout << "posegraph prior node " << init_node_idx << " added" << endl;
            } else /* consecutive node (and odom factor) after the prior added */ { // == keyframePoses.size() > 1 
                gtsam::Pose3 poseFrom = Pose6DtoGTSAMPose3(keyframePoses.at(prev_node_idx));
                gtsam::Pose3 poseTo = Pose6DtoGTSAMPose3(keyframePoses.at(curr_node_idx));

                mtxPosegraph.lock();
                {
                    // 添加odom因子
                    gtSAMgraph.add(gtsam::BetweenFactor<gtsam::Pose3>(prev_node_idx, curr_node_idx, poseFrom.between(poseTo), odomNoise));

                    // gps factor 
                    if(hasGPSforThisKF) {
                        double curr_altitude_offseted = currGPS->altitude - gpsAltitudeInitOffset;
                        mtxRecentPose.lock();
                        gtsam::Point3 gpsConstraint(recentOptimizedX, recentOptimizedY, curr_altitude_offseted); // in this example, only adjusting altitude (for x and y, very big noises are set) 
                        mtxRecentPose.unlock();
                        gtSAMgraph.add(gtsam::GPSFactor(curr_node_idx, gpsConstraint, robustGPSNoise));
                        cout << "GPS factor added at node " << curr_node_idx << endl;
                    }
                    initialEstimate.insert(curr_node_idx, poseTo);                
                }
                mtxPosegraph.unlock();

                if(curr_node_idx % 100 == 0)
                    cout << "posegraph odom node " << curr_node_idx << " added." << endl;
            }
            // if want to print the current graph, use gtSAMgraph.print("\nFactor Graph:\n");

            // 保存关键帧点云和时间
            std::string curr_node_idx_str = padZeros(curr_node_idx);
            pcl::io::savePCDFileBinary(pgScansDirectory + curr_node_idx_str + ".pcd", *thisKeyFrame); // scan 
            pgTimeSaveStream << timeLaser << std::endl; // path 
        }

        // ps. 
        // scan context detector is running in another thread (in constant Hz, e.g., 1 Hz)
        // pub path and point cloud in another thread

        // wait (must required for running the while loop)
        std::chrono::milliseconds dura(2);
        std::this_thread::sleep_for(dura);
    }
} // process_pg

2. process_lcd：回环检测线程 (Loop Closure Detection)

用Scan Context进行回环检测，周期性判断当前帧和历史帧是否构成回环。

主要流程：

• 周期性（如1Hz）调用performSCLoopClosure()，检测当前关键帧是否与历史帧回环；
• 检测到回环时，将回环帧对（索引）push到scLoopICPBuf。

void performSCLoopClosure(void)
{
    if( int(keyframePoses.size()) < scManager.NUM_EXCLUDE_RECENT) // do not try too early 
        return;

    auto detectResult = scManager.detectLoopClosureID(); // first: nn index, second: yaw diff 
    int SCclosestHistoryFrameID = detectResult.first;
    if( SCclosestHistoryFrameID != -1 ) { 
        const int prev_node_idx = SCclosestHistoryFrameID;    // 回环帧id
        const int curr_node_idx = keyframePoses.size() - 1;   // 当前帧id
        cout << "Loop detected! - between " << prev_node_idx << " and " << curr_node_idx << "" << endl;

        mBuf.lock();
        // 回环帧和当前帧id放到队列中,等待后续ICP线程处理
        scLoopICPBuf.push(std::pair<int, int>(prev_node_idx, curr_node_idx));
        // addding actual 6D constraints in the other thread, icp_calculation.
        mBuf.unlock();
    }
} // performSCLoopClosure
// 回环检测线程
void process_lcd(void)
{
    float loopClosureFrequency = 1.0; // can change 
    ros::Rate rate(loopClosureFrequency);
    while (ros::ok())
    {
        rate.sleep();
        performSCLoopClosure();
    }
} // process_lcd

3. process_icp：回环对齐线程 (ICP计算)

主要流程：

• 从scLoopICPBuf队列取出回环帧对，当前帧点云和回环帧前后25帧组成的局部点云进行ICP（点云都是map系），得到当前帧和回环帧的位姿变换；
• 向pose graph添加BetweenFactor（回环因子）。

void process_icp(void)
{
    while(1)
    {
		while ( !scLoopICPBuf.empty() )
        {
            if( scLoopICPBuf.size() > 30 ) {
                ROS_WARN("Too many loop clousre candidates to be ICPed is waiting ... Do process_lcd less frequently (adjust loopClosureFrequency)");
            }

            mBuf.lock(); 
            // 取出一个回环帧对（prev历史帧, curr当前帧）
            std::pair<int, int> loop_idx_pair = scLoopICPBuf.front();
            scLoopICPBuf.pop();
            mBuf.unlock(); 

            const int prev_node_idx = loop_idx_pair.first;
            const int curr_node_idx = loop_idx_pair.second;
            // 当前帧和回环帧的位姿变换
            auto relative_pose_optional = doICPVirtualRelative(prev_node_idx, curr_node_idx);
            if(relative_pose_optional) {
                gtsam::Pose3 relative_pose = relative_pose_optional.value();
                mtxPosegraph.lock();
                // 回环因子
                gtSAMgraph.add(gtsam::BetweenFactor<gtsam::Pose3>(prev_node_idx, curr_node_idx, relative_pose, robustLoopNoise));
                mtxPosegraph.unlock();
            } 
        }

        // wait (must required for running the while loop)
        std::chrono::milliseconds dura(2);
        std::this_thread::sleep_for(dura);
    }
} 

std::optional<gtsam::Pose3> doICPVirtualRelative( int _loop_kf_idx, int _curr_kf_idx )
{
    // parse pointclouds
    int historyKeyframeSearchNum = 25; // enough. ex. [-25, 25] covers submap length of 50x1 = 50m if every kf gap is 1m
    pcl::PointCloud<PointType>::Ptr cureKeyframeCloud(new pcl::PointCloud<PointType>());
    pcl::PointCloud<PointType>::Ptr targetKeyframeCloud(new pcl::PointCloud<PointType>());
    loopFindNearKeyframesCloud(cureKeyframeCloud, _curr_kf_idx, 0, _loop_kf_idx); // use same root of loop kf idx 
    loopFindNearKeyframesCloud(targetKeyframeCloud, _loop_kf_idx, historyKeyframeSearchNum, _loop_kf_idx); 

    // loop verification 
    sensor_msgs::PointCloud2 cureKeyframeCloudMsg;
    pcl::toROSMsg(*cureKeyframeCloud, cureKeyframeCloudMsg);
    cureKeyframeCloudMsg.header.frame_id = "camera_init";
    pubLoopScanLocal.publish(cureKeyframeCloudMsg);

    sensor_msgs::PointCloud2 targetKeyframeCloudMsg;
    pcl::toROSMsg(*targetKeyframeCloud, targetKeyframeCloudMsg);
    targetKeyframeCloudMsg.header.frame_id = "camera_init";
    pubLoopSubmapLocal.publish(targetKeyframeCloudMsg);

    // ICP Settings
    pcl::IterativeClosestPoint<PointType, PointType> icp;
    icp.setMaxCorrespondenceDistance(150); // giseop , use a value can cover 2*historyKeyframeSearchNum range in meter 
    icp.setMaximumIterations(100);
    icp.setTransformationEpsilon(1e-6);
    icp.setEuclideanFitnessEpsilon(1e-6);
    icp.setRANSACIterations(0);

    // Align pointclouds
    icp.setInputSource(cureKeyframeCloud);
    icp.setInputTarget(targetKeyframeCloud);
    pcl::PointCloud<PointType>::Ptr unused_result(new pcl::PointCloud<PointType>());
    icp.align(*unused_result);
 
    float loopFitnessScoreThreshold = 0.3; // user parameter but fixed low value is safe. 
    if (icp.hasConverged() == false || icp.getFitnessScore() > loopFitnessScoreThreshold) {
        std::cout << "[SC loop] ICP fitness test failed (" << icp.getFitnessScore() << " > " << loopFitnessScoreThreshold << "). Reject this SC loop." << std::endl;
        return std::nullopt;
    } else {
        std::cout << "[SC loop] ICP fitness test passed (" << icp.getFitnessScore() << " < " << loopFitnessScoreThreshold << "). Add this SC loop." << std::endl;
    }

    // Get pose transformation
    float x, y, z, roll, pitch, yaw;
    Eigen::Affine3f correctionLidarFrame;
    correctionLidarFrame = icp.getFinalTransformation();
    pcl::getTranslationAndEulerAngles (correctionLidarFrame, x, y, z, roll, pitch, yaw);
    gtsam::Pose3 poseFrom = Pose3(Rot3::RzRyRx(roll, pitch, yaw), Point3(x, y, z));
    gtsam::Pose3 poseTo = Pose3(Rot3::RzRyRx(0.0, 0.0, 0.0), Point3(0.0, 0.0, 0.0));

    return poseFrom.between(poseTo);
}

4. process_isam：优化线程（后端ISAM2）

作用：调用gtsam ISAM2优化器，对pose graph做全局优化，得到全局最优位姿。

void process_isam(void)
{
    float hz = 1; 
    ros::Rate rate(hz);
    while (ros::ok()) {
        rate.sleep();
        if( gtSAMgraphMade ) {    // 如果已经构建过因子图
            mtxPosegraph.lock();
            runISAM2opt();        // 调用一次isam2增量优化
            cout << "running isam2 optimization ..." << endl;
            mtxPosegraph.unlock();

            // 保存优化后（经过全局图优化）的轨迹结果到KITTI格式文本文件
            saveOptimizedVerticesKITTIformat(isamCurrentEstimate, pgKITTIformat);
            // 保存 原始里程计轨迹（未经过优化的位姿）到KITTI格式文本文件
            saveOdometryVerticesKITTIformat(odomKITTIformat); 
        }
    }
}

void runISAM2opt(void)
{
    // called when a variable added 
    isam->update(gtSAMgraph, initialEstimate);   // 用因子图和初值更新isam2内部图模型
    isam->update();                              // 增量优化内部状态
    
    gtSAMgraph.resize(0);
    initialEstimate.clear();

    isamCurrentEstimate = isam->calculateEstimate();  // 得到全局优化后位姿的最优解
    updatePoses();   // 更新位姿
}

5. process_viz_path：轨迹可视化线程

发布odom、轨迹以及tf，供RViz可视化。

6. process_viz_map：地图可视化线程

发布全局地图点云，供RViz可视化。

核心线程之间的关系

• 主线程（process_pg）负责基础数据收集、构建pose graph；

• 回环检测（process_lcd）不断检测历史回环，把候选回环送到ICP线程；

• ICP（process_icp）计算回环位姿约束并添加到因子图；

• ISAM2优化（process_isam）后端优化关键帧位姿；

• 可视化（viz_path/map）线程实时给RViz等提供轨迹、地图。

参考：

1.https://zhuanlan.zhihu.com/p/1004403765

2.https://github.com/gisbi-kim/FAST_LIO_SLAM?tab=readme-ov-file