RGB-D SLAM——回环检测篇(四)

一.回环检测的流程

1.关键帧提取：把每一帧都拼到地图中是不明智的选择。因为帧与帧之间距离很近，导致地图需要频繁更新，浪费时间，所以当机器人的运动超过一个”关键帧”，将关键帧拼到地图里就行了。

2.回环的检测。回环的本质是识别曾经到过的地方。最简单的回环策略，是将新来的关键帧与之前所有的关键帧进行比较，不过这样会导致越往后，需要比较的帧就越多。所以，稍微快点的方法就是在过去的帧里面随机挑选一些，与之进行比较。

伪代码如下：
1.初始化关键帧序列：F，并将第一帧 $f_{0}$放入F中。
2.对于新来的一帧I，计算F中最后一帧与I的运动，并估计该运动的大小e。有以下集中可能性：
a.若e>error，说明运动太大，可能计算错误，丢弃该帧。
b.若没有匹配上(match)太少，说明该帧图像质量不高，丢弃。
c. 若e < Ekey,说明离前一帧很近，舍弃。
3.近距离回环：匹配I与F末尾m个关键帧。匹配成功时，在图里增加一条边。
4.随机回环：随机在F里取n个帧，与I进行匹配。若匹配上，在图里增加一条边。
5.若有新的数据，则回2，没有则进行优化与地图拼接。

主要代码如下：

int main(int argc, char** argv)
{
    vector<FRAME> keyframe;
    vector<FRAME> frame;
    vector<int> keyframe_count;
    keyframe_count.push_back(0);
    int frameid=0;
    for(int i=1;i<600;i++) //将所有的帧导入进来，存储在一个容器中
    {
        FRAME frame_single;
        string path_rgb("/home/hansry/Pose_part/data/rgb_png/");
        string path_depth("/home/hansry/Pose_part/data/depth_png/");
        string suffix(".png");
        stringstream str_rgb,str_depth;
        str_rgb<<path_rgb<<i<<suffix;
        str_depth<<path_depth<<i<<suffix;
        frame_single.rgb=imread(str_rgb.str());
        frame_single.depth=imread(str_depth.str(),-1);
        frame_single.frameid=framei