LeGO-LOAM回环检测

最新推荐文章于 2024-05-18 11:33:25 发布

AutoMan-USTC

最新推荐文章于 2024-05-18 11:33:25 发布

阅读量5k

点赞数 7

分类专栏：激光SLAM

本文链接：https://blog.youkuaiyun.com/Zlp19970106/article/details/108560797

版权

文章目录

1. 总体框架

mapOptimization.cpp中的主函数中，闭环检测是一个单独线程运行的。run()函数中运行的是地图优化位姿的算法。

int main(int argc, char** argv)
{
   
    ros::init(argc, argv, "lego_loam");
    
    ROS_INFO("\033[1;32m---->\033[0m Map Optimization Started.");
    
    mapOptimization MO;
    
    std::thread loopthread(&mapOptimization::loopClosureThread, &MO);    
    std::thread visualizeMapThread(&mapOptimization::visualizeGlobalMapThread, &MO);
    
    ros::Rate rate(200);    
    while (ros::ok())    
    {
           
    	ros::spinOnce();
    	MO.run();
    	rate.sleep();    
    }
    
    loopthread.join();    
    visualizeMapThread.join();
    return 0;
}

2. 回环检测

在LeGO-LOAM中使用了简单的基于轨迹位姿方法的回环检测，论文假设采用了地图优化方法的位姿优化结果偏移小，因此可以这样做。但是实际上无法应对大距离范围下的尺度漂移，存在着数据同源的问题。

2.1检测回环

回环检测运行频率是1Hz。调用的 performLoopClosure函数中首先调用了detectLoopClosure函数。

    void loopClosureThread()
    {
   
        if (loopClosureEnableFlag == false)            
            return;
        ros::Rate rate(1);        
        while (ros::ok()){
               
            rate.sleep();            
            performLoopClosure();        
        }    
     }

detectLoopClosure函数的流程：
1.使用KDtree寻找当前位姿的历史最近位姿：radiusSearch。当找到的位姿满足时间上与当前帧间隔大于30的条件就认为是合格的最近位置，并保留下来。
2.储存最新的这一帧点云，以作为闭环检测匹配计算中的目标值，并转换到世界坐标系下。
3.储存被找到的那一帧历史点云及其附近的几帧点云，并把他们转换到世界坐标系中。
4.如果订阅者数量不是0，则把数据发布出去。

    bool detectLoopClosure(){
   

        latestSurfKeyFrameCloud->clear();
        nearHistorySurfKeyFrameCloud->clear();
        nearHistorySurfKeyFrameCloudDS->clear();

        std::lock_guard<std::mutex> lock(mtx);
        // find the closest history key frame
        std::vector<int> pointSearchIndLoop;
        std::vector<float> pointSearchSqDisLoop;
        kdtreeHistoryKeyPoses->setInputCloud(cloudKeyPoses3D);
        kdtreeHistoryKeyPoses->radiusSearch(currentRobotPosPoint, historyKeyframeSearchRadius, pointSearchIndLoop, pointSearchSqDisLoop, 0);
        
        closestHistoryFrameID = -1;
        for (int i = 0; i < pointSearchIndLoop.size(); ++i<

最低0.47元/天解锁文章