【SLAM学习】ORB-SLAM2-大体框架（OpenVSLAM）

最近更新日期：2022.12.03（已完结，可能会再理解扩充）

---

---

OpenVSLAM

OpenVSLAM是包含ORB-SLAM2内容并重新改写扩充后的仓库，因此和ORB-SLAM2源代码一些代码构造和变量名称不完全一样。

虽然已经OpenVSLAM仓库已经停用了，但因为原来项目所用，因此继续延续以这个复习。

（以下为之前的学习笔记，重新捡起，逐步更新补充中。。。）

大体的学习步骤就是根据b站的ORB-SLAM2的代码讲解视频进行跟进
看的过程中去对应OpenVSLAM里的相应实现

---

一、大体框架

分为三大线程

1. tracking，所有帧都会参与（其中包含一个决定哪些frame是keyframe的步骤。随机性：决定是否是关键帧的因素有很多，不能仅依据当前帧的特点，可能还和当前系统状态等因素有关，所以要用多线程）
2. local mapping，只有关键帧会参与
3. loop closing 回环检测

其中tracking就是简单的对于每一个读如的帧都进行处理。但后两步都是一个死循环，从上一个步骤接收到一个就处理一个。

局部建图和回环检测线程，都会开辟一个单独的thread进行运行。
三个线程之间是通过关键帧队列来进行通信。

对于单目摄像头来说，就是传入一张图片
第一步tracking，要所有帧都参与的，因为每过一帧，都要估计相应相机的位姿
但是对于后边两个都是比较重的任务，所以只有keyframe才参与
（判断当前frame是否是keyframe的函数在tracking里）

所以keyframe是三者的桥梁

二、 锁🔒

三个线程，所以需要锁进行限制读写（Mutex变量）
大括号结束就意味着释放锁。

    // dequeue
    {
        std::lock_guard<std::mutex> lock(mtx_keyfrm_queue_);
        // dequeue -> cur_keyfrm_
        cur_keyfrm_ = keyfrms_queue_.front();
        keyfrms_queue_.pop_front();
    }

三、System类

构造函数：

1. 创建变量 2. 创建三大线程 3. 进程间通信
   三大进程间的通信是通过keyframe关键帧的队列进行通信

tracking线程本身就是system类的主线程
mapping和global optimization是其里边创建的子线程
但逻辑上三者是并行的

四. ORB Extractor

在OpenVSLAM中是在data/Frame.cc类里，调用ORBExtractor类

ORBExtractor只有一个（tracking_module.h中可以看到），成百上千个的Frame共享同一个ORBExtractor
所以说下一帧来的时候会把原图像的金字塔信息覆盖掉，但提取到的特征点信息不会覆盖（保存在了frame里）。
所以ORB-SLAM对于每个图像只保留固定个数的特征点，是稀疏重建。

4.0 构造函数：成员变量 & 构建图像金字塔

ORB：oriented BRIEAF
FAST特征点和ORB描述子本身不具有尺度信息

4.0.1 构造函数（三件小事）

1. 初始化图像金字塔相关变量（层级等）
2. 初始化计算描述子用的pattern（特征子的身份证，写死的）
3. 近似圆形边界umax（即ORB中的ORiented，用于调正方向）

其中金字塔中层级越高图片分辨率越低，ORB特征点越大，相当于离相机越近
参数在yaml配置文件中设定。

另外，umax用于后续计算特征子主方向。需要半径为16的圆。

4.0.2 构造函数（一件大事）

通过构建图像金字塔来得到特征点尺度信息将输入图片逐级缩放得到图像金字塔

具体来说：把图像缩放到对应每个金字塔层级，并补19的边（FAST特征点需要半径为3，ORB描述子需要半径为16）

4.1 特征点搜索和筛选

将30*30的像素图像划分为一个网格。遍历每一个网格搜索：

1. 首先用高相应值搜索，找到了就存储；找不到就用低相应值；再找不到就抛弃。
2. 然后利用八叉树搜索（相当于MNS），对特征点密集的地方进行筛选
3. 最后计算特征点主方向（就是求朝着特征点附近r=19的圆的质心方向的角度，是为了下一步计算ORB描述子）
   

4.2 ORB描述子

对于BRIEF描述子，是在r=16的圆内计算256个点对，每个点对1位，共256对。

对于Oriented BRIEF来说，还要在取点对之前先将特征点周围r=19像素旋转到主方向上，再做BRIEF的操作（在r=16上取256对的点对）。

五、 Landmarks(MapPoint)

ORB-SLAM中的MapPoint相当于OpenVSLAM代码里的landmarks
ORB-SLAM里的Map信息就是特指（特征点、KeyFrame）两个数组

5.1 Landmark(MapPoint) 和 KeyFrame关系

landmark.cc中std::map<KeyFrame*,size_t> observations_保存了当前MapPoint在各个KeyFrame中的索引。
KeyFrame.cc中的std::vector<landmark*> landmarks_记录当前KeyFrame观测到所有的地图点。
observations_中的索引就是在对应KeyFrame中改地图点的索引

num_observations_ 代表有多少相机观测到这个地图点了，如果是单目相机，就多一个相机就+1。（后续mapping过程来判断该MapPoint的质量）

5.2 平均观测距离 & 平均观测方向

最小/最大观测距离
超过这个范围就是相机无法观测到的特征点了

当前观测距离已知：（假设层之间的scale都是1.2倍）
1.要是得到最大观测距离，就可以知道当前是第几个level
2.要是得到第几个level，就可以得到最大观测距离

landmark.cc中的update_normal_and_depth()函数就是用来更新当前landmark的平均观测方向和距离。
！！此函数的调用时机：landmark本身 / KeyFrame对该landmark观测发生变化

平均方向是由所有观测到此landmark的keyframe（observations_）平均得到的，平均距离是参考关键帧（ref_keyfrm）得到的。

那参考关键帧是哪里来的？
其一般是和该地图点关系最紧密，视角最好的

第一条在landmark.h中有说明：

//! get reference keyframe, a keyframe at the creation of a given 3D point
    keyframe* get_ref_keyframe() const;

第二条在erase_observation函数中

if (ref_keyfrm_ == keyfrm) { // 如果对于参考关键帧的观测被删除，取第一个观测到该地图点的keyframe为refKeyFrame
	ref_keyfrm_ = observations_.begin()->first;
}

5.3 landmark的描述子（3D）****

很多keyframe（图中红框）都观测到了这个landmark（图中红圈），每个landmark在这些keyframe中都有对应的特征点（图中红叉）。

选择哪一个作为此landmark的descriptor？
选最有代表性的。
具体来说就是找出特征点descriptor距离其他特征点descriptor距离之和最小。（256位二进制，可以求距离）

因此，一旦对该landmark的观测序列变了的话，就要重新计算landmark描述子

作用：PnP问题
解决3D特征点到2D点投影的时候，怎么知道哪个特征点和这个地图点对应？
就是看此keyframe中哪个特征点的descriptor和landmark的descriptor最像。

5.4 landmark的删除和替换

5.4.1 删除

亮点：先不物理删除，先标记一下，即逻辑删除
因为landmark复杂，删除耗时。
如果要停下，浪费时间；如果不停，别的线程还在用。

那就先将其标记为will_be_erased（ORB-SLAM: bad）
具体标的函数是prepare_for_erasing（ORB-SLAM: set_bad_flag）

其他线程用的话都要看看标记，这样具体删的过程都不用加🔒了，系统得空了再弄。

对于判断bad点的条件，在local mapping里有个函数负责

5.4.2 替换

与删除操作的思想类似，先标记，后操作
直接替换的话也是耗时，而且可能会在替换过程中产生读写冲突。
因此设置一个replaced_指针，默认为NULL。
如果当前被替换，则直接指向新地图点（不用来找我了，去找指针指向的）。
然后剩下的步骤慢慢做就行了

5.4.3 生命周期

对理解很有帮助：
所有非临时landmark都是keyFrame建立的
非keyFrame建立的landmark都很快被删除，只是增强帧间的匹配。

其中创建landmark的2、3条，只有双目/RGBD相机会创建地图点。

六、 Frame

Frame是人民群众，KeyFrame是人大代表，KeyFrame从Frame中来。
Frame只能解决当前帧pose等问题，但KeyFrame才能参与解决整体的建图等问题。

6.1 Frame成员变量 & 函数

6.1.1 相机内参相关的

因为所有的相机的内参都是固定的，因此所有Frame中关于相机内参的变量都是static（只有一份）。通过tracking线程由yaml文件读入，只有第一次构建Frame来构建内参参数。
OpenVSLAM中是在camera/base.h文件中读入，然后frame.h再创建base对象：

//! camera model
    camera::base* camera_ = nullptr;

之前写的：
ORB_SLAM2代码里把相机好多参数都作为Frame对象，而且有一些还不是static
但OpenVSLAM里就都封装在了camera对象里，传给frame对象的构造函数

6.1.2 特征点提取相关的

在Frame.h中针对双目相机的变量，只需要记录左目特征点在右目中匹配特征点的横坐标信息stereo_x_right_，原因是纵坐标与左目的是相同的，横坐标由于有视差所以有差别

6.2 对双目 / 深度相机特征点的预处理

双目为例：通过两个相机进行立体匹配，得到每个特征点的深度，即双目特征点
如果过深，误差过大，则变为单目特征点（用左相机）

深度的threshold在yaml文件中有参数⬇️
并且对于深度相机，利用左目坐标和深度信息，构建虚拟的右目坐标。这样后续深度和双目相机可以同样形式处理。

双目视差公式
b * f是预先算好的

双目相机特征点的匹配过程：

畸变矫正
畸变矫正只发生在单目相机和RGBD相机的左目上

双目相机预先进行了，所以矫正参数为0。
矫正之后使得两个相机的极线处于一个平面，所以在后来匹配的时候才可以认为其纵坐标一样

网格加速
将特征点分配到网格上，加速匹配。

6.3 作用和生命周期

OpenVSLAM代码中，tracking_module就是track线程
每来一帧，就调用track_monocular_image创建一个frame对象

与Frame相关的只有last_frm_和curr_frm_两个变量，tracking完curr就变味last

七、KeyFrame

Covisibility Graph和Spanning Tree都是描述关键帧的共视关系，前者更加稠密
共视关系：两个KeyFrame间有共同观测到的landmark
权重：共同观测到的landmark越多，关系越紧密

7.1 共视图

共视图关系（绿色）：过于稠密，肯定不会优化他。

ORB_SLAM2代码里：对于共视图是在KeyFrame里存储的。
OpenVSLAM代码里是单独开辟出graph_node类来存储共视图相关变量和函数。

7.1.1 成员变量

用于存储与其共视的关键帧和权重：

//! all connected keyframes and their weights
    std::map<keyframe*, unsigned int> connected_keyfrms_and_weights_;

对于connected_keyfrms_and_weights_中的keys和values按照权重降序存储

	//! covisibility keyframe in descending order of weights
    std::vector<keyframe*> ordered_covisibilities_;
    //! weights in descending order
    std::vector<unsigned int> ordered_weights_;

7.1.2 Function

update_connections()是通过对地图点的观测，重新构造Covisibility Graph
其实是rebuild，把之前的变量都清空了，是可以考虑优化的位置

1. 通过遍历当前帧地图点获取其与其它关键帧的共视程度, 存入变量keyfrm_weights
2. 将共视权重超过weight_thr_=15的存入weight_covisibility_pairs
3. 按照权重降序排列，更新到ordered_covisibilities_和ordered_weights_
4. 对于第一次加入生成树的KeyFrame，取共视程度最高的KeyFrame作为父KeyFrame（待后面说了生成树再理解）

update_connections()调用时机：
关键帧与地图点间的连接关系发生变化：即KeyFrame创建和地图点重新匹配关键帧特征点

add_connections()和erase_connection都是在内部调用的，但设为了public，不合理

7.2 生成树

生成树是Covisibility Graph的简化，对其可以进行BA优化，运算量小。

7.2.1 成员变量

父node只有一个，子node可以有很多。spanning_parent_is_not_set_代表该关键帧是否还未加入到生成树中。

	//! parent of spanning tree
    keyframe* spanning_parent_ = nullptr;
    //! children of spanning tree
    std::set<keyframe*> spanning_children_;
    //! flag which indicates spanning tree is not set yet or not
    bool spanning_parent_is_not_set_;

7.2.2 Function

在update_connections()中，一旦关键帧被新创建，第一次调用这个函数，则spanning_parent_is_not_set_就是True，那么第4步中就会将其加入生成树中。

        if (spanning_parent_is_not_set_ && owner_keyfrm_->id_ != 0) {
            // set the parent of spanning tree
            assert(*nearest_covisibility == *ordered_covisibilities.front());
            spanning_parent_ = nearest_covisibility;
            spanning_parent_->graph_node_->add_spanning_child(owner_keyfrm_);  // 共视程度最高的作为父关键帧
            spanning_parent_is_not_set_ = false;
        }

生成树的改变
只有在关键帧删除时，才会牵连生成树的变动。

7.3 关键帧的删除

比删除地图点需要多考虑的地方：

1. 当前KeyFrame可能参与了回环检测（头等大事）
2. 删除的时候要重新指定KeyFrame在生成树中父子关系，防止产生回环

与关键帧的删除相关的变量：

will_be_erased_ 就相当于标记bad_flag。cannot_be_erased_相当于标记是否具有不被删除的权利（即参与回环检测的帧）

	//-----------------------------------------
    // flags

    //! flag which indicates this keyframe is erasable or not
    std::atomic<bool> cannot_be_erased_{false};

    //! flag which indicates this keyframe will be erased
    std::atomic<bool> will_be_erased_{false}; 

ORB-SLAM2里还有一项mbToBeErased是用来标记有特权的关键帧是否需要删除的，OpenVSLAM边没有对这一项的操作。应该是通过阶段的方式来控制的

prepare_for_erasing函数中包含操作较多

1. 将当前keyFrame设为toBeErased
2. 删除keyFrame观测到的landmark中，landmark对本keyFrame的观测。
3. 共视图graph_node_中删除所有连接
4. 维护生成树（需要为每个生成的孤儿找到新父亲，不能产生回环）
5. map_db_与bow_db_的操作(先认为是维护全局变量)

其中维护生成树部分操作比较复杂，具体在graph_node.cc文件的recover_spanning_connections()中
每次寻找与候选父亲节点组的权重最大的一个孤儿节点，纳入到候选父亲节点组中
下图有问题，4到7的权重最大，因此4应该接在7后边，而不是2后

7.4 与landmark相关

调用对于keyFrame中观测landmark列表的增删时机：

7.5 本质图 Essential Graph

生成树 + 回环闭合点 + 强共视关系点（权重超过100的边）

7.6 KeyFrame的生命周期

创建就是在Tracking线程中对于KeyFrame进行的创建，说明要用它来建图了。
删除就是LocalMapping线程有专门用于销毁KeyFrame的地方。

八、Initializer

这个类是只针对单目相机的，ORB-SLAM1就有的代码
双目和深度相机都只需要一张图片就可以初始化，左目图片超500特征点就初始化成功
但单目最少两个才能初始化

OpenVSLAM中初始化对象是在initializer.h中，但一些具体的关键数据结构都在base.h里分离出来，initializer.h中再创建一个叫initializer_的base对象

base.h中：ref_undist_keypts_参考帧中特征点，cur_undist_keypts_当前帧特征点，ref_cur_matches_上述两帧之间的匹配点索引。

    //! undistorted keypoints of reference frame
    const std::vector<cv::KeyPoint> ref_undist_keypts_;
    //! undistorted keypoints of current frame
    std::vector<cv::KeyPoint> cur_undist_keypts_;
    //! matching between reference and current frames
    std::vector<std::pair<int, int>> ref_cur_matches_;

几个类关系：

• initializer类的成员变量中有个initializer_对象，是base类型
• perspective类、bearing_vector类继承了base类

调用关系：

• 首先run_image_slam.cc的main函数调用mono_tracking函数。
• mono_tracking函数创建system类对象SLAM,每读入一帧就调用一次system.feed_monocular_frame
• system.feed_monocular_frame调用system类中的tracking_module对象的track_monocular_image方法
• 其调用track函数，track里调用initialize函数
• 其调用initializer类对象initializer_.initialize方法
• initializer_.initialize第一步调用create_initializer()，将本身的initializer_对象（此处为base对象）特化成了perspective类对象
• initializer_.initialize第二步try_initialize_for_monocular方法中再调用initialize方法，就是调用 perspective类中的计算F和H矩阵的方法了

当前两个帧之间进行特征点匹配，计算F和H。两个矩阵方法哪个得分高用哪个方法。（具体计算方法见下）

8.1 RANSAC （Random sample consensus）

内点、外点：
内点就是应该算作正确的点，外点是不该算作正确的点（图像匹配任务就是不该匹配上的点）
算法思想【以直线拟合任务为例】：
由于增大数据采样量也会增加外点数量，因此不能作为解决办法。因此少量多次
少量指的是每次拟合只使用少量的点，多次指的是多次拟合。
这样总能找到一种解使得包含最多的内点

使用好RANSAC算法的方式（减少采样轮数）：
1.减少外点的比例 2.减少每次采样所利用的样本个数（下图两个坐标）

8.2 计算 F 矩阵

F矩阵等概念和计算过程是多视图几何内容，先当做一个黑匣子

先让所有特征点归一化，然后对于每一个点对八点法计算F矩阵，利用RANSAC循环k（500）次找到最优解（其中每次计算完F矩阵都要反归一化，并进行卡方检验）。

8.3 计算 H 矩阵

过程和计算F矩阵类似。

8.4 卡方检验

其实就是看期望结果和实际结果间的差别大不大。
方式是通过构造卡方统计量。
单目特征点重投影误差自由度为2，因此当卡方大于5.99，就说有95%置信度说错的离谱了（即用到了outliers）。

卡方检验函数check_inliers最后返回的score就用来后续比较，k次RANSAC哪次计算出的值是好的，记录最好的那一次。

8.5 使用基础矩阵F和单应矩阵H恢复运动

以perspective.cc中的reconstruct_with_H为例，调用base.cc中的find_most_plausible_pose，找出其中那个假设有效点的数量明显大于其他假设有效点的数量。

ORB-SLAM2来说，这里假设的数量写死了，三角话就是以下四种可能性。

而OpenVSLAM，假设的数量更加灵活了。用了变量代替。

8.6 需要注意的点

已被证明：计算F矩阵只需要7对点，H矩阵只需要4对点。但作者在计算的时候都是用的8对点来做。原因可能是为了卡方检验时候的标准的统一，因为理论上肯定用的点多更精确。

8.7 对极几何

E和F矩阵的rank都是2，即不满rank。这对分析F的自由度有影响。

极线都交于一点，叫极点。也是一个相机光心在另一个相机平面上的投影（e，f）
两个图搜索的时候，同一个特征点一定可以在对应的极线上找到。所以两个图矫正之后极线水平对齐，就可以水平搜索。

九、Tracking module（以单目为例）

五个步骤：

1. 预处理：ORB特征点提取等
2. *初始化
3. 根据前一帧或附近已有的图片估计相机的pose
4. 根据当前帧pose构建出Local map（这个地图只用于优化当前帧pose）
5. 决定是否生成新的关键帧
   

跟踪状态：

// tracker state
enum class tracker_state_t {
    NotInitialized,
    Initializing,
    Tracking,
    Lost
};

当前帧的跟踪状态和上一帧的跟踪状态

    //! latest tracking state
    tracker_state_t tracking_state_ = tracker_state_t::NotInitialized;
    //! last tracking state
    tracker_state_t last_tracking_state_ = tracker_state_t::NotInitialized;

9.1 预处理

这一部分的步骤，包括提取extract_orb提取ORB特征点、undistort_keypoints计算关键点、计算bearing vector等，都是在frame.cc中frame的构造函数中做的。也就是说来一帧图像创建一个frame就会自动调用预处理的过程。

9.2 初始化

上一个初始化成功的帧就设为参考关键帧ref_keyfrm_
local_keyfrms_和local_landmarks_构成了local map，初始化成功后向其中添加

    //! reference keyframe
    data::keyframe* ref_keyfrm_ = nullptr;
    //! local keyframes
    std::vector<data::keyframe*> local_keyfrms_;
    //! local landmarks
    std::vector<data::landmark*> local_landmarks_;

9.2.1 初始化过程（前两步）

OpenVSLAM中将初始化的过程都打散了，放在initializer.initialize里了。
其中包含三个函数：

1. create_initializer(curr_frm)：将init_frm_初始化为当前读入的帧【因为mono所以只有一帧】，并将其关键点位置初始化为之前匹配的点（prev_matched_coords_）。将初始化器特化为perspective类型。
2. try_initialize_for_monocular(curr_frm)：首先在init_frm_和curr_frm间进行空间和角度的连续性匹配，若匹配特征点太少【min_num_triangulated_】则reset。最终进行perspective中的计算H、F矩阵等操作。
3. create_map_for_monocular ：【下一部分具体说】

		case camera::setup_type_t::Monocular: {
            // construct an initializer if not constructed
            if (state_ == initializer_state_t::NotReady) {
                create_initializer(curr_frm);
                return false;
            }

            // try to initialize
            if (!try_initialize_for_monocular(curr_frm)) {
                // failed
                return false;
            }

            // create new map if succeeded
            create_map_for_monocular(curr_frm);
            break;
        }

9.2.2 初始化过程（第三步）

create_map_for_monocular函数：

1. 首先删除未成功三角化的匹配点对，初始化相机参数。
2. 将初始化帧和当前帧都变为关键帧，计算BoW并加入到地图里（map_db_相当于ORB-SLAM2里的mpMap）
3. 将三角化出来的点创建为landmark，并和两个keyFrame创建双向观测。计算landmark的描述子、更改平均观测距离【回忆landmark部分】
4. 全局BA优化（因为只有两帧，所以全局很快）
5. 两帧间平移尺度归一化（平移尺度归一化只有单目有，就是将最初两帧间的距离为基准），关键帧插入map_db_
6. 更改跟踪状态变量为Succeeded

BoW（词袋）
词袋本质上是一种特征点描述子聚类的结果。
在特征点匹配的时候，就是确定其是属于哪一类的。
如果两个特征点是同一类就在内部搜寻加速匹配；如果不是同一类肯定就会是同一个特征点了。

值得注意的
初始化构建局部地图的这个过程中，处理完一个地图点之后没有显式更新共视图。而是将其传递给local mapping线程，其中有关于更新共视图的函数进行处理。（如下，交给了mapper_）

if (tracking_state_ == tracker_state_t::Initializing) {
        if (!initialize()) {
            return;
        }

        // update the reference keyframe, local keyframes, and local landmarks
        
        update_local_map();

        // pass all of the keyframes to the mapping module
        const auto keyfrms = map_db_->get_all_keyframes();    
        for (const auto keyfrm : keyfrms) {
            // spdlog::info("insert init keyframe");  
            mapper_->queue_keyframe(keyfrm);
        }

        // state transition to Tracking mode
        tracking_state_ = tracker_state_t::Tracking;
    }

9.3 Pose Prediction位姿估计 / Relocalization 重定位

Pose Prediction：指的是估计相机的坐标和朝向

这里就看作是从上一帧到当前的一个平移➕旋转的过程（实际就是一个RT旋转矩阵）

OpenVSLAM中单独用frame_tracker类来编写三种方法
其中比ORB里多了一个robust_match_based_track()方法

1. 优先使用恒速运动模型【状态：Tracking】
2. 基于bow_matcher的参考帧pose估计【状态：Tracking】
3. 基于robust_matcher的参考帧pose估计【状态：Tracking】
4. 重定位【状态：Lost】

9.3.1 恒速运动模型 motion_based_track()

主要思想就是假设相机移动速度是恒定的
然后根据速度和上一帧的位姿得到假设当前帧的位姿，然后进行特征点匹配

其中，速度velocity_来自之前的跟踪过程，完成一次tracking过程，就会update_motion_model()获取跟踪的速度，并优先调用运动模型。

		if (velocity_is_valid_ && last_reloc_frm_id_ + 2 < curr_frm_.id_) {
            // if the motion model is valid
            if (!succeeded){
                succeeded = frame_tracker_.motion_based_track(curr_frm_, last_frm_, velocity_);
            }
        }

motion_based_track() 步骤：

1. last_frame的pose * v 得到cur_frame的初始pose
2. 利用得到的pose在last_frame和cur_frame中寻找匹配的特征点
3. 如果找不到，放宽阈值再搜索；搜不到就false
4. 利用BA优化更新cur_frame的pose，并标记outliers
5. 剔除这些outliers
6. 如果匹配的点数超过阈值，则判定为跟踪成功

9.3.2 参考帧跟踪估计bow_match_based_track()

主要思想就是找最相近的关键帧或者是当前帧的参考关键帧进行估计

1. 计算cur_frame的BoW
2. 利用BoW加速匹配如果匹配小于阈值，false
3. 将last_frame的pose设为cur_frame的pose初始值
4. 直接利用BA优化将pose优化到当前帧接近的pose
5. 剔除outliers
6. 如果匹配的点数超过阈值，则判定为跟踪成功

为什么可以用last_frame的pose设为cur_frame的pose初始值？？
因为BoW匹配的过程相当于已经将一些landmark注册到了cur_frame上（知道了landmark和cur_frame的关系），那么BA优化的时候就可以往与cur_frame真实的pose方向自动优化。

9.3.3 robust_match_based_track()

与bow_match_based_track()类似，也是利用ref_keyframe进行匹配。但不再使用bow_matcher快速搜索，而是利用robust_matcher暴力搜索。

具体过程参考bow_match_based_track()

9.3.4 relocalize()

如果用上一帧的检测失败，即状态是Lost时。对于这一帧的检测就要采用重定位了。

1. 从数据库中取出所有与cur_frame相近的帧作为候选参考关键帧reloc_candidates（而不是前两个方法那样采用最近的ref_keyframe），相近与否的判断方式是根据BoW来判断是否属于同一类的。
2. 遍历所有reloc_candidates，利用BoW进行匹配。然后根据3D的landmark和2D的cur_frame构建PnPsolver，来求解cur_frame的pose【使用RANSAC】。再使用BA优化cur_frame的pose，剔除outliers。然后再重复一次，利用BoW估计pose + BA优化pose。（防止误重定位）。如果cur_frame匹配数量不足min_num_valid_obs_个landmark，那么就再重复估计pose + BA优化的过程。
3. 如果还是找不到足够的匹配特征点，就说明当前帧太差了，就重定位失败了。

9.4 跟踪/创建局部地图

跟踪局部地图步骤：

9.4.1 update_local_map()

map = keyframe + landmark，因此这一部分就是更新这两部分

1. 清空所有landmarks
2. 调用acquire_local_map()，其中第一步find_local_keyframes()将keyframe分为first order和second order。first order就是分别找到所有landmark的共视keyframe，second order是first order的四倍size，包括与first order keyframe共视关系top10的keyframe、以及first order keyframe在graph_node上的父子keyframe。first order在前、second order在后，共同组成了local_keyfrms_。【并将共视关系最强的标记下】
3. 调用acquire_local_map()，其中第二步find_local_landmarks()就直接把获得的local_keyfrms_中所有landmark放进列表，组成local_lms_。
4. 用上述两个变量来更新tracking线程的local_keyfrms_和local_landmarks_，并将共视关系最强的设为curr_frm_.ref_keyfrm_。
5. 更新地图库map_db_

9.4.2 optimize_current_frame_with_local_map()

1. search_local_landmarks() ：通过将local landmark重新投射到cur_frame，获得更多的2D-3D匹配。
2. 根据cur_frame对应的landmark，更新pose
3. count跟踪到的landmark，并对每个landmark计数被观测次数，这个次数被看作是landmark好坏评判标准之一，在local mapping线程里被用作landmark剔除
4. 如果cur frame是通过重定位track来的，那就用更严的标准来判断local map跟踪成功与否（2倍的num_tracked_lms_thr）；其他情况就用1倍 thr 的就OK了。

9.5 关键帧的创建

9.5.1 判定是否需要插入关键帧 new_keyframe_is_needed()

判定条件：（代码得空再看）

原则：只要系统能处理的过来就进行创建插入，多创建点后续构建的地图就更详细
因为后续local mapping过程有地图点剔除的过程，所以这里能插即插。

9.5.2 创建新关键帧 insert_new_keyframe()

1. 创建新的keyFrame，插入mapper_的处理队列里（如果mapper is paused【mapper处理不过来了】就插入失败）。
2. 将新创建的keyFrame设为ref keyFrame【插入失败就不设】

9.6 tracking_module::track() 逻辑理顺

第一种情况，如果状态是Initializing：

1. 开始初始化过程，参考9.2部分。
2. 初始化成功就update_local_map()：9.4.1。
3. 将初始化后的两个keyFrame传给mapping module处理，注意此处包含对于Covisibility Graph更新等操作。（mapping module具体看）
4. 状态改为Tracking

第二种情况，如果状态是Tracking / Lost：

1. 执行landmark的replace，都替换为指针指向的landmark。
2. 根据ref keyframe确定last_frm_的pose。
3. 利用四种方式进行pose estimation 9.3。
4. 成功则执行更新局部地图 9.4，更新恒速模型的速度等。
5. 判断是否需要创建关键帧，若需要则创建并添加进关键帧队列。删除outliers。

9.7 keyFrame & landmark

1. 新创建的landmark，与keyFrame是双向观测
2. 在Tracking module中通过ORBmatcher匹配上的关系，是只有keyFrame对landmark的单向观测。在mapping线程中通过这一特点进行区别是否是匹配上的关系。（其实可以考虑在landmark类里记录创建自己的keyFrame。。）

9.8 参考关键帧

十、Mapping module

五个步骤：

1. 得到关键帧缓冲队列之后，处理keyFrame间的landmark关系之类的。
2. 如果是上一帧新创建的landmark，就进行测试，判断landmark好坏。
3. 创建新地图点（此处地图点就是要注册到map里的真正地图点）
4. 局部BA优化
5. 删除掉冗余的keyFrame

注意：
ORB_SLAM2里Local Mapping线程没有利用到Tracking过程构建的临时地图点，而是重新开始，是一个比较不好的方式

10.0 成员变量

tracking和local mapping线程是用keyfrms_queue_进行交互的

	//! queue for keyframes
    std::list<data::keyframe*> keyfrms_queue_;

keyfrm_acceptability_代表local mapping线程是否还愿意接受关键帧，作为tracking线程是否产生关键帧的参考因素之一。（如果系统很需要keyFrame，就算不愿意也得生成并接收）因为只要tracking生成了关键帧，local mapping就得接收

	//! flag for keyframe acceptability
    std::atomic<bool> keyfrm_acceptability_{true};

线程函数run()就是一直在判断keyfrms_queue_中是否还有keyframe。

1. 首先keyfrm_acceptability_设置为false（表示正在mapping，不愿意接收新的keyFrame）。
2. 判断队列是否有keyFrame，有的话启用核心函数mapping_with_new_keyframe()，进行上述五个步骤。
3. 并将此keyFrame送入闭环检测global_optimizer_对列里。
4. 将keyfrm_acceptability_置回为true。

10.1 store_new_keyframe()

1. 取出对列头的keyFrame（cur_keyfrm_）
2. 计算BoW
3. 对于cur_keyfrm_所有的landmark观测序列：判断landmark是否能看到cur_keyfrm_（见9.7部分）。如果看得到说明是由新keyFrame创建的新landmark，则将其放入local_map_cleaner_类的未经考验landmark序列fresh_landmarks_。如果看不到，说明是通过tracking中ORBmatcher匹配到的老landmark，增加landmark到keyFrame的观测并更新landmark的信息【平均尺度、描述子】。（不过OpenVSLAM中好像新老landmark都更新了）
4. 更新keyFrame的Covisibility Graph和spanning tree信息。（tracking初始化过程中创建keyFrame就在这更新共视图，ORB-SLAM中是在tracking中做）
5. 将cur_keyfrm_加入全局地图库中。

10.2 local_map_cleaner_->remove_redundant_landmarks()

这一步是用来剔除【冗余/不好的】landmark

不好的判断标准：

1. 如果landmark已经被标记为will be erased。
2. 召回率 < 0.3。（召回率是说实际观测到该landmark的帧数/理论上应当观测到的帧数，num_observed_ / num_observable_）过小说明可能该点噪声较大，不能留。
3. 在创建后的2（ORB-SLAM是3）帧内被observed的数目小于2，则不稳定，不能留。
4. 经过2帧后仍未被剔除，则是好的landmark

其中Valid只在遍历队列中删除，Invalid还要被标记为will be erased。

		if (lm->will_be_erased()) {
            // in case `lm` will be erased
            // remove `lm` from the buffer
            lm_state = lm_state_t::Valid;
        }
        else if (lm->get_observed_ratio() < observed_ratio_thr) {
            // if `lm` is not reliable
            // remove `lm` from the buffer and the database
            lm_state = lm_state_t::Invalid;
        }
        else if (num_reliable_keyfrms + lm->first_keyfrm_id_ <= cur_keyfrm_id
                 && lm->num_observations() <= num_obs_thr) {
            // if the number of the observers of `lm` is small after some keyframes were inserted
            // remove `lm` from the buffer and the database
            lm_state = lm_state_t::Invalid;
        }
        else if (num_reliable_keyfrms + 1U + lm->first_keyfrm_id_ <= cur_keyfrm_id) {
            // if the number of the observers of `lm` is sufficient after some keyframes were inserted
            // remove `lm` from the buffer
            lm_state = lm_state_t::Valid;
        }

注意
search_local_landmarks()中有修改landmark的num_observable_值的操作。

1. 当cur_frame的初始pose估计成功之后，num_observable_增加观测到的landmark数量。
2. 对于所有local landmark，如果存在于其视锥范围内，则理应也被观测到，所以num_observable_增加。

optimize_current_frame_with_local_map()中有修改landmark的num_observable_值的操作。

• 如果当前landmark被视为内点，则说明真的被观测到了，则num_observed_增加。

10.3 创建新地图点

10.3.1 create_new_landmarks()

去掉了冗余的地图点，就创建些好的地图点来补充。

1. 将当前帧与其共视程度最高的前n【10】个帧取出，进行两两匹配生成地图点
2. 对于单目相机来说，通过对极几何三角化生成landmark。双目、深度的恢复landmark可选方式较多【可选择左相机、右相机、两个相机的深度恢复地图点】。
   

10.3.2 update_new_keyframe()

融合cur_frame和其共视keyFrame的地图点。

fuse_landmark_duplication()中融合的两个步骤：

1. 正向融合：当前帧地图点融合到共视帧中
2. 反向融合：共视帧地图点融合到当前帧中

具体的融合函数是利用的matcher.replace_duplication

融合过程中存在两种情况：

1. 如果欲融合帧中找到的特征点不存在匹配的landmark，则直接添加当前landmark作为其的观测。
2. 如果特征点已经存在一个landmark的对应了，则选择两个landmark中观测数目较多的点（认为更加精准）
   

10.4 local_bundle_adjuster_->optimize()

局部BA优化cur_frame的局部地图

1. 所有地图点都参与优化
2. 一级共识keyFrame会被优化，二级不会

10.5 local_map_cleaner_->remove_redundant_keyframes()

去除冗余的keyFrame。
去除标准：90%以上的地图点能被超过3个其他keyFrame观测到，则被认为冗余，设为will be erased。
注意
此次新加入的关键帧不会被删除，只会删除之前的关键帧。

十一、Global Optimization module

因为暂时不涉及对于回环检测线程的修改（Loop Closing），略过