近段时间重新回顾了SLAM相关的基础知识，顺便将自己在组会上讲的PPT整理成文档同大家分享但谢绝转载。
本文通过回顾SLAM中单目、双目/RGB-D初始化的过程来比较两种不同的初始化的方法，由于SLAM的众多方案中ORB-SLAM2提供了单目、双目和RGB-D三种模式，所以本文中以ORB-SLAM2的初始化方法为例进行阐述。由于我的水平实在有限，若文中出现错误望大家指正。
一 、单目SLAM的初始化
单目ORB-SLAM2的初始化步骤如下：
- 匹配初始帧；
- 位姿计算；
- 三角测量和地图创建；
- BA优化。
1 匹配初始帧
这一阶段工作是，通过计算连续两帧图片中能够匹配的特征点数量来判断是否可以作为初始帧，即只有当连续两帧中能够匹配的特征点的数量大于某个值时认为该帧（前一帧）为初始帧；
在ORM-SLAM2中认为连续帧匹配点的数量大于100时可以将前一帧作为初始帧并记录两帧的匹配关系；
以下是在ORB-SLAM2中相关部分的代码，功能为对两帧图片进行ORB特征点提取并进行匹配，当匹配的点的数量大于100时认为前一帧可以作为初始帧；可以通过修改代码中的参数来调整判断是否能够作为初始帧的条件。
int nmatches = matcher.SearchForInitialization(mInitialFrame,mCurrentFrame,mvbPrevMatched,mvIniMatches,100);
2 位姿计算
在得到超过100对匹配点后ORB-SLAM2同时计算适用于平面场景的单应矩阵H和适用于普通场景的基础矩阵F;
方法是：首先由抽样点计算出单次抽样的H（四对点）和F矩阵（八点法）,通过若干次RANSAC抽样计算出最优的H和F矩阵；然后选择最合适的结果作为相机的初始位姿。
2.1 八点法
在对极几何约束中两帧图片中一对特征点,
是空间中一点P在图片中的投影，一对特征点可以确定空间中的一点P，在第一帧的坐标系下设P的空间位置为
在针孔相机模型中两个像素点
,
的像素位置为
其中为对应点的深度，将上式用齐次坐标表示为
在归一化平面上有
其中是归一化平面上
对应的点。根据上式有
对上式做以下数学变换：
t^ =t^
t^
=
t^
因为t^ 是一个t和
都垂直的向量（t^
相当于t同
做外积 ）所以同
做内积时等于0，因此上式等于
t^
=0
将,
代入，有：
t^
=0
以上的两个式子都成为对极约束，几何意义是P,,
共面（
,
是相机中心），对极约束中包括了旋转和平移，将这个式子中的t^ R记作本质矩阵E，将
记作基础矩阵F，因此对极约束可以记为
所以相机位姿估计问题是求解E或者F然后求解R和t。
对于E矩阵认为是一个3*3的矩阵，因为任意常数乘以E不变，所以E矩阵的自由度是8（实际上E矩阵的自由度是5，但是对于SLAM运算中八点法和五点法区别不大且会增加麻烦所以我们只考虑E矩阵的尺度等价性用八点法来计算），从上式可以看出一对点可以确定一个关于E矩阵的方程，8个自由度就需要8对点来求解E矩阵，这就是八点法。
2.2 计算位姿
在同时计算单应矩阵和基础矩阵后对两个模型进行打分选择得分高的那个模型用来位姿计算，打分是用求得的E矩阵和F矩阵将前一帧上的特征点投影到下一帧并将下一帧的特征点投影到前一帧来计算重投影误差的和，代码如下：
cv::Mat Hn = ComputeH21(vPn1i,vPn2i); //计算单应矩阵
currentScore = CheckHomography(H21i, H12i, vbCurrentInliers, mSigma); //进行评分
cv::Mat Fn = ComputeF21(vPn1i,vPn2i);//计算基础矩阵
currentScore = CheckFundamental(F21i, vbCurrentInliers, mSigma);// 进行评分
float RH = SH/(SH+SF);//计算评分比,如果RH>0.4，选择单应矩阵来恢复相机位姿，否则选择基础矩阵来恢复相机位姿。
在计算完E矩阵和F矩阵并确定模型后，如果是本质矩阵E，进行SVD分解会得到4组可能的R，t，对这些解进行检查求出唯一真正的解；如果是单应矩阵进行分解（数值法、解析法）得到4组解，利用先验信息进行排除得到唯一解。
3 三角测量和地图创建
已知位姿后通过三角测量可以计算出特征点对应的深度从而生成点云；并以第一帧为世界坐标系创建地图并进行数据关联。
三角测量
三角测量指通过在两处观察同一个角的夹角从而确定该点的距离。数学上可以从下式进行求解，设是两个特征点的归一化坐标，那么存在
现在已知R，t要求解深度，先求解
,对上式左乘
^：
^
=0=
^
^
上式就变成了求解的方程，求解出
也可以求出。但是由于存在噪声，估计的R和t一般不能使上式等于0，因此一般是求最小二乘解而不是零解。
数据关联
地图点与关键帧关联
一个地图点可被多个关键帧观测到，将观测到这个地图点的关键帧与这个地图点进行关联，同时记录关键帧上哪一个特征点与这个地图点有关联。对于单目初始化来说，地图点需要关联第一步创建的两个关键帧；地图点与关键帧上的特征点关联后，计算最合适的描述子来描述该地图点，用于之后跟踪的匹配。
关键帧与地图点关联
一个关键帧上的特征点由多个地图点投影而成，将关键帧与地图点关联。
关键帧与关键帧关联
关键帧之间会共视一个地图点，如果共视的地图点个数越多，说明这两个关键帧之间的联系越紧密。对于某个关键帧，统计其与其他关键帧共视的特征点个数，如果大于某个阈值，那么将这两个关键帧进行关联。
将关键帧和地图点加入到地图中
4 全局BA优化
在初始化的最后一步对只有两个关键帧的地图进行全局BA优化来优化位姿和观测点，以优化后的结果来重新生成点云地图。
同局部BA优化来最小化重投影误差不同，全局BA优化是在求解观测误差的最小二乘。由于观测误差的最小二乘是非线性的，利用了雅克比矩阵和H矩阵的稀疏性进行边缘化来简化运算，其中也使用了图优化理论。
实际上求解观测误差的最小二乘的过程是力较复杂的，在这里就不多赘述了。
值得一提的是，由于单目没有尺度，因此在地图尺寸初始化时选择生成点深度的中位数作为单位尺寸1当作基准来进行地图的尺寸初始化。
二、双目/RGB-D初始化
由于双目和RGB-D相机不需要通过两个相邻帧来恢复地图点深度，所以初始化过程极其相似，只要当前到来帧满足条件即可开始初始化。双目/RGB-D相机已知若干个特征点的深度（通过双目匹配、结构光或者飞行时间等深度计算方法），可以求解二维点对应的世界坐标系下的空间点，即已知若干个3D空间点及其投影的位置；此时使用PnP来估计相机运动;PnP问题的求解方法有很多种，包括P3P, DLT, EPnP, UPnP, BA等，其中ORM-SLAM2使用的PnP方法是EPnP,具体计算方法在这里不多赘述了。
三、 单目-双目/RGB-D初始化的比较
1.初始化的目的是建立三维的空间点和地图并为之后的计算提供初始值；
2.同双目/RGB-D SLAM不同，单目SLAM无法从一帧图片中计算出深度，因此初始化需要两帧连续满足条件的图片来进行初始化；
3.单目SLAM初始化计算位姿是一个对极约束问题，而双目/RGB-D SLAM的初始化计算位姿是一个PnP问题；
4.在初始化成功后单目SLAM和双目SLAM一样是通过PnP来求解相机位姿的；
5.单目SLAM尺度不确定性的原因是因为在通过SVD分解E矩阵求解R，t时计算的t是没有单位的；
6.单目SLAM初始化的过程中对t进行了归一化来固定尺度，即以求解的初始帧的t为单位1，而后的轨迹和平移都将以这个t为单位；
7.单目SLAM的初始化一定要有一定程度的平移，纯旋转是无法完成初始化的。
