一.回环检测的流程
1.关键帧提取:把每一帧都拼到地图中是不明智的选择。因为帧与帧之间距离很近,导致地图需要频繁更新,浪费时间,所以当机器人的运动超过一个”关键帧”,将关键帧拼到地图里就行了。
2.回环的检测。回环的本质是识别曾经到过的地方。最简单的回环策略,是将新来的关键帧与之前所有的关键帧进行比较,不过这样会导致越往后,需要比较的帧就越多。所以,稍微快点的方法就是在过去的帧里面随机挑选一些,与之进行比较。
伪代码如下:
1.初始化关键帧序列:F,并将第一帧 f0 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