ORB_SLAM2::Initializer 用于单目情况下的初始化。
Initializer 的构造函数中传入第一张影像,这张影像被称作 reference frame(rFrame)。在获得第二张影像时传入第二张影像,这张影像被称作 current frame(cFrame)。这一部分传入的代码可以在ORB_SLAM2::Tracking::MonocularInitialization()
中查看,要求 rFrame 与 cFrame 都至少具有 101 个特征点,而且 cFrame 与 rFrame 粗匹配结果不少于 100 个点对。这个粗匹配也很有意思,可以查看ORBmatcher::SearchForInitalization()
(粗匹配是对每一个 rFrame 的特征点选定一定大小的窗口,以该特征点在 rFrame 上的坐标为中心,在 cFrame 上提取出覆盖网格内所有的特征点,计算 ORB 描述子的距离,距离够小就说明是匹配点)。
Initializer::Initialize()
在这个函数中完成初始化。首先生成 RANSAC 需要用的最小子集的集合mvSets
。随后开两个线程同步进行FindHomography
和FindFundamental
,这两个函数分别返回SH
、SF
这两个数值用于判定是使用 H 作为初始模型更好,还是用 F 作为初始模型更好。
在用SH
、SF
判定是使用 H 还是 F 之后就是用 H (ReconstructH()
)或 F (ReconstructF()
)生成 R、t 和对应的可以三角化的点用于初始地图生成。
ReconstructH()
是使用 Motion and structure from motion in a piecewise planar
environment 生成 8 种可能结果,再使用CheckRT()
确定是哪一种最为合适。
Initializer::FindHomography()
每次使用 8 个点通过 SVD 分解计算得到 H21。值得注意的是在进行 Homography 计算之前先进行归一化过程(在函数Normalize()
中进行)。
Initializer::Normalize()
归一化过程是将所有的 KeyPoints 进行一次 Affine Transformation,使得变换后的 KeyPoints 均值为原点 \(0\),方差为单位阵 \(I\)。
变换公式为:
\[ x_i^{\prime} = {x_i - \bar{x} \over \sigma} \\ \bar{x} = {\Sigma{x_i} \over N} \\ \sigma = {\Sigma{\left|x_i - \bar{x}\right|} \over N} \]
归一化的原因是计算出的 H 会依赖于特征点的位置,所以先归一化再计算。(MVG Page 104)
归一化的过程可以使用一个矩阵 \(T\) 表示:
\[ x^{\prime} = Tx = \begin{bmatrix} {1 \over \sigma_x} & 0 & \bar{x}{1 \over \sigma_x} \\ 0 & {1 \over \sigma_y} & \bar{y}{1 \over \sigma_y} \\ 0 & 0 & 1 \end{bmatrix} x \]
后面的计算过程如下:
\[ x_2^{\prime} = H_{21}^{\prime}x_1^{\prime} \]
\[ Tx_2 = H_{21}^{\prime}Tx_1 \]
\[ x_2 = T^{-1}H_{21}^{\prime}Tx_1 \]
\[ H_{21} = T^{-1}H_{21}^{\prime}T \]
Initializer::ComputeH21()
在归一化之后,使用归一化的坐标计算 Homography。
没啥好讲,就是 Direct Linear Transformation,参考 MVG Page 88。
Initializer::CheckHomography()
用 H21 和 H12 分别将 rFrame(1) 中的特征点和 cFrame(2) 转换到另一张影像中,计算匹配点的距离误差,距离误差转换为卡方距离,卡方距离小于 5.991 说明显著性为 5%,应该认为它们匹配成功,否则不成功将这一对匹配标记为 false。注意这里有两个自由度。
https://en.wikipedia.org/wiki/Chi-squared_distribution#Table_of_.CF.872_values_vs_p-values
匹配成功能就能将于显著性相关的数值加入到评分中,评分越高说明由这八个点计算出的 Homography 越正确。即由CheckHomography()
返回的评分currentScore
,取 RANSAC 中评分最高的 Homography 作为最终选定的 Homography。
这个 score 会被传入Initialize()
函数中的 SH,用作计算 SH/(SH+SF),判断是使用 Homography 还是 Fundamental。
Initializer::ReconstructH()
用函数FindHomography()
中 RANSAC 计算得到的 Homography 分解,分解能够得到 8 种可能的 \(R, t\) 结果,用CheckRT()
判断选择哪一种结果。
好像这有点错了吧,应该用所有的 inlier 匹配计算 Homography,再用这个更靠谱的 Homography 分解计算 \(R, t\)。
函数ReconstructH()
最后也输出三角化成功的三维点。
Initializer::FindFundamental()
FindFundamental()
的计算过程与 FindHomography 类似,都是需要进行归一化操作。
函数ComputeF21()
用八点法计算 Fundamental,计算得到的实际 Fundamental 通过设置最小特征值为 0 投影到 Fundamental 空间,作为输出。
函数CheckFundamental()
是将点与线的距离作为误差,计算卡方距离,注意这里有一个自由度,所以显著性检验使用的卡方距离为 3.84。
都差不多,没啥好说的。
Initializer::CheckRT()
这个函数挺重要的,因为分解 H 和 F 都会有很多可能的结果,使用这个函数能够分辨出什么结果是靠谱的。
函数CheckRT()
接受 \(R, t\) ,一组成功的匹配。最后给出的结果是这组匹配中有多少匹配是能够在这组 \(R, t\) 下正确三角化的(即 \(Z\) 都大于0),并且输出这些三角化之后的三维点。
如果三角化生成的三维点 \(Z\) 小于等于0,且三角化的“前方交会角”(余弦是 cosParallax)不会太小,那么这个三维点三角化错误,舍弃。
通过了 \(Z\) 的检验,之后将这个三维点分别投影到两张影像上,计算投影的像素误差,误差大于2倍中误差,舍弃。
总结
ORB 里面对通过“最大值”确定的结果都非常小心。一般要求这个“最大值” outstanding,如 ORBmatcher 的构造函数中就有会传入一个 (0,1) 的数值给成员变量 mfNNratio,只有最小距离小于次小距离的 mfNNratio 倍才能算是匹配成功,不允许出现相似的匹配,而取好那么一点点的匹配作为匹配结果。
在Initializer::ReconstructH()
中最后 8 个可能结果中,最好模型 inlier 数要大于次好模型 inlier 的 1/0.75 倍。