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

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

 // 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_LeGo-LOAM(2)_回环检测_第2张图片

你可能感兴趣的:(3D,SLAM)