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变量）
大括号结束就意味着释放锁

三、System类

构造函数：

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

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

四. ORB Extractor

ORB：oriented BRIEAF
FAST特征点和ORB描述子本身不具有尺度信息
Orbextractor通过构建图像金字塔来得到特征点尺度信息将输入图片逐级缩放得到图像金字塔
金字塔层级越高图片分辨率越低ORB特征点越大，相当于离相机越近
参数在yaml配置文件中设定

三件事：

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

所有的frame都是共享同一个extractor
所以说下一帧来的时候会把原图像的金字塔信息覆盖掉，但特征点信息不会覆盖（保存在了frame里）

五、 MapPoint

视频里的MapPint相当于OpenVSLAM代码里的landmarks
ORB-SLAM里的map信息就是（特征点、关键帧）两个数组

地图点在关键帧里的索引 = 其特征点的索引

1.与关键帧的关系（observations、num_observations_）
std::map<KeyFrame*,size_t> observations_保存了当前关键点对关键帧KeyFrame的观测关系
key为某个关键帧,value为当前地图点在该关键帧中的索引

num_observations_ 代表有多少相机观测到这个地图点了，后续mapping过程有用

2.观测尺度
最小/最大观测距离
超过这个范围就是相机无法观测到的物体了

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

平均观测方向 & 距离

观测距离是根据参考关键帧计算的，其一般是和地图点关系最紧密，视角最好的

地图点的特征描述子（3D）
很多关键帧都观测到了这个地图点，每个地图点都这些关键帧中有对应特征点的描述子
那就选一个最有代表性的描述子作为这个地图点的描述子
去离所有描述子距离和最小的描述子（描述子是二进制串，可以求距离）

3D点到2D点投影的时候
怎么知道哪个特征点和这个地图点对应，就是看哪个特征点的descriptor和3D的最像

地图点的删除和替换
1.删除
因为landmark复杂，删除耗时。防止删的过程中别的线程还在用，那就先标记其为bad
即逻辑删除，具体删的过程以后再说
这样其他线程用的话都要看看标记
*OpenVSLAM代码中set_bad_flag对应的是prepare_for_erasing
bad标记为will_be_erased
2.替换
同样，直接替换的话也是耗时，线程会冲突
因此设置一个replaced_变量，默认为NULL，如果当前被替换，则直接指向新地图点
然后剩下的步骤慢慢做就行了

六、 Frame

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

**********注意***********
以下这一部分是针对双目和深度相机的

虽然我们当前单目相机中使用不到，但就当学习知识了

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

双目视差公式
底下的起点是光心

-----匹配过程-----

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

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

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

七、KeyFrame

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

首先update_connections()就是构建共视图的过程
（其实是rebuild，把之前的变量都清空了，是可以考虑优化的位置）

然后add和erase函数等都是在内部调用的，但设为了public，不合理

2. 生成树
   一个parent和一个child列表
   在生成新的关键帧的时候就将其加入生成树中

3. 关键帧删除
   比删除地图点复杂的地方：

4. 可能参与了回环检测。2.要重新构造他的父子关系，防止产生回环

   参与回环检测的关键帧有不被删除的特权（有个标志位cannot_be_erased=true）

   OpenVSLAM里边没有这一项（等着看代码再看看这里）
   就是该删除的时候检测到有特权先不删，置标记位，后来有空的时候再删
   

其中恢复生成树部分操作比较复杂（我觉得下边这图有点小问题，4应该接在7后边）

4. 关键帧的生命周期
   

八、Initializer

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

在当前帧内调用Initialize函数，当前两个帧之间进行特征点匹配，计算F和H
两个矩阵方法哪个得分高用哪个方法

OpenVSLAM代码中初始化器的相关参数都存在initialize/base.h里边
is_triangulated_ 就是mvbMatched1
几个类关系：
initializer类的成员变量中有个initializer_对象，是base类型
然后perspective类又继承了base类
在initializer的initialize()函数调用create_initializer()
将本身的initializer_特化成了perspective类

RANSAC算法

内点就是应该算作正确的点，外点是不该算作正确的点
思想：少量多次重复实验，总能找到一种解使得包含最多的内点
使用好RANSAC算法思想：1.减少外点的比例 2.减少每次采样使用的样本个数

ref_cur_matches_对应mvMatches12

求取H和F矩阵的过程先当作黑匣子。有了整体过程思路之后再回头理解

九、Tracking线程

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

当前跟踪状态和上一个跟踪状态

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

第一步：预处理：ORB特征点提取、划分网格等（略）
第二步：初始化
代码中初始化过程没有pdf里的第二步（待商榷）
第四步两个帧之间的匹配后比较匹配点，ORB-SLAM2是100，Open代码中是50

词袋
词袋本质上是一种特征点描述子聚类的结果，如果两个特征点是同一类就在内部搜寻加速匹配；如果不是同一类肯定就不是同一个特征点了

tracking中构建局部地图的过程中，处理完一个地图点之后没有更新共视图（待商榷）

平移尺度归一化只有单目有，就是将最初两帧间的距离为标准

第三步：Pose Prediction位姿估计 / Relocalization 重定位
位姿估计
位姿估计就是指相机的位置和朝向
这里就看作是从上一帧到当前的一个平移➕旋转的过程（实际就是一个RT旋转矩阵）
********************
OpenVSLAM中单独用frame_tracker类来编写三种方法
其中比ORB里多了一个robust_match_based_track()方法

1. 第一个是恒速运动模型 motion_based_track()
   主要思想就是假设相机移动速度是恒定的
   然后根据速度和上一帧的位姿得到假设当前帧的位姿，然后进行特征点匹配
   a要是特征点匹配数目超过阈值，就说明真是恒速
   b要是不多，就说明可能出现了速度突变啥的，就得用参考帧跟踪了

2. 第二个是参考帧跟踪估计位姿bow_match_based_track()
   主要思想就是找最近的关键帧或者是当前帧的参考关键帧进行估计
   根据词袋进行匹配，加速匹配
   匹配之后知道当前帧和一些地图点的位置关系后，将上一帧作为当前帧初始值
   然后自己优化，再失败就要设成Lost了

3. robust_match_based_track()

4. 重定位
   下一帧检测到是Lost，就要重定位了
   找数据库里所有的关键帧
   使用PnPsolver估计初始位姿,并根据初始位姿获取足够的匹配特征点
   如果还是找不到足够的匹配特征点，就说明当前帧太差了，就重定位失败了
   第四步：跟踪/创建局部地图
   第五步：关键帧的创建
   原则：只要系统能处理的过来就进行创建插入，多创建点后续构建的地图就更详细
   因为后续local mapping过程有地图点剔除的过程，所以这里能插即插

5. 判定是否需要插入关键帧
   判定条件：
   

6. 关键帧的创建、插入
   把关键帧插入到局部地图里
   Tracking_module总概述

7. 初始状态为Notintialized，创建当前帧对象，构造函数中进行ORB特征提取，将特征划分至网格以后续加速匹配

8. 接收一个新图像之后，改状态为Intializing，进行单目初始化，初始化成功则状态改为Tracking，失败则返回状态

9. 然后运行 track_current_frame() 进行跟踪，采用1运动模型估计/2参考帧估计方法

10. 若跟踪失败，进行重定位，若重定位失败则状态为Lost

11. 若跟踪成功，状态仍为Tracking，进行以下五步：
    a) 更新局部关键帧+局部地图点（并将与当前帧共视点最多的关键帧作为参考关键帧）
    b) 局部地图点投影到当前帧特征点上
    c) 优化当前帧位姿
    d) 更新地图点观测,统计内点个数
    e) 判断是否跟踪成功

12. 判断是否需要创建关键帧，若需要则创建并添加进关键帧队列

注意
所有非临时地图点都是关键帧建立的
非关键帧建立的地图点都很快被删除，左右只是增强帧间的匹配。

十、Local mapping线程

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

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

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

第一步 处理关键帧队列

1. 首先拿出对头的关键帧
2. 然后拿出所有当前帧新创建的地图点放入 fresh_landmarks_ 队列
   注意
   当前帧新创建的地图点是有双向连接的（也就是关键帧能看到地图点，反向亦然）
   而匹配过程生成的地图点是单向连接（也就是地图点看不到关键帧）
   所以就用这个来判断是否是当前帧新创建的地图点
   然后新生成的就直接加入队列，否则就添加单向连接为双向
   

第二步 剔除冗余地图点

OpenVSLAM代码中的阈值是0.3，非0.25
召回率中观测帧数的统计是在tracking线程里的track_local_map中
第三步 创建新地图点
*****去掉了冗余的地图点，就创建些好的地图点来补充
将当前帧与其共视点最多的前n个帧取出，进行两两匹配生成地图点
如果是之前没有的地图点，则进行创建
对于单目相机来说，通过对极几何生成
*****双目、深度就复杂一些（pdf流程图）
（代码待看）
第四步 融合当前关键帧和其共视帧的地图点
正向融合：当前帧地图点融合到共视帧中
反向融合：共视帧地图点融合到当前帧中

融合过程中的两种情况：

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

第五步 局部BA优化
第六步 剔除冗余关键帧
90%以上的地图点能被超过3个其他关键帧观测到。
新加入的关键帧不会被删除，只会删除之前的关键帧

十一、Loop Closing线程