超详细解读ORB-SLAM3单目初始化（下篇）

一 前言

本文承接ORB-SLAM3 细读单目初始化过程(上)，ORBSLAM3单目视觉有很多知识点需要展开和深入，初始化过程是必然要经历的，而网上资料不够系统，因此本文主旨是从代码实现出发，把初始化过程系统化，建立起知识树，以把零碎的知识点串联起来，方便快速学习提升自己。注意，本文虽然从代码出发，但并非讲全部代码细节，如有需要建议直接看源代码，地址是：https://github.com/UZ-SLAMLab/ORB_SLAM3，我自己稍微做了点修改，可以跑数据集的版本，可以参考一下，地址是：https://github.com/shanpenghui/ORB_SLAM3_Fixed

二 初始化主要函数

ORBSLAM单目视觉SLAM的追踪器接口是函数TrackMonocular，调用了GrabImageMonocular，其下面有2个主要的函数：Frame::Frame()和Tracking::Track()，本文和上篇都是按照以下框架流程来分解单目初始化过程，上篇记录了Frame::Frame()，本文就记录Tracking::Track()。

1 Tracking作用

ORB-SLAM3的Tracking部分作用论文已提及，包含输入当前帧、初始化、相机位姿跟踪、局部地图跟踪、关键帧处理、姿态更新与保存等，如图。

2 两个主要函数

单目地图初始化函数是Tracking::MonocularInitialization，其主要是调用以下两个函数完成了初始化过程，ORBmatcher::SearchForInitialization和KannalaBrandt8::ReconstructWithTwoViews，前者用于参考帧和当前帧的特征点匹配，后者利用构建的虚拟相机模型，针对不同相机计算基础矩阵和单应性矩阵，选取最佳的模型来恢复出最开始两帧之间的相对姿态，并进行三角化得到初始地图点。

三 ORBmatcher::SearchForInitialization

这个函数的主要作用是构建旋转角度直方图，选取最优的三个Bin，也就是占据概率最大的三个Bin，如图（数字3被异形吃掉了^-^）。因为当前帧会提取到诸多特征点，每一个都可以作为图像旋转角度的测量值，我们希望能在诸多的角度值中，选出最能代表当前帧的旋转角度的测量值，这就是为什么要在旋转角度直方图中选最优的3个Bin的原因。

这个旋转的角度哪来的呢？就是在计算描述子的时候算的，调用函数IC_Angle,代码是：ORBextractor.cc#L475

keypoint->angle = IC_Angle(image, keypoint->pt, umax);

感兴趣的同学想知道为什么要这么麻烦的选取最优3个角度，请从旋转不变性开始理解，原理参见：3-5-3如何保证描述子旋转不变性?

（https://blog.csdn.net/shanpenghui/article/details/109809723#t20）

四 KannalaBrandt8::ReconstructWithTwoView

1 畸变校正

利用鱼眼模型，对两帧图像的特征点进行畸变校正，代码见KannalaBrandt8.cpp#L219。要注意的是，鱼眼模型的特殊性在于只考虑径向畸变，忽略切向畸变，所以其p_ipi值都是0。想要深入理解鱼眼模型的同学可以参考这篇文章《鱼眼相机成像模型》

(https://blog.csdn.net/u010128736/article/details/52864024)。ORB-SLAM3中对不同模型相机的畸变校正做了区分，当相机模型是针孔的时候，用的畸变校正参数是mDistCoef，当相机模型是鱼眼的时候，用的是虚拟出的相机类，代码参见mpCamera = new KannalaBrandt8(vCamCalib)，为避免重复校正，用了个条件限制，就是在函数Frame::UndistortKeyPoints中判断mDistCoef.at<float>(0)==0.0，代码参见Frame.cc#L734，因为在用鱼眼相机模型的时候，mDistCoef没有赋值，都是0。

cv::fisheye::undistortPoints(vPts1,vPts1,K,D,R,K);
cv::fisheye::undistortPoints(vPts2,vPts2,K,D,R,K);

2 位姿估计

主要由函数TwoViewReconstruction::Reconstruct完成，涉及到的知识点又多又关键的，包括对极约束、八点法、归一化、直接线性变换、卡方检验、重投影等，先从主要流程开始理解。

1. 利用随机种子‍‍‍‍‍‍‍‍‍‍‍‍‍‍‍‍‍‍‍‍‍‍‍‍‍‍‍‍‍‍‍‍‍‍‍‍‍‍‍‍‍‍‍‍‍‍‍‍‍‍‍‍DUtils::Random::SeedRandOnceTwoViewReconstruction.cc#L79，在所有匹配特征点对中随机选择8对匹配特征点为一组for(size_t j=0; j<8; j++)TwoViewReconstruction.cc#L86，用于估计H矩阵和F矩阵。
2. 将当前帧和参考帧中的特征点坐标进行归一化。TwoViewReconstruction::NormalizeTwoViewReconstruction.cc#L753
3. 用DLT方法求解F矩阵 TwoViewReconstruction::ComputeF21TwoViewReconstruction.cc#L273
4. 对给定的F矩阵打分,需要使用到卡方检验的知识 TwoViewReconstruction::CheckFundamentalTwoViewReconstruction.cc#L395
5. 利用得到的最佳模型（选择得分较高的矩阵值，单应矩阵H或者基础矩阵F）估计两帧之间的位姿，代码中对应着函数ReconstructH或ReconstructF。其中，分两个步骤。第一是利用基础矩阵F和本质矩阵E的关系 ，计算出四组解。第二是调用的函数CheckRT作用是用R，t来对特征匹配点三角化，并根据三角化结果判断R,t的合法性。最终可以得到最优解的条件是位于相机前方的3D点个数最多并且三角化视差角必须大于最小视差角。

2.2.4 基础矩阵Fundamental 代数推导

有了以上的示意，我们尝试用数学公式描述极点、极线和极平面之间的关系。看了好几篇文章，感觉还是视觉十四讲里面的代数推导比较明晰，我就直接参考过来，当做记录了，其他比较杂乱，记录在《SLAM 学习笔记 本质矩阵E、基础矩阵F、单应矩阵H的推导》（https://blog.csdn.net/shanpenghui/article/details/110133454），感兴趣的同学可以看看。

设以第一个相机作为坐标系三维空间的点：

2.2.6 结尾

由于知识有限，加上篇幅限制，就不再展开了，这里可以参考另外几篇比较好的文章，有比较详细的推导过程，想深入研究的童鞋可以看看。

1、SLAM入门之视觉里程计(3)：两视图对极约束 基础矩阵

2、SLAM基础知识总结（https://blog.csdn.net/MyArrow/article/details/53704339）

五、总结

单目方案的初始化过程再梳理一下：

1. 对极约束是原理基础，从物理世界出发描述了整个视觉相机成像、数据来源以及相互关系的根本问题，其中印象最深的是把搜索匹配点的范围缩小成一段极线，大大加速了匹配过程。
2. 八点法从求解的角度出发，用公式描述了获得我们想要的解的最小条件，提供了有力的数学基础。
3. 归一化使图像进行缩放，而缩放尺度是为了让噪声对于图像的影响在一个数量级上，从而减少噪声对图像的影响。
4. 直接线性转换则从诸多的测量值中（超过8点的N个匹配点，超定方程）算出了最优的解，使我们基本得到了想要的解。
5. 在已经有的粗解基础上利用统计学方法进行分析，筛选出优质的点（符合概率模型的内点）来构成我们最终使用的一个投影的最优解，利用两帧图像上匹配点对进行相互投影，综合判断内外点，从而最小化误差。
6. 筛选出内外点之后，对两个模型进行打分，选出最优模型，然后通过三角化测量进行深度估计，最终完成初始化过程。

至此，单目的初始化过程（基于基础矩阵F）就完啦，内容较多，希望不对的地方多多指教，相互学习，共同成长。以上仅是个人见解，如有纰漏望各位指出，谢谢。

参考：

1.对极几何及单应矩阵https://blog.csdn.net/u012936940/article/details/80723609

2.2D-2D：对极约束https://blog.csdn.net/u014709760/article/details/88059000

3.多视图几何

https://blog.csdn.net/weixin_43847162/article/details/89363281

4.SVD分解及线性最小二乘问题

https://www.cnblogs.com/houkai/p/6656894.html

5.矩阵SVD分解（理论部分II——利用SVD求解最小二乘问题）

https://zhuanlan.zhihu.com/p/64273563

6.奇异值分解(SVD)原理详解及推导

https://blog.csdn.net/zhongkejingwang/article/details/43053513

7.最小二乘解（Least-squares Minimization ）

https://blog.csdn.net/kokerf/article/details/72437294

8.卡方检验 （Chi-square test / Chi-square goodness-of-fit test)

https://blog.csdn.net/zfcjhdq/article/details/83512680?utm_medium=distribute.pc_relevant.none-task-blog-BlogCommendFromBaidu-3.control&depth_1-utm_source=distribute.pc_relevant.none-task-blog-BlogCommendFromBaidu-3.control

9.样本标准差与自由度 n-1 卡方分布关系的证明

https://blog.csdn.net/robert_chen1988/article/details/90640917

10.证明残差平方和除随机项方差服从卡方分布

https://www.docin.com/p-1185555448.html

11.本质矩阵优化分解的相对位姿估计

http://www.doc88.com/p-6931350248387.html

12.单目移动机器人的相对位姿估计方法

https://www.doc88.com/p-7744747222946.html

13.三角化求深度值（求三位坐标）

https://blog.csdn.net/michaelhan3/article/details/89483148