欢迎您访问程序员文章站本站旨在为大家提供分享程序员计算机编程知识!
您现在的位置是: 首页

3D SLAM_LeGo-LOAM(2)_回环检测

程序员文章站 2022-03-07 13:07:00
...

Lego-LOAM的回环检测策略比较简单,它同时对距离和时间作考量。
1.利用了PCL中基于半径的近邻搜索算法,以机器人当前位姿为搜索点,查找半径为7m范围内的若干个位姿;
2利用时间作为约束,如果历史位姿对应时间与当前位姿对应时间的时间差太小,说明是个小回环,意义不大,作者在程序里将时间差设置为大于30s。
下图表述了两种情况:
3D SLAM_LeGo-LOAM(2)_回环检测上图表示机器人在正常建图的过程,下图表示机器人回到原点,开始判断是否满足回环检测的条件。
对应代码也就是这一小段

 // find the closest history key frame
        std::vector<int> pointSearchIndLoop;
        std::vector<float> pointSearchSqDisLoop;
        kdtreeHistoryKeyPoses->setInputCloud(cloudKeyPoses3D);//构建位姿的kdtree
        //查询点为机器人的当前位姿、历史关键帧搜索半径、查找到的位姿索引,查找到的位姿与机器人位姿之间的距离平方和
        kdtreeHistoryKeyPoses->radiusSearch(currentRobotPosPoint, historyKeyframeSearchRadius, pointSearchIndLoop, pointSearchSqDisLoop, 0);
        closestHistoryFrameID = -1;
        for (int i = 0; i < pointSearchIndLoop.size(); ++i)//遍历查找到的若干个位姿
        {
            int id = pointSearchIndLoop[i];
            //cloudKeyPoses6D->points[id],这里得到的是当前查找到的某候选位姿在cloudKeyPoses6D中的坐标、时间、索引等值。具体可以看cloudKeyPoses6D的数据结构。
            if (abs(cloudKeyPoses6D->points[id].time - timeLaserOdometry) > 30.0)
            {
                closestHistoryFrameID = id;//这里记录下该位姿在pointSearchIndLoop中的索引,找到回环,跳出
                break;
            }
        }
        if (closestHistoryFrameID == -1)//在指定的半径内,没有找到对应的位姿,结束;
        {
            return false;
        }

拓展:关于PCL中关于radiussearch用法

// Neighbors within radius search
std::vector<int> pointIdxRadiusSearch;
std::vector<float> pointRadiusSquaredDistance;
float radius =7;

if(kdtree.radiusSearch (searchPoint, radius, pointIdxRadiusSearch, pointRadiusSquaredDistance) > 0 )
{
  for (size_t i = 0; i < pointIdxRadiusSearch.size (); ++i)
    std::cout << "    "  <<   cloud->points[ pointIdxRadiusSearch[i] ].x 
              << " " << cloud->points[ pointIdxRadiusSearch[i] ].y 
              << " " << cloud->points[ pointIdxRadiusSearch[i] ].z 
              << " (squared distance: " << pointRadiusSquaredDistance[i] << ")" << std::endl;
}
相关标签: 3D SLAM