CL关于ORB_SLAM的那些事（Initializer）
Initializer部分的学习总结
看了这么多天孤零零的cpp文件之后，我们今天小小了解一下SLAM部分比较重要的一个模块，那就是初始化，ORB SLAM也是第一个提出自动初始化的开源系统。话不多说，进入我们的主题。
打开.h文件，里面只包含了Frame的头文件，所以我们在看这一部分的代码时，就不存在我们没接触的函数了。
Initializer类中包含的函数功能有：
- 构造函数，函数的参数列表：关键帧、sigma常数1.0、迭代数200
- 返回类型为bool的初始化函数
#####################下面的函数都是私有的##################### - 计算单应矩阵
- 计算基础矩阵
- 计算2到1的单应矩阵
- 计算1到2的单应矩阵
- 检查计算的单应矩阵
- 检查计算的基础矩阵
- 从基础矩阵中恢复旋转矩阵和平移矩阵
- 从单应矩阵中恢复旋转矩阵和平移矩阵
- 三角化函数
- 归一化函数
- 检查计算得到的旋转向量和平移矩阵
- 分解本质矩阵
Initializer类中维护的变量
- 将int和int的pair重定义为Match
#####################下面的变量都是私有的##################### - 关键帧1中的关键点
- 关键帧2中的关键点
- 存放Match类型的容器
- 标记关键帧1中的关键点是否匹配上的标记位
- 传感器的内参矩阵
- 标准偏差和方差
- ransac最大的迭代次数
- 存放ransac系列点的容器
打开cpp文件时，看见其包含的除了自身的头文件之外，还有词袋相关的头文件、优化和匹配
function by function环节
构造函数
Initializer(const Frame &ReferenceFrame, float sigma, int iterations)
函数的参数列表：参考帧、偏差和迭代次数
获取该帧的内参矩阵、关键点、标准偏差、标准方差和ransac的最大迭代次数
初始化函数
Initialize(const Frame &CurrentFrame, const vector &vMatches12, cv::Mat &R21, cv::Mat &t21, vectorcv::Point3f &vP3D, vector &vbTriangulated)
函数的参数列表：当前帧、1到2的匹配、1到2的旋转矩阵、1到2的平移矩阵、3D点集和被三角化的标志位
之前的参考帧是帧1、当前帧是帧2
获取该帧内的所有关键点
清空1到2的匹配，重置1到2匹配容器的预留的size
重置了1匹配标志位容器的size
遍历传入的1到2的匹配，将匹配的关系建立成pair的关系，存入之前清空的1到2的匹配容器内，1匹配标志位容器的标志也置为true；如果没有匹配上的位置，标志位容器的该位置赋值false。
计算得到匹配上的对数
定义size_t类型的容器
用匹配上的对数预留size_t类型容器的大小
定义size_t类型的另一个容器
将匹配上的点对从0开始存入vAllIndices的容器内
定义存放ransac迭代的8个点集的容器，容器的大小就是最大的迭代次数
调用词袋中的随机数产生函数（暂时是个人猜测）
循环最大迭代的次数，将vAllIndices拷贝给vAvailableIndices，进入最大值为8的循环，在匹配的点对中随机取8个匹配点对组成一个点集，选中的点，直接在容器删除了，保证只被选中一次。
定义单应矩阵匹配和基础矩阵匹配的bool类型的容器
定义得分、计算得到的单应矩阵、基础矩阵
定义了两个线程同步进行两个矩阵的计算
最后计算得分的比例，如果单应矩阵的得分占比大于0.4，那么使用单应矩阵进行初始化（分解单应矩阵得到旋转和平移矩阵）；否则使用基础矩阵进行初始化（分解基础矩阵得到旋转和平移矩阵）。要是都没满足就直接返回false。
找到最佳的单应矩阵
FindHomography(vector &vbMatchesInliers, float &score, cv::Mat &H21)
获取匹配上的点对数
归一化坐标值，都是2D点
这边的归一化函数是它自己写的，我们后面再具体看它的归一化过程，往下它取了T2矩阵的逆矩阵。
定义score，初始化为0
定义bool型匹配上的内点的容器
定义存放一组点的容器，这里是两帧，所以是两个容器
定义每组点对计算出的单应矩阵1到2的，2到1的
定义当前是否为内点的标志位，初始化为false
定义当前的分数
循环进行迭代，计算每一组的单应矩阵，这边的计算过程调用了ComputeH21()函数，调用CheckHomography()函数计算得分，选出分数最高的那一组
寻找最佳的基础矩阵
FindFundamental(vector &vbMatchesInliers, float &score, cv::Mat &F21)
大致的过程和寻找最佳的单应矩阵类似，只是在计算时，调用的是计算基础矩阵的函数，最后check的部分也是check基础矩阵，其余的部分都差不多。
计算单应矩阵
ComputeH21(const vectorcv::Point2f &vP1, const vectorcv::Point2f &vP2)
函数传入匹配上的点对，在帧1和帧2中的匹配点
按照上图的推导出的式子进行A矩阵的填充，好像有点不对，我推导与程序里成相反数，不过没啥影响。最后利用opencv自带的SVD函数进行奇异值分解，求得H矩阵。
计算基础矩阵
ComputeF21(const vectorcv::Point2f &vP1,const vectorcv::Point2f &vP2)
函数的输入也是匹配上的点对，这边补充理解这两个矩阵之间的区别以及自由度。
关于基础矩阵的一些思考，都写在图片中了。从程序上来看，关于基础矩阵的求解，利用的是经典的八点法，只不过为了消除噪声的影响，在构造大矩阵之前需要对像素坐标点进行归一化处理，然后再用SVD进行矩阵分解，在得到初步的基础矩阵之后，还有比较重要的一步，把刚刚得到的基础矩阵再进行SVD分解，保证它的秩为2。
校核单应矩阵
CheckHomography(const cv::Mat &H21, const cv::Mat &H12, vector &vbMatchesInliers, float sigma)
函数的参数列表：1到2、2到1的单应矩阵、匹配信息和标准偏差
获取匹配上的点对数
取出单应矩阵的每个元素值
定义得分、阈值和标准偏差的平方逆
循环遍历匹配的点对，计算两帧之间的重投影误差，这边的误差是像素距离的平方，最后乘上的标准偏差平方的逆，得到最终的距离。判断其与阈值的大小关系，如果超过了，那么这个点没有得分，且标记位标记为false；如果没有超过，按照其与阈值的差值进行得分的累加。标记为false的匹配关系也将标记为false。最后返回得分。由于单应矩阵计算了两个方向，这边在校核的时候，两个方向同时进行了校核，得分是累加。
校核基础矩阵
CheckFundamental(const cv::Mat &F21, vector &vbMatchesInliers, float sigma)
获取匹配上的点对数
取出基础矩阵中的每一个元素
定义得分、阈值、最高分和标准偏差的平方逆
循环遍历匹配的点对，利用点到直线的距离公式，进行距离的计算，乘上标准偏差的平方逆得到最后的距离，如果距离超过阈值，则标记为false；如果没有，按照其与最高低分的差距进行得分的累加。标记为false的匹配，匹配信息的容器内也会标记为false。最后返回的得分。这边基础矩阵也是校核两次，得分也是累加的。
从基础矩阵中恢复出旋转和平移矩阵
ReconstructF(vector &vbMatchesInliers, cv::Mat &F21, cv::Mat &K, cv::Mat &R21, cv::Mat &t21, vectorcv::Point3f &vP3D, vector &vbTriangulated, float minParallax, int minTriangulated)
函数的参数列表：匹配信息、基础矩阵、传感器内参、旋转和平移矩阵、三维点的容器、是否三角化的标记容器、最小的平行距离和最小的被三角化的数量
获取经过check之后好的匹配点对的数量
从基础矩阵中获取本质矩阵
这边补充一点关于本质矩阵的内在性质和如何分解出R和t的公式推导，以便我们看接下来的程序。图片的形式展示：
从图片中，我们可以看出由于本质矩阵其本身的特性，在分解的过程中，我们将得到四组不同的R和t，所以在程序中可以看到，它定义了两个旋转矩阵和2个差一个负号的平移矩阵。还有，它定义了四组三维点、是否被三角化的标记位容器和平行距离。
通过调用CheckRT()函数得到每一组的评价
取出四组中评价最好的一组
在0.9倍的匹配好的点对数和给的阈值中确定最小的被三角化的阈值
定义similar变量，用于判断最高的评价是否明显
如果有一组的评价高于0.7倍的最高评价，那么similar变量自增1
如果最高评价都低于阈值，那么直接返回false。另外最高评价与其他组没有拉开差距，那么也将直接返回false。（similar>1）
找到最高评价的那组，在保证其平行距离大于阈值，才能进行三维点、三角化的标志位容器、旋转矩阵和平移矩阵的赋值，最后返回true。如果其平行距离不满足要求，也将直接返回false。
从单应矩阵中恢复出旋转和平移矩阵
ReconstructH(vector &vbMatchesInliers, cv::Mat &H21, cv::Mat &K, cv::Mat &R21, cv::Mat &t21, vectorcv::Point3f &vP3D, vector &vbTriangulated, float minParallax, int minTriangulated)
函数的参数列表：匹配信息、单应矩阵、旋转和平移矩阵、三维点容器、三角化标志位的容器和平行距离、最小的三角化数量
下面的两张图，是我自己一个推导过程，从单应矩阵分解出R和t总共有6种情况。程序里吧图片中第二种情况进行了拆分，所以是8种情况。
所以在程序中进行求解时，首先获取匹配好的点对数量
然后就是构建图片上的G矩阵，程序里是A矩阵。
对A矩阵进行SVD分解
引入s变量，这和图片上的推导是一致的
对分解出的奇异值进行比较，保证只有在d1大于等于d2大于等于d3的情况下进行接下来的求解。
定义了旋转、平移和平面法向量
上图中这是求解了一种情况，其他的情况和这个差不太多，自己手推一下就很清楚了，和程序里的是一样的求解过程。
分解除了R和t之后，后面就要在这八组中选出最佳的那一组。
循环这八种情况，调用CheckRT()函数，找到最佳和次佳，最佳的会记录组的索引、平行距离、三维点和被三角化的数量。次佳的就只是记录评价。
只有当0.75倍的最佳评价高于次佳评价且最佳的平行距离大于阈值、三角化的数量大于阈值、评价中好的点数大于0.9倍的匹配好点对的总数这些条件满足时，最佳的R和t才会被当成是这次的解，并且拷贝给维护变量，还有三维点和三角化的数量，最后返回true。否则，将返回false。
将匹配点对进行三角化
Triangulate(const cv::KeyPoint &kp1, const cv::KeyPoint &kp2, const cv::Mat &P1, const cv::Mat &P2, cv::Mat &x3D)
函数的参数列表：两个匹配上的关键点、两个投影矩阵和三维点坐标
初始定义一个4行4列的矩阵，具体的构建方式，如下图推导所见，
图上的公式推导和程序里第二和第四行差一个负号，但是因为等于0，所以程序里这样的操作不会影响到最后的结果。然后利用SVD求解三角化之后的三维坐标，最后归一化Z值为1。
归一化函数
Normalize(const vectorcv::KeyPoint &vKeys, vectorcv::Point2f &vNormalizedPoints, cv::Mat &T)
函数的参数列表：关键点的容器、标准化之后的点和变换矩阵
定义点集的形心的x和y坐标
获取点集内关键点的个数，根据这个数量来预留标准化点的容器的size
循环计算形心
将点集的形心移动到坐标系的原点
计算标准化之后点的x和y坐标的绝对值之和
计算绝对值之和的平均值
定义了x和y方向的因子，绝对值之后的平均值的倒数
更新标准化之后的点（之前标准化后的点乘上绝对值之后的平均值的倒数，这样做了之后，保证了点集中点的x和y坐标保持在1左右，保证了离原点的距离为根号2左右）
最后定义单位矩阵，T矩阵
0，0的位置存放x方向的绝对值平均值的倒数
1，1的位置存放y方向的绝对值平均值的倒数
0，2的位置存放负的x形心乘上x方向的绝对值平均值的倒数
1，2的位置存放负的y形心乘上y方向的绝对值平均值的倒数
校核R和t矩阵
CheckRT(const cv::Mat &R, const cv::Mat &t, const vectorcv::KeyPoint &vKeys1, const vectorcv::KeyPoint &vKeys2, const vector &vMatches12, vector &vbMatchesInliers, const cv::Mat &K, vectorcv::Point3f &vP3D, float th2, vector &vbGood, float ¶llax)
函数的参数列表：旋转和平移矩阵、两组对应关键点的容器、匹配关系、匹配信息、传感器参数矩阵、三维点、阈值、是否好点的标志位、平行距离
获取传感器的参数
初始化好的bool型容器都为false，size为关键点1的容器大小
存放三维点的容器的size也为关键点1的容器大小
定义了存放两点之间的Cos夹角的容器，size也是关键点1的容器大小
定义，构建帧1的投影矩阵
定义帧1的光心坐标为0
定义，构建帧2的投影矩阵
根据传入的R和t来计算帧2的光心坐标
定义Good的数量为0
循环遍历匹配关系中的对应点，通过好匹配的判断，取出对应关键点，调用三角化函数 Triangulate()，得到3D点坐标。判断得到的三维点是否存在在无穷远的情况，如果没有就直接往下执行；如果有，那么在帧1中的关键点的索引位置标记为false，然后直接跳过本次循环。
计算该3D点到帧1和帧2的光心的向量（归一化），计算向量之间夹角的余弦值。
判断：
1）z值是否小于等于0
2）夹角很小，也就意味在无穷远处
当同时满足时，直接跳出本次循环。
利用R和t，变换到帧2中在来判断，满足上述条件，也将直接跳出本次循环。
计算重投影误差，重投影误差大于阈值的也将跳出本次循环。这边进行两次投影，在帧1、帧2中，只要两者中有一个重投影误差大于阈值，就将直接跳出本次循环。
通过所有的判断后，会将夹角的余弦值存在容器内，三角化的3D点将被存放在关键点在帧1的索引处。Good自增1。关键点在帧1的索引处的good也将被标记为true。
最后判断好点是否大于0，如果大于0的话，将夹角的余弦值按从大到小的顺序排序
最后定义了一个索引值，如果（好点的数量-1）小于50，索引就是（好点的数量-1），不然就是50。
取该位置的角度来代表这两帧之间的夹角，如果好点的数量为0，那么这边的夹角也就为0。
最后返回好点的数量。
分解本质矩阵
DecomposeE(const cv::Mat &E, cv::Mat &R1, cv::Mat &R2, cv::Mat &t)
函数的参数列表：本质矩阵、旋转和平移矩阵
根据上上图上的推导
先是对本质矩阵进行SVD分解
那么平移矩阵t为U矩阵的最小列
平移矩阵最后归一化一下
这边要定义一个推导过程中的W矩阵，它是一个正交矩阵
R = uWvt或者u*W.t()*vt
最后判断这样恢复出来的旋转矩阵行列式是否大于0，如果出现小于0的情况，原来的旋转矩阵需要取一下反，这是对旋转矩阵的要求。
这篇博客写了整整三天，仔细研究了各个矩阵的性质，尤其是本质矩阵和单应矩阵。如何从中恢复出旋转和平移矩阵，我都进行了认真的推导，这从侧面也反映了初始化这个过程在整个ORB SLAM里面扮演者很重要的角色。初始化的内容基本就差不多了。
时间：2019年08月17日
作者：hhuchen
机构：河海大学机电工程学院