3D SLAM_LeGo-LOAM（2）_回环检测

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;
}

另外关于这一块里面的数据流差点儿把我弄晕了，这里贴个图理解一下。