地图初始化过程
SLAM系统要运行一般都会进行一个初始化的操作，在这个过程中确立坐标系，估计机器人的初始位姿，建立初始的地图。 对于单目相机而言由于尺度的缺失，仅通过单幅图像是不能进行初始化的，需要两帧具有一定视差的图像来完成这一任务。 而双目和深度相机，由于可以直接获取深度信息，所以不存在尺度不确定性的问题，只要一帧的特征点足够多就可以直接完成初始化。
针对单目相机，ORB的作者提出了一种自动化的初始化策略，不需要像它的前身PTAM那样人为的选择两个具有较大视差的帧，使用起来十分方便。针对平面场景(planar scene)和非平面场景(non-planar scene)， ORB-SLAM2提供了单应(homography)和基础矩阵(fundamental matrix)两种几何模型。它会在两个线程中同时计算这两个模型，然后根据一些启发式信息选取一个较合适的模型。
本文中，我们重点分析单目初始化的基本原理及其实现方式，并简单讨论一下深度相机的初始化过程。 先介绍单目相机的帧间位姿估计方法， 再结合代码分析 ORB-SLAM2 中单目相机初始化的总体过程， 然后详细介绍单应矩阵和基础矩阵的计算方法，
1. 单目相机的帧间位姿估计方法
虽然从单幅图像中无法得到距离信息，但是我们可以拿着相机运动，研究运动前后两帧图像中某些点的映射关系，还是可以估计出相机运动的。 我们把前后帧相机的相对运动关系称为帧间位姿，用符号\(\langle \boldsymbol{R}, \boldsymbol{t} \rangle\)来表示。 其中\(\boldsymbol{R}\)是相机的旋转矩阵，\(\boldsymbol{t}\)则是平移向量。
基于对极约束，ORB-SLAM2 计算出了相机的基础矩阵 \(\boldsymbol{F}\)，进而求得帧间的相对位姿变换关系。 这种对级几何关系描述的是两个视角内在的映射关系，具有一般性。但在一些情况下，比如说相机只有旋转运动，图像中存在大面积的平面， 基础矩阵会出现退化，容易受到测量噪声的干扰。针对基础矩阵的一些缺陷，有时我们还会基于摄影变换关系，估计帧间的单应矩阵 \(\boldsymbol{H}\)， 通过该矩阵也可以给出一个帧间的位姿变换关系。
针对单目相机只要我们估计出前两帧的帧间位姿关系，就可以确立一个参考尺度，进而完成地图的初始化。 在研究代码之前，让我们先准备一点理论知识，搞明白基础矩阵和单应矩阵是如何计算的，如何估计出帧间位姿的吧。
1.1 对级约束与基础矩阵
如右图所示，我们拿着相机分别在点\(\boldsymbol{C}\)和\(\boldsymbol{C'}\)观测空间中一点\(\boldsymbol{P}\)，该点在两个视图平面上分别被投影到了点\(\boldsymbol{x}\)和\(\boldsymbol{x'}\)。 显然，由于两次测量都是对同一个点\(\boldsymbol{P}\)的观测，所以\(\boldsymbol{Cx}\)和\(\boldsymbol{C'x'}\)的延长线相交与点\(\boldsymbol{P}\)，即点\(\boldsymbol{P}, \boldsymbol{C}, \boldsymbol{C'}, \boldsymbol{x}, \boldsymbol{x'}\)都在一个平面上，这个平面被称为极面(epipolar plane)，用符号\(\boldsymbol{\pi}\)来表示， 连线\(\boldsymbol{CC'}\)被称为基线(baseline)，基线分别与两个像平面相交与点\(\boldsymbol{e}, \boldsymbol{e'}\)，这两个点被称为极点(epipole)， 它们分别与成像点\(\boldsymbol{x}, \boldsymbol{x'}\)的连线\(\boldsymbol{xe}, \boldsymbol{x'e'}\)所在的直线被称为极线(epipolar line)。
假设 \(\boldsymbol{P}\) 相对于第一帧相机坐标系的坐标为\(\begin{bmatrix} x & y & z \end{bmatrix}^T\)， 成像点\(\boldsymbol{x}\)的齐次坐标为\(\begin{bmatrix} u & v & 1 \end{bmatrix}^T\)，根据针孔相机模型，我们能够写出如下的投影关系：
$$ \begin{equation}\label{f1} \boldsymbol{x} = \cfrac{1}{z} \boldsymbol{KP} \Longleftrightarrow \begin{bmatrix} u \\ v \\ 1 \end{bmatrix} = \cfrac{1}{z} \begin{bmatrix} f_x & 0 & c_x \\ 0 & f_y & c_y \\ 0 & 0 & 1 \end{bmatrix} \begin{bmatrix} x \\ y \\ z \end{bmatrix} \end{equation} $$其中矩阵\(K\)为相机的内参矩阵，记录了相机的xy轴上的焦距\(f_x, f_y\)和光心坐标\(c_x, c_y\)。 相机经过位姿变换\(\langle \boldsymbol{R}, \boldsymbol{t} \rangle\)后，观测到\(\boldsymbol{P}\)点坐标为：
$$ \begin{bmatrix} x_2 \\ y_2 \\ z_2 \end{bmatrix} = \boldsymbol{R}\begin{bmatrix} x \\ y \\ z \end{bmatrix} + \boldsymbol{t} $$记此时成像点\(\boldsymbol{x'}\)的齐次坐标为\(\begin{bmatrix} u' & v' & 1 \end{bmatrix}^T\)，我们有投影关系:
$$ \begin{equation}\label{f2} \boldsymbol{x'} = \cfrac{1}{z_2} \boldsymbol{K}\left(\boldsymbol{RP} + \boldsymbol{t}\right) \Longleftrightarrow \begin{bmatrix} u' \\ v' \\ 1 \end{bmatrix} = \cfrac{1}{z_2} \begin{bmatrix} f_x & 0 & c_x \\ 0 & f_y & c_y \\ 0 & 0 & 1 \end{bmatrix} \begin{bmatrix} x_2 \\ y_2 \\ z_2 \end{bmatrix} \end{equation} $$将式(\(\ref{f1}\))改写成 \(\boldsymbol{P} = z\boldsymbol{K}^{-1}\boldsymbol{x}\) 之后代入式(\(\ref{f2}\))整理后有：
$$ \begin{array}{cl} & z_2 \boldsymbol{K}^{-1} \boldsymbol{x'} = z \boldsymbol{R}\boldsymbol{K}^{-1}\boldsymbol{x} + \boldsymbol{t} \\ \Leftrightarrow & z_2 [\boldsymbol{t}]_{\times}\boldsymbol{K}^{-1}\boldsymbol{x'} = z [\boldsymbol{t}]_{\times}\boldsymbol{R}\boldsymbol{K}^{-1}\boldsymbol{x} \\ \Leftrightarrow & z_2 \left(\boldsymbol{K}^{-1}\boldsymbol{x'}\right)^T[\boldsymbol{t}]_{\times}\boldsymbol{K}^{-1}\boldsymbol{x'} = z \left(\boldsymbol{K}^{-1}\boldsymbol{x'}\right)^T [\boldsymbol{t}]_{\times}\boldsymbol{R}\boldsymbol{K}^{-1}\boldsymbol{x} \\ \Leftrightarrow & z \boldsymbol{x'}^T\boldsymbol{K}^{-T}[\boldsymbol{t}]_{\times}\boldsymbol{R}\boldsymbol{K}^{-1}\boldsymbol{x} = 0 \end{array} $$上式就是一个等式为零的约束，所以其中\(z\)的取值实际上没有什么作用，只要非零就行。 我们把矩阵\(\boldsymbol{K}^{-T}[\boldsymbol{t}]_{\times}\boldsymbol{R}\boldsymbol{K}^{-1}\)称为基础矩阵， 用符号\(\boldsymbol{F}\)表示。\(\boldsymbol{K}\)是相机的内参矩阵，可以通过标定得到。 \([\boldsymbol{t}]_{\times}\)是相机平移向量\(\boldsymbol{t}\)构成的斜对称矩阵， 相当于向量的叉积运算。 \([\boldsymbol{t}]_{\times}\boldsymbol{R}\)被称为本征矩阵(Essential Matrix)，记为\(\boldsymbol{E}\)。上式可以简记如下，被称为对极约束。
$$ \begin{equation}\label{f3} \begin{array}{c} \boldsymbol{x'}^T \boldsymbol{Fx} = 0 \\ \boldsymbol{x'}^T \boldsymbol{K}^{-T}\boldsymbol{E}\boldsymbol{K}^{-1}\boldsymbol{x} = 0 \\ \end{array} \end{equation} $$我们主要关注这里的本征矩阵\(\boldsymbol{E}\)，它是一个\(3\times 3\)的矩阵，包含了相机的所有变换信息。虽然它有9个未知参数，但由于对极约束是一个等零的约束， 所以该矩阵乘上任何一个非零标量都不会改变约束。因此，这个本征矩阵是缺少尺度信息的。我们只需要提供8个匹配的特征点代入上式，解零空间就可以求得\(\boldsymbol{E}\)。 再对其进行奇异值分解后就可以获得平移向量\(\boldsymbol{t}\)和旋转矩阵\(\boldsymbol{R}\)。 只要图像的纹理丰富一些，我们就可以得到很多匹配的特征点对，这样就可以通过 RANSAC 的方式估计帧间位姿，在一定程度上提高估计结果的可靠性。
基础矩阵或者说是本征矩阵的几何意义就是，在两个不同的位置上观测空间中同一个点，其成像一定在极线上。 也就说对极约束只能把一个点约束到一条直线上，并不能建立起两个成像点的一一对应关系。 这也正是基础矩阵和本矩阵缺少尺度信息的一个结果。但是在一些特定的场景下，我们还是可以得到匹配点的一一对应关系的。比如下面要介绍的平面图像和单应矩阵。
1.2 摄影变换和单应矩阵
很多时候为了计算方便，我们会假设相机采集的图像就是一个平面，比如说安装在无人机上对地观测的相机、安装在机器人上对着天花板的相机等等。从不同的视角观测同一个平面的过程， 可以通过摄影变换的形式建立起两幅图像之间各点的一一对应关系。 这个映射关系可以用一个\(3 \times 3\)的齐次矩阵\(\boldsymbol{H}\)来表示，我们称之为单应矩阵。
假设观测的平面\(\alpha\)在第一帧相机坐标系的法向量为\(\boldsymbol{N}\)，到相机的距离为\(d\)， 记\(\alpha\)上一点\(P\)在第一帧相机坐标系下的坐标记为\(\boldsymbol{X_1} = \begin{bmatrix} x_1 & y_1 & z_1 \end{bmatrix}^T\)，按照平面的点法式方程， 可以得到如下的关系：
$$ \boldsymbol{N}^T\boldsymbol{X_1} = d \Longrightarrow \cfrac{1}{d}\boldsymbol{N}^T\boldsymbol{X_1} = 1 $$若相机发生了\(\langle \boldsymbol{R}, \boldsymbol{t} \rangle\)的变换后，\(P\)点在新的相机坐标系下的坐标为\(\boldsymbol{X_2} = \begin{bmatrix} x_2 & y_2 & z_2 \end{bmatrix}^T\)， 那么有运动关系：
$$ \begin{equation}\label{f_frame} \begin{array}{rl} & \boldsymbol{X_2} = \boldsymbol{RX_1} + \boldsymbol{t} \\ \Rightarrow & \boldsymbol{X_2} = \boldsymbol{RX_1} + \boldsymbol{t}\left( \cfrac{1}{d}\boldsymbol{N}^T\boldsymbol{X_1} \right) \\ \Rightarrow & \boldsymbol{X_2} = \left(\boldsymbol{R} + \boldsymbol{t}\cfrac{1}{d}\boldsymbol{N}^T\right)\boldsymbol{X_1} \end{array} \end{equation} $$考虑相机的内参，根据式\((\ref{f1})\)，将\(\boldsymbol{X_1}, \boldsymbol{X_2}\)投影到成像平面上有：
$$ \begin{equation}\label{f_homo_project} \begin{cases} \boldsymbol{x} = \cfrac{1}{z_1} \boldsymbol{K} \boldsymbol{X_1} \\ \boldsymbol{x'} = \cfrac{1}{z_2} \boldsymbol{K} \boldsymbol{X_2} \end{cases} \Rightarrow \begin{cases} \boldsymbol{X_1} = z_1\boldsymbol{K}^{-1} \boldsymbol{x} \\ \boldsymbol{X_2} = z_2\boldsymbol{K}^{-1} \boldsymbol{x'} \end{cases} \end{equation} $$其中，\(\boldsymbol{x}, \boldsymbol{x'}\) 分别是 \(\boldsymbol{X_1}, \boldsymbol{X_2}\) 在图像中的像素齐次坐标， 将上式\((\ref{f_homo_project})\)代入式\((\ref{f_frame})\)，有：
$$ \begin{equation}\label{f_homography_dlt} \begin{array}{rl} & z_2\boldsymbol{K}^{-1}\boldsymbol{x'} = z_1\left(\boldsymbol{R} + \boldsymbol{t}\cfrac{1}{d}\boldsymbol{N}^T\right)\boldsymbol{K}^{-1}\boldsymbol{x} \\ \Rightarrow & \boldsymbol{x'} = \cfrac{z_1}{z_2}\boldsymbol{K}\left(\boldsymbol{R} + \boldsymbol{t}\cfrac{1}{d}\boldsymbol{N}^T\right)\boldsymbol{K}^{-1}\boldsymbol{x} \end{array} \end{equation} $$令\(\boldsymbol{H} = \boldsymbol{K}\left(\boldsymbol{R} + \boldsymbol{t}\cfrac{1}{d}\boldsymbol{N}^T\right)\boldsymbol{K}^{-1}\)，即是两幅图像的单应矩阵。 这个单应矩阵建立了两个图像之间的映射关系 \(\boldsymbol{x'} = s\boldsymbol{H}\boldsymbol{x}\)。 其中，\(s = z_1 / z_2 \)是两幅图像之间的尺度因子，并没有体现在单应矩阵中。所以这种映射模型同样存在尺度缺失的问题，由于相机内参可以通过标定获得， 所以只需要计算\(\boldsymbol{H_{\alpha}} = \boldsymbol{R} + \boldsymbol{t}\cfrac{1}{d}\boldsymbol{N}^T\)即可通过奇异值分解的方法进一步的计算出相机的位姿。 与基础矩阵和本征矩阵类似，我们可以通过匹配的特征点来计算该矩阵。下面我们结合代码，深入了解一下单目相机的初始化过程。
2. 单目相机初始化的总体过程
void Tracking::Track() {
if(mState==NO_IMAGES_YET)
mState = NOT_INITIALIZED;
mLastProcessedState=mState;
unique_lock lock(mpMap->mMutexMapUpdate);
if(mState==NOT_INITIALIZED) {
if(mSensor==System::STEREO || mSensor==System::RGBD)
StereoInitialization();
else
MonocularInitialization();
mpFrameDrawer->Update(this);
if(mState!=OK)
return;
}
初始化的最基本任务就是建立坐标系，估计机器人的初始位姿，创建初始的地图。对于单目相机而言，由于尺度的缺失，还需要建立一个基础的尺度，以后的地图重建都是在这个基础尺度上进行的。
在分析轨迹跟踪的总体过程时，我们看到 ORB-SLAM2 在类 Tracking 的成员函数 Track() 完成了地图的初始化工作。 如右侧的代码片段所示，当检查状态 mState 发现尚未进行初始化的时候，针对单目相机，将通过函数 MonocularInitialization() 完成初始化操作。
接下来我们将详细分析函数 MonocularInitialization()，将会看到 ORB-SLAM 会选择两帧具有足够多的匹配特征点和视差的图像，把相对较早的那帧图像当作参考帧进行初始化。 解算出两帧图像下相机的相对位姿变换，以参考帧的相机坐标系为基础建立世界坐标。在该坐标系下相机的初始位姿为单位矩阵，即没有旋转和位移。 它还会对匹配的特征点三角化，估计相对的空间点坐标，并以参考帧下特征点深度的中位数为标准，建立基础尺度。进而构建地图点，建立初始地图。
2.1 初始化的总体业务流程
void Tracking::MonocularInitialization() {
if (!mpInitializer) {
if(mCurrentFrame.mvKeys.size() > 100) {
mInitialFrame = Frame(mCurrentFrame);
mLastFrame = Frame(mCurrentFrame);
mvbPrevMatched.resize(mCurrentFrame.mvKeysUn.size());
for(size_t i=0; i < mCurrentFrame.mvKeysUn.size(); i++)
mvbPrevMatched[i]=mCurrentFrame.mvKeysUn[i].pt;
mpInitializer = new Initializer(mCurrentFrame,1.0,200);
fill(mvIniMatches.begin(),mvIniMatches.end(),-1);
}
return;
}
右侧是函数 MonocularInitialzation() 的代码片段。指针 mpInitializer 是类 Tracking 中为了支持单目相机初始化而定义的对象。在函数的一开始先检查该指针， 如果它是个空指针就以当前帧作为参考帧实例化对象 mpInitializer。为了保证初始化的质量，只有当图像的特征点数量超过了100个，才进行初始化操作。
如片段中第4、5行所示，在实例化 mpInitializer 之前，先将暂时将当前帧标记为初始帧，并用指针 mInitialFrame 记录，同时用mLastFrame记录当前帧。 此外，还要把当前帧中提取的矫畸变(undistorted)后的特征点保存到容器 mvbPrevMatched 中，如6到8行所示。 然后在第9行中以当前帧为参数构造初始化器，最后将mvIniMatches中所有的特征点匹配值都置为-1，表示它们都还没有匹配。
如果初始化对象已经被具例化了，说明我们已经有了一个参考帧。如下面左侧代码片段所示，此时我们再次检查当前帧的特征点数量。只有特征点足够多(> 100)，才认为找到了用于初始化的第二帧图像。 否则将销毁 mpInitializer 重新记录初始帧。 为排版紧凑一些，我们增加了下面右侧的函数 FreeInitializer。
|
|
然后，在下面的第 18 行中构建了一个 ORB 特征点匹配器，通过接口 SearchForInitialization 针对初始化进行特征匹配。 如果匹配点数量少于 100 个，认为当前帧和初始帧不适合初始化，将销毁 mpInitializer 重新记录初始帧。 类 Tracking 的成员变量 mvIniMatches 中记录了初始帧中各个特征点与当前帧匹配的特征点索引关系。
ORBmatcher matcher(0.9,true);
int nmatches = matcher.SearchForInitialization(mInitialFrame,mCurrentFrame,mvbPrevMatched,mvIniMatches,100);
if(nmatches < 100) {
FreeInitializer();
return;
}
通过了层层考验之后，我们通过对象 mpInitializer 的接口 Initialize() 完成初始化操作，估计相机的旋转量和平移量，并对特征点进行三角化形成初始的地图点。 这些结果将被保存在Rcw, tcw, mvIniP3D, vbTriangulated中。其中 vbTriangulated 是与 mvIniMatches 一一对应的布尔型数组， 用于记录对应的匹配特征点对是否成功三角化了。下面这个 for 循环用来剔除匹配点对中没有成功三角化的点对。 这个剔除的过程完全可以放到 mpInitializer->Initialize 中进行。
cv::Mat Rcw; // Current Camera Rotation
cv::Mat tcw; // Current Camera Translation
vector<bool> vbTriangulated; // Triangulated Correspondences (mvIniMatches)
if(mpInitializer->Initialize(mCurrentFrame, mvIniMatches, Rcw, tcw, mvIniP3D, vbTriangulated)) {
for(size_t i=0, iend=mvIniMatches.size(); i < iend;i++) {
if(mvIniMatches[i] >=0 && !vbTriangulated[i]) {
mvIniMatches[i]=-1;
nmatches--;
}
}
成功的完成了初始化之后，我们就得到了当前帧相对于参考帧的相机变换关系。此时我们可以把参考帧的相机位姿当作原点构建世界坐标系，以后的相机位姿估计都是相对于该坐标系的。 所以这里设定初始帧的齐次矩阵为单位阵，并用刚刚估计的旋转矩阵和平移向量构建当前帧的齐次矩阵。最后，调用函数CreateInitialMapMonocular来完成地图的初始化工作。
mInitialFrame.SetPose(cv::Mat::eye(4,4,CV_32F));
cv::Mat Tcw = cv::Mat::eye(4,4,CV_32F);
Rcw.copyTo(Tcw.rowRange(0,3).colRange(0,3));
tcw.copyTo(Tcw.rowRange(0,3).col(3));
mCurrentFrame.SetPose(Tcw);
CreateInitialMapMonocular();
} }
2.2 类 Initializer
我们已经看到，单目相机的初始化专门有一个类 Initializer(.h, .cc)来完成。下面折叠的代码中列举了它的所有成员变量， 主要是记录了相机内参、参考帧与当前帧的特征点和匹配关系、RANSAC 算法的迭代参数和样本组。
cv::Mat mK; // 相机内参矩阵 3x3
std::vector<cv::KeyPoint> mvKeys1; // 参考帧的矫校畸变后的特征点 (Frame 1)
std::vector<cv::KeyPoint> mvKeys2; // 当前帧的矫畸变后的特征点 (Frame 2)
typedef std::pair<int,int> Match; // 匹配点对, first 参考帧，second 当前帧
std::vector<Match> mvMatches12; // 匹配点对
std::vector<bool> mvbMatched1; // 对应参考帧中每个特征点，在当前帧中是否有匹配点
float mSigma; // 标准差
float mSigma2; // 方差
int mMaxIterations; // RANSAC 最大迭代次数
std::vector<std::vector<size_t>> mvSets; // RANSAC 样本组集合
如下面的代码所示，它的构造函数有三个参数，其中 ReferenceFrame 是用于初始化的参考帧，sigma 和 iterations 是两个运行参数，分别定义了标准差和最大迭代次数。 在函数体中还完成了mK和mvKeys1的赋值，它们分别是相机的内参矩阵和参考帧中提取的特征点。
Initializer::Initializer(const Frame &ReferenceFrame, float sigma, int iterations) {
mK = ReferenceFrame.mK.clone();
mvKeys1 = ReferenceFrame.mvKeysUn;
mSigma = sigma;
mSigma2 = sigma*sigma;
mMaxIterations = iterations;
}
bool Initializer::Initialize(const Frame & CurrentFrame,
const vector<int> &vMatches12, cv::Mat &R21, cv::Mat &t21,
std::vector<cv::Point3f> &vP3D, std::vector<bool> &vbTriangulated)
{
mvKeys2 = CurrentFrame.mvKeysUn;
mvMatches12.clear();
mvMatches12.reserve(mvKeys2.size());
mvbMatched1.resize(mvKeys1.size());
for(size_t i=0, iend=vMatches12.size(); i < iend; i++) {
if(vMatches12[i] >= 0) {
mvMatches12.push_back(make_pair(i,vMatches12[i]));
mvbMatched1[i]=true;
} else
mvbMatched1[i]=false;
}
实际的初始化过程都是由成员函数 Initialize 完成的，如右侧的代码片段所示，它有6个参数，其中 CurrentFrame 是当前帧，vMatches12 中记录了两帧特征点匹配关系。 vMatches12 与参考帧的特征点一一对应，记录了当前帧中匹配的特征点索引。 若成功完成了初始化，将通过参数 R21 和 t21 输出当前帧相对于参考帧的姿态矩阵和平移向量。 输出参数 vP3D 中记录了所有三角化的地图点，vbTriangulated 记录匹配的特征点是否被三角化了。
在函数的一开始，使用成员变量 mvKeys2 记录下当前帧中的特征点。然后遍历输入参数 vMatches12，将匹配点对应关系记录到成员容器 mvMatches12 中， 同时用 mvbMatched1 标记参考帧中的特征点是否有匹配对象。
特征点是很容易误匹配的，但我们并没有太好的手段把这些误匹配的特征点剔除掉。错误的匹配点对势必会对相机的位姿估计产生负面的影响。 RANSAC 算法能够有效的降低负面影响。它通过随机采样的方法，每次从样本集中只取出能够解决问题的最少样本，求解位姿。 求解单应矩阵只需要4个匹配点对，基础矩阵则需要8个。对样本组输出的相机位姿计算一个损失函数。多次随机采样之后，选取损失函数最小的那组样本的解作为最后的相机位姿输出。
一般情况下，误匹配的特征点还是很少的。RANSAC 随机采样最少的样本求解方程，很大概率上能够找到一组匹配点对集合，不存在误匹配，相比于那些有误匹配的样本组应当更准确， 损失函数值也就越低。在 ORB-SLAM2 中，这一 RANSAC 过程被分为两个部分来执行了。首先对所有的匹配点对进行随机采样，准备样本组集合。由 mvSets 记录了所有 RANSAC 样本组，每个样本组都有8个特征点， 如下面的代码片段所示。这一段代码有着比较明确的计算目的，感觉封装到一个函数里会比较好一些。
const int N = mvMatches12.size();
// Indices for minimum set selection
std::vector<size_t> vAllIndices;
vAllIndices.reserve(N);
std::vector<size_t> vAvailableIndices;
for(int i = 0; i < N; i++)
vAllIndices.push_back(i);
// Generate sets of 8 points for each RANSAC iteration
mvSets = std::vector<std::vector<size_t>>(mMaxIterations,vector<size_t>(8,0));
DUtils::Random::SeedRandOnce(0);
for(int it = 0; it < mMaxIterations; it++) {
vAvailableIndices = vAllIndices;
// Select a minimum set
for(size_t j = 0; j < 8; j++) {
int randi = DUtils::Random::RandomInt(0, vAvailableIndices.size()-1);
int idx = vAvailableIndices[randi];
mvSets[it][j] = idx;
vAvailableIndices[randi] = vAvailableIndices.back();
vAvailableIndices.pop_back();
}
}
然后在两个线程中，完成 RANSAC 的求解过程，计算单应矩阵和基础矩阵，记录到局部变量 H, F 中。 局部变量 SH 和 SF 分别记录了两种计算模型解的损失函数值。具体的求解过程，我们将在后文分析函数 FindHomography 和 FindFundamental 时详细介绍。
vector<bool> vbMatchesInliersH, vbMatchesInliersF;
float SH, SF;
cv::Mat H, F;
thread threadH(&Initializer::FindHomography,this,ref(vbMatchesInliersH), ref(SH), ref(H));
thread threadF(&Initializer::FindFundamental,this,ref(vbMatchesInliersF), ref(SF), ref(F));
threadH.join();
threadF.join();
最后按照ORB-SLAM论文的说法， 根据启发式规则参数 \(R_H = \cfrac{S_H}{S_H + S_F}\) 来选择进行初始化的模型，并调用 ReconstructH 和 ReconstructF 从单应矩阵或者基础矩阵中恢复出相机的位姿。
float RH = SH/(SH+SF);
if(RH > 0.40)
return ReconstructH(vbMatchesInliersH,H,mK,R21,t21,vP3D,vbTriangulated,1.0,50);
else //if(pF_HF > 0.6)
return ReconstructF(vbMatchesInliersF,F,mK,R21,t21,vP3D,vbTriangulated,1.0,50);
return false;
}
3.3 地图初始化
现在，我们已经成功的估计了相机的位姿，并对匹配的特征点进行了三角化得到了它们的世界坐标。接下来就需要调用函数CreateInitialMapMonocular来完成世界地图的初始化工作。 下面是这个函数代码片段，一开始先根据参考帧和当前帧创建两个关键帧(KeyFrame)。
void Tracking::CreateInitialMapMonocular() {
KeyFrame* pKFini = new KeyFrame(mInitialFrame,mpMap,mpKeyFrameDB);
KeyFrame* pKFcur = new KeyFrame(mCurrentFrame,mpMap,mpKeyFrameDB);
然后更新词袋，并将这两个关键帧插入地图中。
pKFini->ComputeBoW();
pKFcur->ComputeBoW();
mpMap->AddKeyFrame(pKFini);
mpMap->AddKeyFrame(pKFcur);
接着在一个for循环中根据成功三角化的特征点创建地图点(MapPoint)，并建立起地图点与关键帧之间的可视关系。
for (size_t i=0; i < mvIniMatches.size(); i++) {
if(mvIniMatches[i] < 0)
continue;
cv::Mat worldPos(mvIniP3D[i]);
MapPoint* pMP = new MapPoint(worldPos,pKFcur,mpMap);
// 省略建立各种关联关系的语句
mpMap->AddMapPoint(pMP);
}
此时，我们就可以根据地图点和关键帧之间的关系提取出共视图(covisibility graph)和基图(essential graph)了。下面两句话用于更新关键帧的连接关系。
pKFini->UpdateConnections();
pKFcur->UpdateConnections();
最后进行一次全局的BA优化，就完成了初始地图的构建。 论文中的初始化介绍也就到此为止了。
Optimizer::GlobalBundleAdjustemnt(mpMap,20);
看起来相机的位姿也估计了，地图也建立了，故事应该结束了。但是对于单目而言尺度仍然是缺失的，所以函数CreateInitialMapMonocular还进一步的以当前地图点深度的中位数为基础建立基础尺度， 以后的地图重建工作都是在这个基础的尺度上进行的。下面的代码片段中，先获取了初始关键帧下的深度中位数，并进一步的检查该深度以及当前关键帧跟踪上的地图点数量。
float medianDepth = pKFini->ComputeSceneMedianDepth(2);
float invMedianDepth = 1.0f/medianDepth;
if(medianDepth < 0 || pKFcur->TrackedMapPoints(1) < 100) {
cout << "Wrong initialization, reseting..." << endl;
Reset();
return;
}
最后初始化基线和地图点的尺度。至此，我们才算完成了初始化的操作，建立了基础尺度和初始地图。
// Scale initial baseline
cv::Mat Tc2w = pKFcur->GetPose();
Tc2w.col(3).rowRange(0,3) = Tc2w.col(3).rowRange(0,3)*invMedianDepth;
pKFcur->SetPose(Tc2w);
// Scale points
vector<MapPoint*> vpAllMapPoints = pKFini->GetMapPointMatches();
for(size_t iMP=0; iMP < vpAllMapPoints.size(); iMP++) {
if(vpAllMapPoints[iMP]) {
MapPoint* pMP = vpAllMapPoints[iMP];
pMP->SetWorldPos(pMP->GetWorldPos()*invMedianDepth);
}
}
// 省略Tracking的各种成员的更新
}
4. 深度相机的初始化过程
在rgbd_tum.cc中，ORB通过SLAM.TrackRGBD(imRGB, imD, tframe);喂数据。
这个TrackRGBD函数就是用来跟踪深度相机位姿的，下面是它的代码片段，
它有三个参数分别记录了相机拍摄的RGB图像、深度图、和拍摄时的时间戳。在函数之初先检查一下传感器类型，若不是深度相机则报错退出。
和单目相机的初始化过程类似，先检查系统的工作状态并相应的做处理，
在将图像和时间戳数据提供给轨迹跟踪器mTracker进行处理，得到描述相机位姿的齐次矩阵。
cv::Mat System::TrackRGBD(const cv::Mat &im, const cv::Mat &depthmap, const double ×tamp) {
if(mSensor != RGBD) {
cerr << "ERROR: you called TrackRGBD but input sensor was not set to RGBD.." << endl;
exit(-1);
}
// 省略对工作模式和复位状态检查和处理
cv::Mat Tcw = mpTracker->GrabImageRGBD(im,depthmap,timestamp);
// 省略状态和成员变量更新
return Tcw;
}
下面是轨迹跟踪器(.h, .cc)的成员函数GrabImageRGBD的代码片段，同样有三个参数分别记录了RGB图像、深度图和时间戳。 在函数的一开始根据RGB图像的通道数以及存储方式，调用OpenCV的函数将彩色图像转换为灰度图。如果深度图的数据不是32位浮点数，或者提供了深度图尺度系数，就根据该系数将之转换为实际的物理世界尺度。 然后通过ORB特征提取器mpORBextractorLeft，将图像和时间戳转换为当前帧(mCurrentFrame)，并调用Track函数跟踪相机轨迹，并返回当前相机位姿。
cv::Mat Tracking::GrabImageRGBD(const cv::Mat &imRGB,const cv::Mat &imD, const double ×tamp) {
mImGray = imRGB;
cv::Mat imDepth = imD;
// 彩色图转灰度图
if(mImGray.channels()==3) {
if(mbRGB)
cvtColor(mImGray,mImGray,CV_RGB2GRAY);
else
cvtColor(mImGray,mImGray,CV_BGR2GRAY);
} else if(mImGray.channels()==4) {
if(mbRGB)
cvtColor(mImGray,mImGray,CV_RGBA2GRAY);
else
cvtColor(mImGray,mImGray,CV_BGRA2GRAY);
}
// 调整深度图尺度
if((fabs(mDepthMapFactor-1.0f) > 1e-5) || imDepth.type() != CV_32F)
imDepth.convertTo(imDepth,CV_32F,mDepthMapFactor);
mCurrentFrame = Frame(mImGray,imDepth,timestamp,mpORBextractorLeft,mpORBVocabulary,mK,mDistCoef,mbf,mThDepth);
Track();
return mCurrentFrame.mTcw.clone();
}
地图的初始工作在Track函数中完成了。实际上无论单目双目还是深度相机，最后都是通过Track函数完成初始化，并进行视觉里程计跟踪的。如下面的代码片段所示， 当检查mState仍处于NOT_INITIALIZED的状态的时候，就是根据传感器类型完成初始化。在ORB-SLAM中深度相机的数据是通过模拟双目来完成定位建图的，所以这里也是调用StereoInitialization完成初始化的。
if(mState==NOT_INITIALIZED) {
if(mSensor==System::STEREO || mSensor==System::RGBD)
StereoInitialization();
else
MonocularInitialization();
mpFrameDrawer->Update(this);
if(mState!=OK)
return;
}
下面是StereoInitialization的代码片段。首先检查当前帧的特征点数量，如果太少就放弃了。然后将当前帧的姿态设定到原点上，并以此构建关键帧添加到地图对象mpMap中。
void Tracking::StereoInitialization() {
if (mCurrentFrame.N <= 500)
return;
mCurrentFrame.SetPose(cv::Mat::eye(4,4,CV_32F));
KeyFrame* pKFini = new KeyFrame(mCurrentFrame,mpMap,mpKeyFrameDB);
mpMap->AddKeyFrame(pKFini);
然后，检查当前帧的特征点，如果有深度信息，就依此信息将之还原到3D物理世界中，新建地图点并将之与关键帧关联上。
for(int i = 0; i < mCurrentFrame.N; i++) {
float z = mCurrentFrame.mvDepth[i];
if (z > 0) {
cv::Mat x3D = mCurrentFrame.UnprojectStereo(i);
MapPoint* pNewMP = new MapPoint(x3D,pKFini,mpMap);
pNewMP->AddObservation(pKFini,i);
pKFini->AddMapPoint(pNewMP,i);
pNewMP->ComputeDistinctiveDescriptors();
pNewMP->UpdateNormalAndDepth();
mpMap->AddMapPoint(pNewMP);
mCurrentFrame.mvpMapPoints[i]=pNewMP;
}
}
cout << "New map created with " << mpMap->MapPointsInMap() << " points" << endl;
此时就已经完成了地图的初始构建工作，接下来就是更新系统的相关变量和状态。
mpLocalMapper->InsertKeyFrame(pKFini);
mLastFrame = Frame(mCurrentFrame);
mnLastKeyFrameId=mCurrentFrame.mnId;
mpLastKeyFrame = pKFini;
mvpLocalKeyFrames.push_back(pKFini);
mvpLocalMapPoints=mpMap->GetAllMapPoints();
mpReferenceKF = pKFini;
mCurrentFrame.mpReferenceKF = pKFini;
mpMap->SetReferenceMapPoints(mvpLocalMapPoints);
mpMap->mvpKeyFrameOrigins.push_back(pKFini);
mpMapDrawer->SetCurrentCameraPose(mCurrentFrame.mTcw);
mState=OK;
}
5. 完
对于单目相机整个初始化过程可以总结为六个步骤：
- 计算两帧图像的ORB特征点，并进行匹配；
- 在两个线程中以RANSAC策略并行的计算单应矩阵和基础矩阵；
- 根据评判标准在单应矩阵和基础矩阵之间选择一个模型；
- 根据选定的模型分解相机的旋转矩阵和平移向量，并对匹配的特征点进行三角化；
- 建立关键帧和地图点，进行完全BA(full BA)优化；
- 以参考帧下深度的中位数为基准建立基础尺度；
对于双目和深度相机而言，由于不需要确定基础尺度，可以直接依据深度信息，来轨迹特征点在3D世界中的位姿，并完成地图的初始构建工作。