vins-fusion代码解读[四] 图像回环检测loop_fusion主体

SLAM新手,欢迎讨论。

这篇主要讲loop_fusion包的程序结构,loop_fusion主要作用:利用词袋模型进行图像的回环检测。在vinsmono中,该程序包处于pose_graph包内。vins_fusion与vins_mono一个差别在于,回环检测的点云数据在mono中有回调供给VIO进行非线性优化,而在fusion中,VIO估计完全独立于回环检测的结果。即回环检测的全局估计会受到VIO的影响,但是VIO不受全局估计的影响。

程序入口:loop_fusion的pose_graph_node.cpp

pose_graph_node

(1)定义了全局变量。最重要的是定义了全局姿态类:Posegraph posegraph。在定义了这个变量的过程中,由于posegraph的初始化,因此开启了一个持续不断的新线程:
optimize4DoF,即全局优化一个4自由度的过程。
该过程由于IMU的存在,使得位姿6自由度的问题一下子降了两维。并且线程每结束一次优化的过程,线程就会睡眠2秒钟,再继续。optimize4DoF优化的前提条件:optimize_buf不等于空。添加optimize_buf的内容只有两个地方,一个是addKeyFrame()函数,一个是loadKeyFrame()函数。
loadKeyFrame()函数:只要在LOAD_PREVIOUS_POSE_GRAPH有,即我们有原来的地图信息,加载的时候,才会发生。
addKeyFrame()函数:外部调用类pose_graph的第一个函数。即通过findConnection()函数判断是否有检测到回环,检测到后满足一定条件就把回环帧的序号添加optimize_buf内部。

main()函数

  • 读取配置文件,通过fsSetting进行相应的参数配置
    比较重要的是读入了vocabulary_file,即在support_files里面的brief_k10L6.bin。以及BRIEF_PATTERN_FILE。通过posegraph.loadVocabulary为posegraph的类成员 BriefDatabase db设置属性以及BriefVocabulary voc赋值。以及为BRIEF_PATTERN_FILE赋值。为后期keyframe的构建创造一个基础。
  • 根据LOAD_PREVIOUS_POSE_GRAPH,即是否要加载原有的地图信息,如果加载了之前的信息,则posegraph.loadPoseGraph,之前所有的关键帧的序号sequence都设置为0,base_sequence也是0。不过不加载之前的信息,base_sequence=1。
  • 订阅话题,发布话题
  • 开启一个新线程:process()
    程序主要的执行内容。
  • 开启一个控制台键盘控制线程:command()
    键盘控制输入,有提供两种选择,在控制台上写入:
    ‘s’:把当前Posegraph存储起来,并且把整个loop_fusion包的进程关掉。
    ‘n’:图像更新序列,new_sequence()

process()

  • image_buf,pose_buf,point_buf三个buff都不为空的时候,才进行运行,否则程序就休息5毫秒,继续监测。
    (1)image_buf,存放image_callback()图像回调函数的信息,接收相机话题的原始图片。并且当两张图片的时间戳相差1秒,或者说当图像的时间戳小于上一帧的时间戳,可以认为图像出现新序列,因此new_sequence()
    (2)point_buf,存放point_callback()关键帧的三维特征点信息的回调。并且在这个回调函数中,同时利用point_cloud标准数据结构发布特征点三维点云以供可视化操作。发布的话题为:point_cloud_loop_rect。注意,此时的点云信息不是单纯vio得到的点云信息,而是在pose_graph参考坐标系下,关键帧点云的位置。因为vio自己有一个参考坐标系,而pose_graph求解出来的位姿图也有自己的坐标系,这两个坐标系有一个变换关系,写在了pose_graph.r_drift,pose_graph.t_drift里面。
    (3)pose_buf,存放pose_callback()回调函数的信息。订阅的是关键帧的vio得到的关键帧的位姿信息。
    从上面订阅的信息我们可以看出,只有当相机有关键帧的时候(vins_estimator中有对应的规则判断该帧是不是关键帧),才会进行进一步处理,整个loop_fusion处理的信息不是原始图像,而是一帧帧关键帧
  • 接着程序开始找一帧keyframe 对应的image_buf,point_buf,pose_buf三者的数据。即point_buf和pose_buf这个存储的点云和位姿是对应(image_buf里面的)哪一帧关键帧。最终的结果
    (1)image_msg存放关键帧的图像原始信息
    (2)point_msg存放了该关键帧里面的3D点云信息,由VIO以及pose_graph.r_drift, pose_graph.t_drift 解算得到。
    (3)pose_msg存放了该关键帧的位姿信息。
  • 根据上面得到的三个信息,来构造关键帧!!用数据结构KeyFrame来表示,这里用了十个变量的KeyFrame构造函数来构造这个关键帧,十个变量的KeyFrame来构造函数默认没有brief_descriptor,因为我们VIO前端提取的并不需要特征点的描述子。在构造关键帧的时候,函数同时又增加了阈值大于20的FAST角点,来增加关键帧特征点的数量,同时,对这些特征点用相应的描述子来进行表述。构造出一个向量容器,用来存放所有特征点的描述子,即KeyFrame类成员vector brief_descriptor
    注意点:只有当两个关键帧之间的位移量(T-last_t)>阈值SKIP_DIS时候,才会构造新的关键帧。但是程序默认的SKIP_DIS为0。就是除非T和last_t完全相等,否则就会进入构造函数。
  • 构造好的keyframe,通过posegraph.addKeyFrame(keyframe,1),加入到全局姿态图当中去,第二个参数代表是需要回环检测detect_loop,这里直接默认设置需要。

PoseGraph类

先简单介绍下loop_fusion整个程序里面最重量级的类:PoseGraph类。整个程序都是维护这个main函数里面定义的全局变量PoseGraph posegraph来进行的。通过PoseGraph 的类函数,addKeyFrame来进行关键帧的添加。重要的成员变量:
listkeyframelist 最重要的类成员,整个回环检测的过程就是用来维护这个关键帧链表list,当检测到回环的时候,更新这个链表的数据,即关键帧的位姿。
BriefDatabase db 图像数据库信息,通过不断更新这个数据库,可以用来查询,即回环检测有没有回到之前曾经来过的地方。

addKeyFrame()

  • 检查该关键帧的序号是否有跳变,整个loop_fusion里面,每当出现new_sequence()时候,关键帧的sequence会自增1,为什么一直需要关心一个关键帧的sequence呢?因为这里一个关键帧的位姿的参考系是建立在某一个sequence下面的,不同的sequence对应了不同w_t_vio,w_r_vio,就是该sequence 和base_sequence之间坐标系的转换关系。
    -posegraph坐标系是以world frame作为坐标系的,world frame为base sequence(载入原来的图像数据库)或者(没有载入图像数据库的话)用sequence=1的序列作为坐标系。PoesGraph里面的两个类成员 w_t_vio,w_r_vio描述的就是当前序列的第一帧,与世界坐标系之间的转换关系。
  • 通过detectLoop(),查找到一个最合适得分最高的闭环候选帧(如果没找到,loop_index=-1),此时本质是通过bag of word中的db.query()来进行查找的。并且无论有没有查找到,都用db.add()把当前关键帧的描述子添加到图像的数据库db中去。
  • 若detectLoop()能够找到相似的一帧,此时,通过findConnection() 判断新旧两帧关联,匹配点大于25对,相对的偏航角位移小于30度并且相对位移少于20m,则返回true,其余情况均返回false。在true的条件下,更新参考系转化的变量。并且把当前帧的序号加入到optimize_buf中去。
  • 最终,把该帧keyframe 添加到keyframelist中去。从此,keyframelist又增添了一个关键帧元素。

optimize4DoF()

简单说下这个函数,optimize_buf一有东西,意味着该帧已经被检测出回环了,因此就开始优化,优化的对象就是keyframelist中每个关键帧的四个自由度,包括x,y,z,yaw。同样是ceres问题求解

你可能感兴趣的:(SLAM)