CL关于ORB_SLAM的那些事（KeyFrame）
KeyFrame部分的学习总结
上一次阅读MapPoint的代码时，很多函数里涉及到关键帧部分的一些函数，所以今天我们来看一下KeyFrame部分的代码。
老规矩，先从头文件看起
KeyFrame头文件中包含了昨天我们看到MapPoint、词袋向量、特征向量、ORBVocabulary、ORBextractor、Frame和KeyFrameDatabase的头文件，从这些包含的头文件来看，这个类还是很重要。其实，在这个ORB SLAM中，关键帧的策略也是比较关键的。所以进入主题吧。
KeyFrame包含的函数：
- 自身的构造函数
- 设置位姿
- 获取位姿
- 获取位姿的逆
- 获取传感器中心的坐标
- 获取双目的中心坐标
- 获取旋转矩阵
- 获取平移矩阵
- 利用词袋计算特征
- 增加连接
- 移除连接
- 更新连接
- 更新最好的Covisibility，怎么理解这个东西呢，后面还要根据论文和代码来理解一下，查了一下翻译，也没找到它的解释。
- 获取连接的关键帧
- 获取向量Covisibility下的关键帧
- 获取最好的Covisibility关键帧
- 根据权重来获得Covisibility
- 获取权重
- 增加子树
- 移除子树
- 改变父节点
- 获取子节点
- 获得父节点
- 判断此关键帧是否有子节点
- 增加回环的边
- 获取回环的边
- 添加MapPoint
- 移除MapPoint的匹配关系（索引）
- 移除MapPoint的匹配关系（MapPoint）
- 替换MapPoint的匹配
- 获取MapPoint（获得的是一系列的点）
- 获取MapPoint的匹配
- 跟踪MapPoint
- 获取MapPoint（这边只是获取一个MapPoint）
- 获取区域内的特征点（有一种很熟悉的感觉哦，之前的某一个类内也实现了这个函数，那这边会不会有不一样呢？）
- 重投影双目
- 判断是否在图像中（又是一个似曾相识的函数）
- 设置不要移除
- 设置要移除
- 设置坏的标志位（+1）
- 判断是好是坏（+1）
- 计算场景中的中位深度
- 权重的比较（静态函数，在头文件中实现了，直接比较输入的两个参数的大小，a>b 返回true）
- 比较两个关键帧的帧号（第一帧的帧号>第二帧的帧号，返回true）
类内维护的变量有：
下一帧的帧号、当前的ID、当前帧的帧号、时间戳信息、栅格的长宽、栅格部分高和宽的倒数（这四个变量都是用在加速特征点匹配部分）、跟踪参考帧、关键帧中使用目标点（这两个变量是用在跟踪部分）、关键帧的局部BA、关键帧的固定点的BA（这两个变量用在局部地图部分）、回环队列、回环字符标志、回环得分、重定位的队列、重定位的字符标志、重定位的得分（这几个变量都用在关键帧数据集部分）、全局BA的变换矩阵、之前全局BA的变换矩阵、关键帧用于全局BA（这几个变量用于闭环检测部分）、一系列标定参数、关键点的数量、存放关键点的容器、存放去畸变的关键帧的容器、存放右图上每一个值的容器、存放图上深度值的容器、描述子、词袋向量、特征向量、相对父节点的变换矩阵、尺度层数、尺度因子、取完对数的尺度因子、放尺度因子的容器、存放尺度因子平方的容器、存放平方倒数的容器、图像的边界值（最小X和Y，最大X和Y）、内参矩阵
###################下面是protected的变量###################
从传感器到世界的变换矩阵
从世界到传感器的变换矩阵
传感器的中心坐标
双目中心点的坐标
存放关联关键点的地图点的容器
关键帧数据集类型的指针
ORB词汇类型的指针
二维容器的栅格，存放int类型的值
map映射，被连接的关键帧的权重
按顺序连接的关键帧，这些关键帧存放在关键帧指针的容器中
顺序权重
第一个是否连接的标志位
关键帧类型的父节点指针
set容器内存放关键帧指针类型的子节点
set容器内存放关键帧指针类型的闭环边
判断不要移除的标志位
判断将被移除的标志位
判断坏点的标志位
基线距离的一半
Map类型的指针
姿态锁、关键帧连接锁、特征锁
下面就一个一个来看其中的函数
CPP文件中包含了除了自身头文件之外，还包括Converter.h、ORBmatcher.h两个头文件。
一上来，也是初始化下一帧帧号为0
#构造函数
KeyFrame()
函数的参数列表：当前帧、地图Map指针和关键帧数据集的指针
将当前帧内的信息，如帧号、时间戳、帧栅格的长宽、栅格长宽的倒数、然后都是初始化为0的数量变量、传感器内参数赋值等，基本就是当前帧的所有信息，拷贝了一份在KeyFrame中。其中的第一个连接变量赋值为true，其余bool类型的变量都是false。
函数内部：
- 将下一帧的帧号赋值给mnId，然后自增1
- 根据栅格的列数重置栅格的size
- 将该真的栅格内信息拷贝了一份给关键帧类内的变量
- 最后将当前帧的姿态调用SetPose()函数拷贝出来
计算词袋方面的变量
ComputeBoW()
首先是判断词袋向量和特征向量是否为空
在都不为空的基础上，调用Converter中的转换函数将描述子转换成描述子向量，调用ORBvocabulary中的变换函数将描述子向量、词袋向量、特征向量和词袋树的层数
设置姿态
SetPose()
函数的参数列表：变换矩阵
将变换矩阵拷贝给另一个变量
从中获取旋转矩阵、平移矩阵、 旋转矩阵的逆矩阵和传感器的中心坐标
定义变换矩阵的逆矩阵，将刚刚计算的旋转矩阵的逆矩阵放到原本旋转矩阵的位置，刚刚计算的传感器的中心坐标其实也是原来平移矩阵的逆，所以这边就把之前计算的传感器的中心坐标放在了逆变换矩阵平移向量的位置，定义了一个4行1列的矩阵，第一行存放的是基线长度的一半，第二、三行都是0、最后一行是1，用刚刚得到的变换矩阵的逆矩阵与该点坐标相乘得到新坐标系下的坐标。
获取姿态信息
GetPose()
函数内直接return变换矩阵
获取逆姿态信息
GetPoseInverse()
函数内直接return逆变换矩阵
获取传感器的中心坐标
GetCameraCenter
函数内直接return SetPose()函数内计算得到的Ow变量
获取双目的中心坐标
GetStereoCenter()
函数内直接return SetPose()函数内计算得到的Cw变量
获取旋转矩阵
GetRotation()
函数内直接return 变换矩阵中的旋转矩阵的部分
获取平移矩阵
GetTranslation
函数内直接return 变换矩阵中的平移矩阵的部分
增加连接
AddConnection()
函数的参数列表：关键帧和权重
判断该关键帧是否在已连接的关键帧集中了，如果没有，利用map的数据建构，建立该关键帧和权重的映射；如果有了，就判断其权重是否这次的权重相同，不相同的话就直接用这次的权重值更新。最后调用UpdateBestCovisibles()函数来更新，具体怎么实现的，后面再看这个函数。
下面就是这个函数
UpdateBestCovisibles()
定义存放关键帧和int类型的pair的容器
根据连接上的关键帧的数量，设置上面定义的容器的大小
通过循环将原来的连接信息，拷贝给上面定义的容器内，只是这边在连接时，权重在钱，关键帧在后
按照权重的大小进行排序
将关键帧和权重分别取出放进容器中
将这种按权重拍好序的关键帧放到维护变量中，包括权重。
获取连接上的关键帧
GetConnectedKeyFranems()
定义set的关键帧容器
将存放连接关键帧的map中取出关键帧的部分放进set的容器中
返回set的容器
获取N帧排好序的关键帧
GetBestCovisibilityKeyFrame()
函数的参数列表：一个整型的数（需要取多少帧关键帧）
判断当前排好序的容器中存放的关键帧的数量，如果小于N，那就全部取出，然后return；如果满足数量，取的时候也就取N帧关键帧，然后返回。
根据权重获得关键帧
GetCovisibleByWeight()
判断按顺序排放的关键帧的容器内是否为空，如果是，就直接返回空容器；如果不是，就调用upper_bound()，经过查阅一定的资料了解到，该函数是在排好序的数组中进行查找大于某一个出入参数的数字，最后返回那个数的位置。（补充学习lower_bound()函数是寻找第一个大于等于的数然后返回位置）不存在的话返回end，另外，这边再补充该函数的第四个参数，调用了关键帧中的一个比较函数，它的作用主要是控制二分法查找方向的控制，如果是true那么二分法就会往右边进行，而如果是0的话那么就往右边进行。
如果upper_bound()函数没找到符合要求的，那么就返回end，所以这边也就判断，返回值是否是end，如果是函数直接返回空容器；如果不是，就代表找到了，计算它的位置，然后取出在这个位置之前的所有关键帧，最后返回。
获取权重
GetWeight()
判断连接关键帧是否有这个关键帧，在有的基础上返回map的映射值；如果没有，那么直接返回0。
添加MapPoint
AdddMapPoint()
函数的参数列表：MapPoint和索引
按照索引在维护变量里存放该MapPoint
删除MapPoint的匹配关系1
EraseMapPointMatch()
函数的参数列表：索引
直接在维护变量中的该索引位置处置空
删除MapPoint的匹配关系2
EraseMapPointMatch()
函数的参数列表：MapPoint
获取该MapPoint在当前关键帧中的索引index
根据索引将存放MapPoint的容器那个位置的指针置空
替换MapPoint的匹配关系
ReplaceMapPointMatch()
函数的参数列表：索引index和MapPoint
直接就是将维护变量中该索引index处的MapPoint替换成参数列表中传入的MapPoint。
获取MapPoint
GetMapPoint()
定义set的容器，存放MapPoint
将存放MapPoint的容器内取出好的MapPoint，存放到set的容器内，最后返回set的容器
跟踪MapPoint
TrackedMapPoint()
函数的参数列表：最少的观测次数
定义初始化点数为0
观测次数大于0，check的标志为就为true
循环遍历该关键帧中的所有关键点
判断这个点是否是MapPoint
MapPoint是否是好点（被多次观测到）
check该点的观测次数是否大于等于最小次数，如果是点数++
不check的话，就直接点数++
最后返回可被跟踪的点数
获取MapPoint的匹配关系
GetMapPointMatch()
直接返回存放匹配关系的MapPoint容器
获取索引位置处的MapPoint
GetMapPoint()
函数的参数列表：索引值
根据索引值返回MapPoint
更新连接关系eKeyFrames();
UpdateConnections()
初始定义map映射关系的关键帧计数容器
定义MapPoint指针类型的容器，维护变量中的相关信息全部拷贝到了这个容器中
循环遍历此容器，取出好的MapPoint，获取其观测信息，存放在函数内定义的map容器内，然后循环map容器里的关键帧，查看他们的帧号，排除帧号和当前帧号相同的关键帧，如果不是，map映射关系的关键帧计数容器在该关键帧处的自增1，计算完数之后，判断该容器的是否存在观测这个MapPoint的关键帧，但没有观测的关键帧的这种情况一般是不会发生的。
往下就看计数是否大于添加连接的阈值
定义了一定的阈值、最大值和关键帧的指针
定义了存放int和关键帧的pair的容器
根据计数容器的大小设置了pair容器的大小
循环遍历计数容器，寻找观测MapPoint最多的关键帧，只有大于规定的阈值的关键帧和观测次数成对存放到pair容器内，添加关键帧的连接（调用AddConnection()函数）。
判断pair容器总是否有结对的关键帧，如果是空的话，就把最多的观测的关键帧成对存放进pair容器内
按照观测次数的多少进行排序
用list来单独存放关键帧和他们的观测次数
更新维护变量中的连接的关键帧的权重、排好序的关键帧、排好序的权重
如果第一个是连接的且当前帧不是第一帧的话，连接上的第一帧就是父节点，增加子节点，第一帧连接上的标志位为false。
添加子节点
AddChild()
函数的参数列表：关键帧
直接在存放子节点的set容器内插入传入这一帧
删除子节点
EraseChild()
函数的参数列表：关键帧
直接将存放在set中的子节点删除
改变父节点
ChangeParent()
函数的参数列表：关键帧
将该关键帧赋值给父节点
调用该关键帧的AddChild()函数增加子节点
获取子节点
GetChild()
函数直接返回存放子节点的维护变量
获取父节点
GetParent()
函数直接返回维护变量中的父节点
判断某关键帧是否是子节点
hasChild()
函数的参数列表：关键帧
直接调用存放子节点的容器count方法来判断，传入的此关键帧是否在子节点中
添加回环的边
AddLoopEdge()
函数的参数列表：关键帧
将不删除的标志位置为true
在存放回环边的容器内存放此关键帧
获取回环的所有边
GetLoopEdges()
函数直接返回存放回环边的容器
设置不删除的标志位
SetNoErase()
在函数内直接将维护变量中的不删除标志位置为true
设置删除
SetErase()
判断回环边容器内是否有关键帧，如果没有，不删除的标志位赋值为false
如果删除的标志位为true，直接调用SetBadFlag()函数。
设置坏的标志位
SetBadFlag()
判断当前帧的帧号，保证不是第0帧
不删除的标志位是true，那么被删除的标志位就是true
（这边不太能理解这边这两个标志位的关系，从函数名的定义上来看，一个是设置不移除，另一个是移除。在设置不删除的函数里设置不删除的标志为true能理解，在设置移除的函数里，通过被删除的标志位来选择性的调用SetBadFlag()。在此函数里，竟然是通过不被删除的标志位来给被删除的标志赋值true，感觉这里面的逻辑关系有点乱，后面在调用的时候再看吧。不过从函数内实现的过程，当不删除的标志位为true时，虽然被删除的标志位赋值了true，但是SetBadFlag()函数也直接return了。所以？？？？）
接着往下看这个函数，循环遍历连接上的关键帧-权重的容器，对每一帧关键帧进行连接的移除
循环遍历MapPoint，如果是MapPoint的话，将观测相关的信息也删除
清空连接关键帧-权重和顺序存放关键帧的容器
更新生成树
定义存放关键帧的父坐标系的set容器
将父节点存入该容器
在子节点容器非空的情况下进行循环，定义bool型的变量Continue并初始化为false，定义了最大值-1，定义了两个关键帧的指针，循环遍历子节点容器，取每一个关键帧，判断该关键帧的好坏，获取按权重排序的关键帧，在其中取帧号相同的那一帧，获取那一帧的权重，获取权重最大的那一帧，保存关键帧、连接上的那一帧关键帧、权重值和Continue标志位置为true。Continue标志位真的置为true时，该关键帧的父节点更换成那个权值最大的关键帧，此关键帧也将被插入父坐标系的容器内，删除存在在子节点容器中的那个关键帧。
判断子节点的容器内是否为空，循环遍历子节点，子节点中没有连接父坐标的节点都连接此关键帧的原始父节点。
父节点删除该子节点
获取父节点的逆姿态与变换矩阵相乘得到一个坐标值
Bad标志位置为true
最后地图中删除这一关键帧
关键帧数据集中也删除这一关键帧
总体再看此函数的功能，当一帧关键帧要被移除时，需要将与他关联的关键帧进行更新，最后才能在地图和数据集中对它进行移除。所以这个函数的重点是怎么把关联在此关键帧上的关系全部进行更新。再来看它的更新方式，首先搞清楚的是，这里面包含的变量都在此关键帧内，所有这边变量的改变和其他关键帧内的变量无关。记住这一点再往下看，连接的信息、关键帧全部清空，这里面涉及到生成树的更新，所以一开始的生成树的父节点还是原来那个将被删除的关键帧的父节点，循环子节点容器内的关键帧，获取排好序的关键帧，取帧号和要被删除的关键帧的帧号相同的那一帧且权重值最大的那一帧，那么该子节点就以这一帧为自己的父节点，其他的子节点就以原来被删除的关键帧的父节点作为自己的子节点，原来的父节点就直接删除他的子节点，也就是这个要被删除的关键帧，最后就是在地图和数据集中进行改关键帧的删除。
坏点的标志位
isBad()
直接返回bad的标志位
删除连接
EraseConnection()
函数的参数列表：关键帧
定义的Update的标志位为false
判断要删除的关键帧在连接的关键帧内是否存在，如果有，直接删除，更新的状态为true，最后调用UpdateBestCovisibles()来更新。
获取某区域内的特征点
GetFeaturesInArea()
函数的参数列表：x、y中心坐标，窗口size
确定具体的区域，返回该区域内所有的关键点的索引值
判断点是否在像素平面内
IsInImage(const float &x, const float &y)
两个方向的坐标直接和最大最小值比较，只要超过，就不在像素平面内。
从函数名看不出是什么功能的函数
UnprojectStereo(int i)
获取 i 对应点的深度值
深度值大于0，就计算该点在相机坐标系中的坐标
乘上变换矩阵，变换到世界坐标系下的坐标
直接返回该点的坐标值
这下看出来了，加了Un的protect其实是反投影，且必须是在有深度值的情况下。
计算场景中的中位深度
ComputeSceneMedianDepth(const int q)
定义MapPoint的容器、变换矩阵
将维护变量中的MapPoint信息拷贝给函数内定义的MapPoint的容器，变换矩阵也拷贝给函数内定义的变换矩阵。
定义存放深度值的容器
单独取出与Z相乘的那一行，以及平移矩阵Z部分的值
循环遍历每一个三角化得到的MapPoint，获取他们在世界坐标系下的坐标，转换到当前帧的坐标系下
对每个深度值进行排序，获取1//q处的深度值
返回该深度值
时间：2019年08月14日
作者：hhuchen
机构：河海大学机电工程学院