CL关于ORB_SLAM的那些事（MapPoint）

MapPoint部分的学习

昨天在匹配方面一直提及MapPoint，它到底是何方神圣呢，其实从可视化的角度来讲，MapPoint就是我们运行ORB SLAM时在界面上看到的那些地图点，红色黑色的，接下来让我们深入了解一下这个类中都封装了什么函数，维护了哪些变量。

打开头文件，可以看到包括了KeyFrame、Frame和Map三个头文件。其中实现的函数有：

1. 两个构造函数，一个用于关键帧，另一个用于普通的Frame
2. 设置世界坐标系下的坐标
3. 获取世界坐标系下的坐标，因为后面涉及到匹配投影问题，所以需要这样的函数接口，获取世界坐标系下MapPoint的三维坐标
4. 获取MapPoint的法向量
5. 获取参考关键帧
6. 初始定义map映射(关键帧 int)的观测次数，获取观测次数
7. 返回值为int类型的观测次数的计算
8. 添加关键帧的观测
9. 删除关键帧的观测
10. 获取MapPoint在该关键帧中的索引值
11. 判断MapPoint是否在该关键帧中
12. 设置坏的标记位
13. 判断MapPoint是否是坏点
14. 替换MapPoint
15. MapPoint指针类型的获取替换的MapPoint
16. 增加可视？？？？
17. 增加寻找？？？？
18. 获取被找到的频率
19. 内敛函数，获取找到，直接return mnFound
20. 计算特有的描述子
21. 获取MapPoint的描述子
22. 更新法向量和深度值
23. 获取尺度不变的最大和最小距离
24. 根据距离计算所在的金字塔层数，这里分了两个，一个关键帧，另一个普通帧

该类维护的变量：
当前帧号
下一帧的帧号
第一个关键帧的帧号
第一帧
观测的次数
跟踪时的投影x、y和搜索区域r
判断该点是否还在视野中的标志位
跟踪点的尺度因子
跟踪的视角的余弦值
当前帧跟踪参考
上一帧看到的数量
关键帧的局部的BA
关键帧使用的坐标
BA的Pose
关键帧的全局BA
静态的全局锁
##########我是一个分割线，以下的变量都是被保护的############
世界坐标系下的坐标
map映射容器的，关键帧和被观测点的索引
平均观测角度，也就是法向量
描述子Mat
可视化数量
找到的数量
坏点的标记位
MapPoint指针类的变量，用于替换MapPoint
尺度不变的距离范围，从最小到最大
Map类的指针，用来维护整个地图
姿态锁和特征锁

CPP的文件中包含了自身的头文件，还包含了我们昨天刚刚看过的ORBmather头文件。
先是初始化静态变量的下一帧帧号为0

MapPoint的构造函数一共是两个，参数列表不一样，第一个比较简单，这边只简要提一下，它的参数列表为三维Pose、参考关键帧和地图，其余维护的变量如果是数就0，可视化和找到的就是1，指针是空的，bool类型的初始化为false。
其中的操作是，拷贝了世界坐标系下的坐标，然后设置了法向量为3行1列的矩阵，初始的元素值都是0，最后记录一下帧号，下一帧的帧号自增。

MapPoint()
函数的参数列表为位姿、地图指针、当前帧和帧的索引
初始化第一个关键帧的索引为-1，第一帧就是传入参数中的这一帧的帧号，观测数量为0，跟踪的参考为0，上一帧能看到的为0，后面的变量在这就不一一阐述，除了传入了地图，其余都初始化成了0、空指针和false

1. 先把世界坐标拷贝了出来
2. 获取当前帧的传感器在世界坐标系下的坐标
3. 用世界坐标系下的位姿向量减去传感器的中心坐标，得到法向量，最后再归一化
4. 其实这边的Pos有点像是一个点的坐标，这边也是减去传感器的中心坐标，计算距离，获取传入函数内的索引的关键点的金字塔层数，获得该层的尺度因子以及当前帧的金字塔总层数
5. 计算最近和最远的距离
6. 获取该索引点的描述子
7. 因为MapPoint可以在跟踪阶段被创立，也可以在局部地图中创立，所以要设置锁来避免id冲突
8. 记录当前的id，下一帧的帧号自增

SetWorldPos()
设置世界坐标系下的坐标，这边只是一个Mat类型的CopyTo就解决了

GetWorldPos()
获取世界坐标系下的坐标，这边是直接返回Mat的clone，直接返回Mat，它只是一个指针，后面在用的时候可能会出现一点问题。

GetNormal()
获取法向量也是直接return之前构造函数里计算的法向量的clone，不多说。

GetReferenceKeyFrame()
获取参考关键帧，返回参考关键帧的那个变量

AddObservation()
该函数的参数列表：关键帧和索引值
调用mObservations map映射变量来判断此关键帧是否已经在观测中了，如果是，这里就不会添加；如果不是，往下记录下此关键帧以及此MapPoint的索引，算是记录下的观测信息，继续看此点在此关键帧右图中是否有值，如果有，观测次数自增2；如果没有，那么观测次数自增1。

EraseObservation()
移除观测信息，函数的参数列表：关键帧
首先是初始默认bad的标志位为false，然后考察此关键帧是否在观测的序列中，如果在的话，就获取其中的索引，如果在该关键帧的右图中有值，那么这边在移除的时候，需要减去2（因为上面在添加观测时，就是这么添加进去的，所以），如果没有，那就只要减去1就可以。然后移除该关键帧，往下判断该帧是否是参考关键帧，如果是，参考关键帧换成观测的第一帧。最后判断此点的被观测的次数，如果小于等于0，那么该MapPoint就是个坏点，调用SetFlag()函数进行标志位的设置。

GetObservations()
获取观测信息，，直接return map映射的mObservations信息

Observations()
获取观测次数，直接返回nObs变量

SetBadFlag()
设置坏的标记位
定义观测信息的map，先定义bad标志位为true，将维护变量里的观测信息赋值给函数内定义的观测信息map，清空维护变量，循环遍历这个点所有的观测信息，移除匹配关系，最后在地图中移除这个MapPoint。

GetReplaced()
获取代替的MapPoint，直接返回维护变量mpReplaced

Replace()
替换函数，参数列表：一个MapPoint
如果传入的该MapPoint就是当前的MapPoint，直接跳出；如果不是，和上面的SetBadFlag()函数差不多的操作，处理函数里将可见的数量和找到的数量赋值给函数内定义的变量，将该点赋值给可替换的MapPoint。循环遍历所有的观测信息，判断此MapPoint是否在该关键帧中，如果不在，就要替代他的匹配关系，然后增加该点被此关键帧观测的信息；如果该点在此关键帧中，那么只要移除原来MapPoint的匹配信息，最后增加这个MapPoint找到的数量以及可见的次数，计算这个点独有的描述子，地图中要移除原来的那个MapPoint。

isBad()
返回MapPoint是否是坏点的标记位

IncreaseVisible(int n)
IncreaseFound(int n)
在原来原有的基础上增加次数

GetFoundRatio()
计算找到率，这边就用的找到的次数除上可见的次数来代表

ComputeDistinctiveDescriptors()
计算MapPoint特有的描述子

1. 定义一个描述子的容器，存放所有观测的描述子
2. 在保证该点不是坏点的基础上，再进行处理，将观测信息拷贝到函数内定义的变量，如果没有任何观测数据，那就算了。
3. 根据观测次数来重新安排描述子容器的大小
4. 循环遍历观测信息，获取每一个关键帧，在保证关键帧是好的情况下，将该点在此关键帧中的描述子取出来，存到函数内定义的描述子容器内。
5. 判断描述子容器的size，保证在不为0的情况下再继续，计算描述子之间的距离，这边只计算了一个方阵的上半部分，因为两边是对称的。
6. 循环取每一行的距离信息，对其进行排序，取最中间的那个距离，然后比较每一行的中间距离，取最小的那一行，最后取那一行的描述子当做是那个点的描述子

GetDescriptor()
获取描述子，就是将上面那个函数得到的描述子return回来。

IsInKeyFrame()
判断该关键帧是否在观测的关键中，利用map的count，把该关键帧直接扔到count函数中，直接返回结果就可以。

UpdateNormalAndDepth()
更新法向量和深度值
也是先定义函数内的观测信息map，还定义了关键帧和Pose
排除坏点之后，完成定义变量的赋值
定义3行1列的零法向量
循环遍历观测信息，取每一个关键帧，获取世界坐标系下的传感器中心坐标，然后将归一化的法向量进行累加。
获取该MapPoint的世界坐标，计算起到参考关键帧的距离，获取其在此参考关键帧中的金字塔层数，进而获取其尺度信息，最后还需要获取此参考关键帧的金字塔层数，计算带入了尺度的最大和最小距离，法向量的平均值就是该点的法向量。

GetMinDistanceInvariance()
获取最小尺度不变距离，直接return上面计算的最小距离的0.8倍

GetMaxDistanceInvariance()
获取最大尺度不变距离，直接return上面计算的最大距离的1.2倍

PredictScale()
预测当前距离的尺度层，函数的参数列表：当前的距离和所在的关键帧
更新函数中计算的最大距离除上当前的距离，得到一个比例
将这个比例取log除上所在关键帧中的log尺度因子，得到预测的尺度层。如果小于0，就是等于0；如果大于等于关键帧中的最大层数，此尺度层为那个最大层数-1。最后返回计算得到的尺度层。

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