CL关于ORB_SLAM的那些事（Tracking）

关于Tracking部分的学习总结

看完了ORB SLAM的初始化部分，下面我们就来看一下Tracking部分。这也是整个ORB SLAM系统中比较重要的一个部分，话不多数说，开始吧。

头文件部分

这里面包含了opencv关于特征的两个头文件、可视化view.h、FrameDrawer.h、Map.h、局部地图LocalMapping.h、闭环检测LoopClosing.h、Frame.h、ORBVocabulary.h、KeyFrameDatabase.h、ORBextractor.h、Initializer.h、MapDrawer.h和System.h
看到这么多的头文件，感觉今天的代码有点难读啊，但是既然已经开始了，就要坚持看到最后。

Tracking类内有的函数：

1. 构造函数
2. 采集双目相机信息的函数
3. 采集RGB_D相机信息的函数
4. 采集单目相机信息的函数
5. 设置局部地图
6. 设置闭环检测
7. 设置可视化
8. 改变传感器的标定参数
9. 通知只进行Tracking
10. 系统重启
    ################我只是个分割线，下面的函数都是protected的函数#########################
11. 跟踪的函数
12. 双目初始化函数
13. 单目初始化函数
14. 创建初始化单目的地图
15. 校核替换最后一帧
16. 跟踪参考关键帧
17. 更新最后一帧
18. 用运动模型进行跟踪
19. 重定位
20. 更新局部地图
21. 更新局部point
22. 更新局部关键帧
23. 跟踪局部地图
24. 搜索局部点
25. 需要新的关键帧
26. 创建新的关键帧

Tracking类内部维护的变量
用枚举类枚举了5种跟踪状态，包括系统没准备好、还没有图像信息、未初始化、准备好的矩阵和丢失。
用枚举类定义当前的状态以及上一帧处理的状态
int定义传感器的类型
定义Frame的当前帧
定义Mat的当前灰度图
###单目初始化定义的变量#####
vector类型的vector，定义上一次匹配信息
vector类型的vector，定义匹配信息
之前匹配的2D点，Point2f类型的容器
存放3D点的容器
Frame类型初始化的那一帧

存放相对帧位姿（Mat）的list
存放参考关键帧的list
存放帧Times的double容器
存放是否丢失的标注位，bool类型的容器
是否只进行跟踪的标志位
###########下面的变量都是protected的################
是否只是VO的标志位
提取特征的类指针（提取左图的、右图和初始化一共三种）
词袋类型的，ORB词汇类型的指针
关键帧数据集的类指针
初始化类的指针(这个变量只用于单目初始化)
关键帧类的指针，定义的参考关键帧
存放关键帧的局部关键帧容器
存放MapPoint的局部地图点的容器
系统类的指针
可视化类的指针
画每一帧类的指针
画地图类的指针
地图类的指针
标定参数，内参矩阵和畸变矩阵和基线
最大帧数
最小帧数
深度值阈值
深度值的地图尺寸因子
帧中当前匹配的内点数量
关键帧类定义的上一关键帧的指针
Frame类型定义的上一帧
上一帧关键帧的ID
上一帧重定位的帧号
Mat定义的速度
颜色空间是否是RGB的bool型的标志位
暂时存放MapPoint的list

function by function

也是先来看一下引用的头文件，除了自身的头文件之外，opencv关于特征点的头文件、ORBmatcher.h、FrameDrawer.h、Converter.h、Map.h、Initializer.h、Optimizer.h和PnPsolver.h，看这头文件的阵势，在这个cpp文件里肯定能学到很多东西，终于看到我想看的优化部分，话不多说，开始我们的function by function。

构造函数
Tracking()
函数的参数列表：系统类的指针、ORB词汇类的指针、画帧信息类的指针、画地图类的指针、地图类的指针、关键帧数据集品类的指针、常字符串类型的配置文件的路径和传感器的类型。
维护变量的初值：
状态：没图像信息的状态
传感器的状态：参数传递
只跟踪的标记位为false
只是VO的标记位为false
ORBVocabulary类指针：参数传递
关键帧数据集类指针：参数传递
初始化指针指向空
系统类的指针：参数传递
可视化类指针指向空
画Frame类的指针：参数传递
mpMapDrawer，画地图类的指针：参数传递
上一次重定位的帧号为0
函数的内部执行内容：
读取配置文件，获取传感器的各个参数
构建内参矩阵和畸变矩阵
基线长度
传感器的帧率30
最小帧为0，最大帧为30
把这些信息打印在屏幕上
读取图像的通道顺序
提取ORB特征的一些参数，包括特征点的数量、尺度因子，金字塔的层数、最小、最大的fast角点检测阈值。
右图提取特征点指针，指向一块新内存，处理右图，如果是双目，程序中将再定义右图提取特征点指针，指向一块新内存，处理左图；如果是单目，初始化提取特征点的指针指向一块新内存，单独处理单目初始化时的特征点提取。单目这边唯一不同的是，初始化时提取的特征点数量是平时的两倍。
这边如果是双目或者是RGB_D相机的话，深度值的阈值也将通过读取配置文件中的数据并通过一定的计算得到。
下面继续根据因为是RGB_D相机，地图的深度值因子，也需要通过配置文件进行读取，如果该因子小于一定的值，那么该因子将被置1，否则该因子，就是这个数的倒数。

设置局部地图
SetLocalMapper()
函数的参数列表：局部地图类的指针
维护的局部地图指针被传入的值覆盖

设置闭环检测
SetLoopClosing()
函数的参数列表：闭环检测类的指针
维护的闭环检测类的指针被传入的值覆盖

设置可视化
SetViewer()
函数的参数列表：可视化类的指针
维护的可视化类的指针被传入的值覆盖

采集双目图像信息
GrabImageStereo()
函数的参数列表：左右图、时间戳
将左图赋值给维护变量里的灰度图
定义一个Mat的变量，接收右图的信息
判断图像的通道数，根据不同的情况，进行图像的灰度处理，保证进入SLAM系统的图像是单通道的灰度图
调用Frame中双目的构造函数，完成当前帧的特征点的提取，然后调用Track()函数，最后返回当前帧的变换矩阵（从传感器坐标系到世界坐标系）。

采集RGB_D相机图像信息
GrabImageRGBD()
函数的参数列表：彩色图、深度图和时间戳
流程和双目的差不多，但是在进行Frame封装之前，根据尺度因子，对深度图进行一定的处理。后面都是Track，返回当前帧的变换矩阵。

采集单目相机的图像信息
GrabImageMonocular()
函数的参数列表：彩色图、时间戳信息
过程也是类似的，只不过单目这边的关于特征点的提取，没完成初始化的时候，用的是Initial的指针，初始化了之后就是普通的左图指针。

跟踪函数
Track()
在Tracking类的构造函数内，状态被初始化为了还没图像的状态，所以这边的第一个if判断条件是满足的，再往下将状态改成未初始化，将此状态赋值给上一次处理的状态。
获取地图更新的锁，锁住，地图不让更新了
判断到当前的状态为未初始化，按照不同的传感器，调用不同的初始化函数，双目和RGB_D，就调用StereoInitialization()，单目就调用MonocularInitialization()。初始化函数执行完了之后，就调用画Frame类型中的Update()函数，更新新的Frame显示，判断初始化是否成功。看状态值，如果是OK，那就是初始化成功，就直接return。
判断到当前的状态已经是完成了初始化的状态
定义一个OK的bool型变量
判断只跟踪的标志位为false，如果是，再往下判断当前的状态是否是OK，如果是，调用CheckReplacedInLastFrame()（局部地图有可能会改变上一帧中的跟踪的MapPoint，所以这边要调用校核上一帧中被替换的MapPoint），再判断速度的容器是否为空或者当前帧的帧号还不到上次重定位帧号+2，那么就进行参考关键帧的跟踪，否则就利用运动模型进行跟踪，当运动跟踪失败后，就调用跟踪参考关键帧，保证跟踪的稳定性；如果不是，说明系统丢了，就直接调用重定位的函数Relocalization()，OK标志位用来接收重定位的状态（失败还是成功）。
判断只跟踪的标志位为true，如果状态为丢，那么就调用重定位的函数Relocalization()；如果状态不为丢，那么就判断VO的标志位，在构造函数里初始化为false，如果是false的话，然后判断是否有速度值，有就用运动模型跟踪，没有就跟踪参考关键帧。
判断VO的标志为true：
定义OKMM的bool变量并赋值false，定义OKReloc的bool变量并赋值false。
定义存放地图点的容器
定义Out存放bool型的容器
定义相机到世界坐标系的变换矩阵
如果速度不为空的话，跟踪运动模型，将当前帧的MapPoint、外点和变换矩阵拷贝给之前定义的变量。
往下进行重定位函数的调用
判断跟踪运动模型是否成功，重定位是否成功
如果两者都成功了，系统更相信跟踪运动模型的结果，就把跟踪运动模型得到的变换矩阵、MapPoint和外点赋值给当前帧。
另外因为是VO，循环遍历当前帧的关键点数量，如果当前的MapPoint是且不是外点，该MapPoint增加被find的次数。
如果只有重定位成功了，那么只VO的标志位赋值false。
最后两种跟踪模式只要有一种成功了，OK就会被赋值true
将参考关键帧赋值给当前帧的关键帧
如果只跟踪的标志位是false，且之前的跟踪是OK的，那么就跟踪的局部地图，调用TrackLocalMap()。
如果只跟踪的标志位是true，且之前的跟踪是OK的，另外VO的标志位是false，那就跟踪局部地图，调用TrackLocalMap()函数。（程序里补充了当VO的标志位为true时，就意味着和地图中的MapPoint有较少的匹配关系，我们不能得到局部地图，所以不会调用跟踪局部地图函数，一旦系统重定位，又将会使用局部地图）
如果之前的OK标志位的状态是OK，那么当前的状态就是OK；
如果之前的OK标志位的状态不是OK，那么当前的状态就是LOST。
调用更新界面上灰度图的函数，进行界面内容的更新
#######再往下就是如果跟踪是好的，就要考虑是否插入关键帧#######
如果跟踪是好的，判断上一帧中的变换矩阵是否为空，如果不是，获取上一帧中的逆旋转和逆平移矩阵，构建上一帧的逆变换矩阵，然后左乘当前帧的变换矩阵，得到速度信息（三维坐标的变化）；如果上一帧速度值为空，那么这一帧的速度信息就是空。
调用画地图点中设置相机当前帧的姿态，传入当前帧的变换矩阵。
循环遍历当前帧的所有的关键点，取对应的MapPoint，如果是MapPoint，判断其被观测的次数，如果小于1，那么这个MapPoint就是一个外点，进行标记false且MapPoint的这个位置也将指向空。
遍历暂时存放MapPoint的list，进行删除。
list清空
校核是否需要新的关键帧，如果需要的话就创建新的关键帧。调用如下代码：

if(NeedNewKeyFrame())
     CreateNewKeyFrame();

这些函数，我们往下都会看到，这边就提及到。
往下程序中把当前帧中不是外点的MapPoint全部指向空
如果系统的状态是丢，且地图中的关键帧也不大于5帧，那么系统将会调用重启函数进行重启，然后直接return。
如果当前帧的速度信息不为空，将当前帧的变换矩阵乘上参考关键帧的逆姿态获得从当前位置到参考关键位置的变换矩阵。把这个矩阵存放到相对帧位置的容器内，把当前帧的参考关键帧的存入参考的容器内，把当前帧的时间戳信息存入帧的时间戳容器内，把当前帧是否为丢的状态存入记录的容器内。
如果当前帧的速度信息为空，将相对帧姿态的最后一个相对姿态存入，将最后一个参考关键帧存入，将最后一帧的时间戳信息存入，当前帧的是否为丢的状态存入记录的容器内。

立体视觉初始化
StereoInitialization()
判断当前帧的关键点是否大于500
设置当前帧的初始姿态，4×4的单位阵
第一帧必然是关键帧，所有定义初始化关键帧指针，调用关键帧的构造函数，建立关键帧。
将这一关键帧添加进Map的关键帧中。
循环遍历所有的关键点，判断深度信息是否大于0，调用Frame的反投影函数，得到在相机坐标系下的3D坐标。新建新的MapPoint，在MapPoint内添加此关键帧对该点的观测，在该关键帧中添加这个点的MapPoint，计算该MapPoint的描述子，更新法向量和深度值，在Map中添加该点的MapPoint。、
在屏幕上打印出这一新的地图中有多少个地图点
在局部地图中插入该关键帧
把当前帧赋值给上一帧
当前帧的帧号赋值给上一关键帧的帧号
初始化的关键帧赋值给上一帧的关键帧
初始化的关键帧存入局部关键帧的容器内
获取现在地图中的所有MapPoint存放在局部MapPoint的容器内
初始化的关键帧赋值给参考关键帧
初始化的关键帧赋值给当前帧的参考关键帧
调用地图中的设置参考MapPoint函数SetReferenceMapPoints()传入局部MapPoint
将初始化的关键帧存入地图中的关键帧源头的容器内
调用画地图中的设置当前帧位姿的函数SetCurrentCameraPose()，传入当前帧的变换矩阵
最后将现在的系统的状态赋值OK

单目初始化
MonocularInitialization()
由于在构造函数内，初始化的指针指向空，所以这边的判断语句必然满足
判断当前帧的关键点的数量是否大于100
把当前帧拷贝给初始化帧、上一帧
匹配信息容器的大小也和关键帧的数量一样
循环遍历关键点，将待匹配点的像素坐标赋值给前匹配的容器内
判断初始化的指针是否有指向的内存，如果有就delete。
将初始化指针指向一块新内存，用new，调用初始化的构造函数
在初始化的匹配中容器内，所有值都赋值-1
最后return
如果判断初始化的指针有地址
如果当前帧的关键点数量不大于100
直接delete初始化的指针，指针指向空
匹配信息的容器中的所有元素赋值-1
最后直接return
如果当前帧的关键点数量满足要求，下面就要寻找匹配关系了。
ORBmatcher类实例化一个对象，调用匹配的构造函数
调用匹配中专用于初始化的匹配函数SearchForInitialization()进行匹配
匹配点对数不小于100时，才会继续，不然将直接delete初始化的指针，让其指向空，最后直接return。
继续定义旋转和平移向量
定义点对被三角化的情况，bool型的容器
调用初始化的Initialize()函数，如果得出了用哪个矩阵进行初始化，循环遍历匹配信息，如果有两点的匹配关系但是没有被三角化的点，直接删除匹配关系，匹配点对数自减。
设置初始化帧的姿态为4×4的单位矩阵
构造当前帧的变换矩阵
设置当前帧的姿态
调用建立初始化单目地图的函数CreateInitialMapMonocular()。

建立初始化单目地图
CreateInitialMapMonocular()
初始化的第一帧和第二帧都封装成了关键帧
调用关键帧的计算词汇函数
在地图中插入这两帧关键帧
循环遍历匹配点对，创建世界坐标系下的MapPoint，调用MapPoint类的构造函数，将该MapPoint添加进这两关键帧中，该MapPoint也添加这两关键帧的观测，计算该MapPoint的描述子，更新这个MapPoint的法向量和深度值，在地图中添该MapPoint。
初始化关键帧更新连接
当前关键帧更新连接
在屏幕上打印地图中MapPoint的数量
开启优化函数，调用的全局BA，函数传入地图指针，一个常数值20。//终于看到了优化，竟然有点激动！
计算场景中的深度中位值
场景中的中位深度值的倒数（逆中位深度值）
如果中位深度值小于0或者当前关键帧中被观测的次数小于1的MapPoint的点数小于100，那么本次的初始化是错误的，系统将重启，调用Reset()函数，最后直接return。
获取当前帧的变换矩阵
用逆中位深度值进行了当前帧的姿态的更新
用逆中位深度值进行所有MapPoint的尺度更新
将初始化关键帧、当前关键帧插入局部地图中
设置当前帧的pose
将当前帧的帧号赋值给上一关键帧的帧号
把当前关键帧拷贝给上一帧的关键帧
将当前关键帧和初始化的关键帧存入局部关键帧
获取当前地图里所有的MapPoint，拷贝给局部MapPoint
当前的关键帧赋值给参考关键帧和当前帧的参考关键帧
当前帧赋值给上一帧
地图中设置参考MapPoint，函数传入的参数为当前地图里所有的MapPoint
调用画地图中的设置当前相机位姿的函数，函数传入的参数是当前关键帧的位姿
将初始化的关键帧存入关键帧原点的容器内
最后把系统的状态赋值为OK

校核被替换上一帧中的MapPoint
CheckReplacedInLastFrame()
循环遍历上一帧中的MapPoint，如果指向MapPoint的话，调用MapPoint中的GetReplaced()函数获取该点被替换的MapPoint，如果这个点确实存在，那么就进行替换。

跟踪参考关键帧
TrackReferenceKeyFrame()
计算当前帧的词袋向量
实例化ORBmatcher类型的对象
定义存放匹配MapPoint的容器
调用匹配中的SearchByBoW()函数来进行进行匹配
匹配的数量，如果少于15，直接return false
将匹配上的MapPoint拷贝给当前帧的MapPoint
先直接用上一帧的姿态来设置当前帧的姿态
调用优化类中的相机姿态优化函数进行当前帧姿态的优化
循环遍历当前帧中的所有关键点，取MapPoint中的外点，将其进行删除，具体就是先将该索引位置MapPoint的指针置空，置空之后那么这个点也不存在是不是外点了，所以外点标志为置为false，这个点也不适合在视场里继续被跟踪，所以这个点的被跟踪的标志为置false，把当前帧的帧号赋值给这个点最后被哪个帧看见，匹配数自减。如果这个点不是外点且被观测的次数大于0，那么进行计数。
最后满足刚刚说的条件的点的数量大于10，就返回true，不然就返回false。

更新上一帧
UpdateLastFrame()
取上一帧的参考关键帧当做是之前的关键帧
取最后的相对帧间的位姿变换（参考关键帧-上一帧）
利用这个位姿变换乘上参考帧在世界坐标系下的坐标得到上一帧在世界坐标下的位姿，调用设置姿态的函数，传入这个计算好的姿态。
判断上一帧是否是关键帧，或者传感器是单目，或者仅跟踪的标志位为false，只要满足这三个条件之一，直接return，不用更新上一帧。
定义存放pair深度，id的容器
根据上一帧关键点的数量，给上面的容器预留出size
循环遍历上一帧的所有关键点，获取他们的深度值，如果深度值大于0，就把这个深度值和关键点的索引pair起来，存入容器中。
判断这个容器内是否存在点对，如果没有就直接return
根据深度值的大小进行从小大的排序
循环遍历该容器的每一个点对，取他们关键点索引，定义一个是否要创建新点的标志位变量，一开始赋值为false，如果该索引对应的MapPoint没有产生，该标志位置true，或者是产生了MapPoint，但是这个MapPoint被观测的次数小于1，该标志位也置true。如果要创建新的MapPoint，那么就把上一帧中该索引的关键点反投影到相机坐标系下，得到其3D坐标，利用MapPoint的构造函数，生成一个新的MapPoint，将这个新点放入上一帧的MapPoint容器内，这个点也被存入暂时点的容器内，记录点数的变量自增。如果不需要添加MapPoint，那么记录点数的变量也会自增，记录上一帧中所有的MapPoint的数量。
如果该点的深度值大于阈值且得到的MapPoint的数量大于100，那么直接跳出循环。这个循环保证了此帧的MapPoint肯定是不小于100个的，然后100个以后的点的深度值也是不大于阈值的。

用运动模型进行跟踪符
TrackWithMotionModel()
用ORBmatcher类实例化一个匹配的对象
调用上面刚刚讲的那个函数，进行上一帧中信息的更新
用速度值（两帧之间的变换）乘上上一帧中的变换矩阵，得到当前帧的Pose，调用设置当前帧Pose的函数进行设置Pose
把当前帧中的MapPoint都指向空
定义一个阈值
如果传感器是双目，那这个阈值就是7；如果不是，那这个阈值就是15。
调用投影寻找匹配函数，函数的最后一个参数是传感器等于单目
如果匹配的点对数量少于20，就将当前帧的匹配的MapPoint进行置空，放宽到原来2倍的匹配窗口size，再进行匹配
如果匹配的点对数量还是少于20，那么将直接返回false。
调用优化类内的位姿优化的函数，传入当前帧
循环遍历当前帧中的所有关键点，取其对应的MapPoint，判断是否有MapPoint，保证MapPoint的基础上，继续进行外点的判断，如果是外点的话，将删除这个点的所有信息，把关键帧该索引处的MapPoint置空，外点置false，这个点也不适合在视野里进行跟踪，该标志位置false，当前帧的ID赋值给该 MapPoint的最后一次被观测的帧号变量，匹配的数量自减。如果不是外点且被观测的次数大于0，匹配上的MapPoint的数量自增。
如果只是跟踪的模式，匹配上的MapPoint的数量与10进行比较，小于10，VO的标志位为true；大于10，该标志位为false，直接返回 true（匹配点对数大于20）或者 false（匹配点对数不大于20）。
如果不是跟踪模式，直接返回匹配上的MapPoint的数量与10比较的结果，不小于10，返回true；小于10，返回false。

跟踪局部地图
TrackLocalMap()
更新局部地图，调用UpdateLocalMap()函数
寻找局部点，调用SearchLocalPoints()函数
循环遍历当前帧中的关键点，获取对应位置的MapPoint，判断该点是否为外点，如果不是，该MapPoint就增加了被找到的次数，如果不是只跟踪的模式，判断该MapPoint被观测的次数是否大于0，如果是匹配的内点自增1；如果是只跟踪模式，匹配内点数量直接自增1。
该点是外点，在双目的情况下，该MapPoint指向空。
如果当前帧的帧号小于（上一次重定位的帧号的+最大帧率(30)）且匹配的内点数量少于50，那么跟踪失败
过了上面的判断，如果内点数少于30，那么跟踪也是失败，否则就是跟踪成功。

判断是否需要新的关键帧
NeedNewKeyFrame()
如果是只跟踪模式，那么直接返回false
如果局部地图被闭环检测冻结，那么也不会插入关键帧。这边是判断局部地图是否被停止或者是停止要求，如果满足其一，直接返回false。
当地图中的关键帧数量大于最大帧率且当前帧的帧号距离上次重定位的帧号少于最大帧率时，就直接返回false。
定义最少观测的次数为3
如果地图中的关键帧不大于2帧，最少观测的次数设置为2。
获取参考关键帧中能跟踪的MapPoint的数量（观测次数不少于2）
调用局部地图类中的接收关键帧的函数，判断是否要接收。
定义int类型的两个变量，不跟踪的close点为0，跟踪的close点为0
如果传感器不是单目，循环遍历当前帧的关键点，判断其深度值是否大于0且深度值是否小于规定的阈值，如果同时满足，判断是否存在MapPoint且是不是外点，如果同时满足，跟踪的close点的数量自增；如果不同时满足，那么不跟踪的close点的数量自增。
如果跟踪的close点的数量小于100且不跟踪的close点的数量大于70，需要插入close点的标志位置true。
定义一个比率阈值0.75
如果关键帧的数量小于2，这个阈值改为0.4
如果传感器是单目，那么这个阈值改为0.9
当前帧的帧号是否大于上一关键帧帧号+最大帧率
当前帧的帧号是否大于上一关键帧帧号+最小帧率且局部地图中是否接收关键帧
传感器是不是单目且匹配的内点数量是否小于0.25的参考关键帧中的好MapPoint的数量或者是否需要插入close点
匹配的内点是否小于参考关键帧中好MapPoint数量的之前定义的阈值倍或者是否需要插入新的close点且匹配的内点数是否大于15
针对以上的四种情况，总结一下

如果前三种情况中，满足了一种且满足第四种情况，那么就有可能需要插入关键帧。
如果不是，那么就直接返回false。
往下继续判断局部地图是否接收关键帧，如果接收，那么直接就返回true；如果不接受（正忙），先把BA打断，判断传感器是否是单目，如果是，直接返回false；如果不是，判断局部地图中的关键帧队列中的关键帧的数量是否小于3，如果是，直接返回true；如果不是，就返回false。

创建新的关键帧
CreateNewKeyFrame()
调用局部地图中的设置不停的函数，传入的参数为true
如果没停，直接return
利用关键帧的构造函数进行关键帧的封装
将此关键帧赋值给参考关键帧
当前帧的参考关键帧也是此关键帧
判断传感器是否是单目，如果不是，调用当前帧的更新位姿矩阵。定义pair<深度值，索引>的容器；用当前帧的关键点的数量给这个容器预留出size；循环遍历当前帧中的关键点的深度，如果大于0，将深度值和索引pair起来存放在之前定义的容器内。如果该容器内有点对，将其按照深度值从小到大排序；循环遍历，按照深度值从小到大取对应的索引（这边的算法流程和更新上一帧的函数差不多），定义初始化创建的标志位为false，获取该索引位置的MapPoint，如果还不是的话，创建的标志位置true；如果该点观测的次数少于1，那么也需要创建新的点，该创建标志位置true，该点置空。如果需要创建，先是计算相机坐标系下的坐标，新建MapPoint，点添加观测，该关键帧添加这个MapPoint，计算这个新MapPoint的描述子，更新这个新MapPoint的法向量和深度值，地图中也添加这个新的MapPoint，该MapPoint存入当前帧中的MapPoint的容器内，点数++；如果不需要添加，点数直接++；最后保证插入的MapPoint的数量大于100，先是保证数量大于100，在保证了数量了之后就要保证这些点的深度值小于阈值。
将该关键帧插入局部地图的关键帧
局部地图不停止的标志位置false
当前帧的ID赋值给上一关键帧的帧号
关键帧拷贝给上一关键帧

寻找局部点
SearchLocalPoints()
遍历当前帧中的所有MapPoint，如果是有指向的MapPoint，在这个点是好点的基础上，增加该点被观测的次数，上一次观测这个点的帧号也就是当前帧了，在视场中能被跟踪的标志位置false；如果该点是坏点，直接将其置空。
循环遍历局部MapPoint，判断其最后一次观测是否是当前帧，如果是，直接循环下一个MapPoint；判断是否是坏点，如果是，直接循环下一个MapPoint；调用当前帧的isInFrustum()函数，判断该MapPoint的一些属性（深度是否大于0、投影到当前帧中是否在视场中、是否在尺度不变的距离内、观测角是否在要求的范围内），满足条件的话，增加该点的可见次数，匹配数量++
如果能被匹配点的数量大于0就开始匹配
定义ORBmatcher的对象
定义阈值为1
如果是RGB_D的话，阈值改为3
如果当前帧的帧号小于（上一次重定位帧号+2），阈值改为5
调用匹配中的SearchByProjection()函数，进行匹配。

更新局部地图
UpdateLocalMap()
调用地图类中的设置参考MapPoint的函数，函数传入的参数是局部MapPoint
调用更新局部关键帧的函数
调用更新局部点的函数

更新局部点
UpdateLocalPoints()
将存放局部MapPoint的容器清空
循环遍历局部关键帧，取其中的一帧关键帧，获取其中存放匹配上的MapPoint的容器，循环遍历其中的每一个MapPoint，判断是否是MapPoint，如果不是，就直接continue；如果该点跟踪参考帧号等于当前帧的帧号，也直接continue；如果这个点是好点，将其存放如局部MapPoint的容器中，该点的跟踪参考帧帧号赋值当前帧的帧号。

更新局部关键帧
UpdateLocalKeyFrames()
定义关键帧和索引之间的map变量
循环变量当前帧中的所有关键点，取对应的MapPoint，如果当前点是个好点，获取这个点的观测信息。循环遍历这个点的所有观测信息，记录每个关键帧观测到MapPoint的数量；如果这个点是个坏点，就直接在当前帧中的MapPoint进行置空。
如果最初定义的map变量的为空，就直接return
定义max变量为0
定于max关键帧指向空
将局部的关键帧清空
预留局部关键帧的大小为上面map容器的size
循环遍历该map中的信息，取每一帧关键帧，如果该关键帧是坏的，就直接return；得到观测到MapPoint最多的关键帧及其观测到的MapPoint的数量；将每一帧好的关键帧存入局部关键帧的容器内，将当前帧的帧号赋值给跟踪参考帧的帧号。
循环遍历，刚存入的局部关键帧，如果局部的关键帧已经超过80帧了，后面也就不会执行了，直接跳出整个循环；如果没有超过，取出当前的关键帧，调用关键帧的GetBestCovisibilityKeyFrames()函数，这边的函数参数为10，相当于是取连接的前10帧关键帧，如果容器内少于10帧，就将容器内所有的关键帧取出来，循环遍历这里面的关键帧，在该关键帧非bad的情况下，判断该帧的参考关键帧是否为当前帧，如果不是，将该关键帧存入局部关键帧的容器内且该帧的跟踪参考关键帧帧号赋值为当前帧的帧号。
获取该关键帧的子帧
循环遍历所有的子帧，判断条件和上面一样，同样将满足要求的帧存入局部关键帧中，将跟踪参考帧帧号赋值为当前帧的帧号。
获取该关键帧的父帧
如果存在，且其参考关键帧不是当前帧，那么也将这帧存入局部关键帧中，将跟踪参考帧帧号赋值为当前帧的帧号。
判断是否有share最多MapPoint的关键帧，如果有，就将此帧当做是参考关键帧，将该参考关键帧赋值给当前帧的参考关键帧。

重定位
Relocalization()
调用当前帧计算词袋向量的函数
获取关键帧数据集队列内的关键帧
如果队列中没有，就直接返回false
定义此关键帧容器内的关键帧的数量
定义ORBmatcher类型的对象
定义PnPsolver类型指针的容器
pnp求解器容器的大小等于关键帧的数量
定义存放MapPoint类型的二维容器
该容器的行数为关键帧的数量
定义是否丢弃的bool型容器
容器的大小还是关键帧的数量
循环那个容器内的候选关键帧，如果这一帧是bad，容器对应位置的丢弃的标志位置true；否则，进行词袋向量的匹配，如果匹配的点对数少于15，该关键帧位置处的丢弃标志位置true；；否则，就进行PnP的求解。
这样的循环，肯定能求解出很多组解
定义匹配的标志位为false
重新实例化了匹配的对象，只是函数传入的第一个参数变成0.9
while（候选的数量大于0并且匹配的标志位为false）就一直指向
在while中，循环遍历所有的候选关键帧，判断其是否要被丢弃，如果是就直接下一关键帧；否则，定义内点的bool型容器，定义内点的数量，定义没有其他解的bool型变量，取其求解结果，调用求解器内的迭代函数，求解得到变换矩阵，当迭代的次数达到5次之后，没有其他解的bool型变量置true，在没有其他解的bool型变量为true的情况下，将丢弃的标志位置true，候选关键帧数量自减。如果有求解出的变换矩阵，将该变换矩阵拷贝给当前帧的变换矩阵，定义存放MapPoint指针的set，获取内点的数量，将内点对应的MapPoint拷贝给当前帧的MapPoint，也将这些MapPoint插入set内；如果不是内点，当前帧该位置的MapPoint置空。
调用优化类中位姿优化函数，进行优化，这边先猜测是返回的好的MapPoint数量
如果数量少于10，将继续下一循环。如果数量满足要求，循环当前帧的关键点，判断是否为外点，如果是就将当前帧对应位置的MapPoint置空。
如果内点的数量不少于50，匹配的标志位置ture，相当于是当前帧和这关键帧匹配上了，直接跳出for循环，while循环也因为匹配标志位置true也停止循环。
如果好的MapPoint的数量少于50，通过将匹配上的MapPoint投影到当前帧内进行寻找匹配，如果通过这样匹配得到的额外的匹配点对，加上之前的内点数仍然少于50，那么循环下一关键帧；如果加起来不少于50了，重新进行优化，返回内点数，如果这次的数量不少于50，与上面的不少于同样的操作；如果这次的数量在30到50之间，在来一次投影匹配，如果相加不少于50，进行当前帧的位姿优化，循环遍历当前帧的关键点，将外点对应的MapPoint都置空。

系统重启
Reset()
屏幕上打印"系统重启"
如果可视化指针不为空，发出停止要求，如果可视化没有被停止，每次循环睡眠3秒。

屏幕上打印“重启局部地图”
局部地图类发出重启要求
屏幕上打印“完成”

屏幕上打印“重启闭环检测”
发出重启要求
屏幕上打印“完成”

屏幕上打印“重启数据集”
将关键帧数据集清空
屏幕上打印“完成”

地图清空
关键帧的下一帧帧号置0
普通帧的下一帧帧号置0
系统状态重置为无图片
如果是已经完成初始化的，delete初始化指针，将其指针置空

相对帧位姿的容器清空
参考帧清空
帧时间清空
丢失的容器清空
如果可视化的指针还有指向，将其释放。

改变标定参数
ChangeCalibration(const string &strSettingPath)
函数的参数列表：文件的路径
读取文件内的相机参数
更新内参和畸变参数
将帧内的初始化计算的标志为置true

通知值跟踪
InformOnlyTracking(const bool &flag)
函数的参数列表：bool型的flag
直接将参数赋值给只跟踪的标志位

时间：2019年08月21日
作者：hhuchen
机构：河海大学机电工程学院