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问题求解