CL关于ORB_SLAM的那些事（MapPoint）
MapPoint部分的学习
昨天在匹配方面一直提及MapPoint，它到底是何方神圣呢，其实从可视化的角度来讲，MapPoint就是我们运行ORB SLAM时在界面上看到的那些地图点，红色黑色的，接下来让我们深入了解一下这个类中都封装了什么函数，维护了哪些变量。
打开头文件，可以看到包括了KeyFrame、Frame和Map三个头文件。其中实现的函数有：
- 两个构造函数，一个用于关键帧，另一个用于普通的Frame
- 设置世界坐标系下的坐标
- 获取世界坐标系下的坐标，因为后面涉及到匹配投影问题，所以需要这样的函数接口，获取世界坐标系下MapPoint的三维坐标
- 获取MapPoint的法向量
- 获取参考关键帧
- 初始定义map映射(关键帧 int)的观测次数，获取观测次数
- 返回值为int类型的观测次数的计算
- 添加关键帧的观测
- 删除关键帧的观测
- 获取MapPoint在该关键帧中的索引值
- 判断MapPoint是否在该关键帧中
- 设置坏的标记位
- 判断MapPoint是否是坏点
- 替换MapPoint
- MapPoint指针类型的获取替换的MapPoint
- 增加可视？？？？
- 增加寻找？？？？
- 获取被找到的频率
- 内敛函数，获取找到，直接return mnFound
- 计算特有的描述子
- 获取MapPoint的描述子
- 更新法向量和深度值
- 获取尺度不变的最大和最小距离
- 根据距离计算所在的金字塔层数，这里分了两个，一个关键帧，另一个普通帧
该类维护的变量：
当前帧号
下一帧的帧号
第一个关键帧的帧号
第一帧
观测的次数
跟踪时的投影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
- 先把世界坐标拷贝了出来
- 获取当前帧的传感器在世界坐标系下的坐标
- 用世界坐标系下的位姿向量减去传感器的中心坐标，得到法向量，最后再归一化
- 其实这边的Pos有点像是一个点的坐标，这边也是减去传感器的中心坐标，计算距离，获取传入函数内的索引的关键点的金字塔层数，获得该层的尺度因子以及当前帧的金字塔总层数
- 计算最近和最远的距离
- 获取该索引点的描述子
- 因为MapPoint可以在跟踪阶段被创立，也可以在局部地图中创立，所以要设置锁来避免id冲突
- 记录当前的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特有的描述子
- 定义一个描述子的容器，存放所有观测的描述子
- 在保证该点不是坏点的基础上，再进行处理，将观测信息拷贝到函数内定义的变量，如果没有任何观测数据，那就算了。
- 根据观测次数来重新安排描述子容器的大小
- 循环遍历观测信息，获取每一个关键帧，在保证关键帧是好的情况下，将该点在此关键帧中的描述子取出来，存到函数内定义的描述子容器内。
- 判断描述子容器的size，保证在不为0的情况下再继续，计算描述子之间的距离，这边只计算了一个方阵的上半部分，因为两边是对称的。
- 循环取每一行的距离信息，对其进行排序，取最中间的那个距离，然后比较每一行的中间距离，取最小的那一行，最后取那一行的描述子当做是那个点的描述子
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
机构：河海大学机电工程学院