vio笔记

1.从ros_vio.cc入 main()，

2.创建SLAM系统，ORB_SLAM2::System SLAM

  进入system（system.cc line211）:创建关键帧数据库、创建地图、创建用于可视化的Drawer.

   初始化Tracking线程：从设置文件中，装载相机参数，Load ORB参数。

   初始化Local Mapping线程并launch：获得局部窗口大小。

   初始化LoopClosing线程并launch

   初始化Viewer线程并launch   

设置线程之间的指针。

3.在 main（）中参数设置ConfigParam，再：

数据对齐：imu数据与读取的RGB数据

4.在在 main（）中开始Track.line173: SLAM.TrackMonoVI();输入图像数据和IMU数据及对应的时间戳，输出相机位姿。当跟踪失败时，输出为空。

先检查模式：如果当前处于定位模式，等待直到Local Mapping结束，将只跟踪设为1，定位模式（localization）设为0.

再进入tracking中的GrabImageMonoVI（）：

如果没有初始化或者目前还没有图片时，使用Frame(参数少一个)（Frame.cc line 416）,否则使用Frame(参数多一个)（Frame.cc line143）:

    其中，少一个参数的Frame,(scale level info ???) ORB提取，为加速特征匹配将关键点对齐网格。

    其中，多一个参数的Frame,与上面的函数相比，多了而一个copy imu 数据的部分。

 接着进入Track()：

     先判断是否有图，再获取map线程锁，map不被改变，根据map是否更新有不同的更新flag的操作. 如果状态为未初始化，则进行双目或RGBD对map的初始化：【】设置帧原始的pose,创建关键帧，将关键帧插入地图中，创建地图点并融进关键帧中等等。再更新FrameDrawer.

     如果系统状态为初始化后，则Track 帧。使用匀速模型或重定位（当跟踪失败时）来初始化相机位姿：当跟踪和局部建图同时开启时（这也是常用模式，除非单独使用‘只跟踪’模式）。局部见图可能改变了上一帧中的一些地图点，所以需要CheckReplacedInLastFrame()。当视觉-惯性初始化之后，重定位之后的20帧，开始只使用视觉跟踪-参考帧模型跟踪：计算词袋向量，和参考帧进行ORB匹配，找到足够的匹配后，使用pnp求解器Optimizer::PoseOptimization(&mCurrentFrame)，并丢弃异常值。如果V-I没有被初始化，视为纯V-slam,没有IMU数据，参考帧模型跟踪，没有IMU数据，匀速模型跟踪，若匀速模型失效，使用参考帧模型跟踪（Tracking.cc line950）。匀速模型：ORB匹配，根据上一帧的参考关键帧来更新上一帧的位姿，如果在定位模式下，创‘vo’点。

   如果状态不为ok的话，进行重定位Relocalization()：计算词袋向量。当跟丢时进行重定位。跟丢后，查询关键帧数据库中的关键帧候选DetectRelocalizationCandidates（）：搜索所有与当前帧共享一个词的关键帧，计算共享足够词数的关键帧，计算相似性得分，由共视来累加分数，返回所有分数高于0.75×最好分的关键帧。

如果查询没有候选关键帧，则重定位也失败。否则，和每个候选参考帧进行ORB匹配，找到足够的匹配后，使用pnp求解器。（其后还有可选择性地执行一些P4P的RANSAC迭代（此处跳过，tracking line2147），直到找到由足够inlier支持的相机位姿），如果有相机位姿和匹配的初始化估计，跟踪局部地图TrackLocalMap()：其中更新局部地图，搜索局部点，并优化位姿。也有TrackLocalMapWithIMU（）的选择（此处跳过，tracking line301）。

   如果状态OK的话，在重定位后，加帧来重计算IMU偏置。

    如果跟踪正常，检查是否插入了一个关键帧。

   保存帧的位姿信息，之后输出轨迹。

    Track()结束。

    返回位姿信息。GrabImageMonoVI()结束。

TrackMonoVI()结束。

等bLocalMapAcceptKF()返回0，局部建图结束，停止。

保存关键帧轨迹及位姿。

结束所有线程。