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