ORB-SLAM3 细读单目初始化过程（终结篇）

本文原创，转载请说明地址：https://blog.csdn.net/shanpenghui/article/details/110522368

一、前言

请阅读本文之前最好把ORB-SLAM3的单目初始化过程再过一遍，以提高学习效率。单目初始化过程中最重要的是两个函数实现，分别是构建帧（Frame）和初始化（Track）。接下来，就是完成初始化过程的最后一步：地图的初始化，是由CreateInitialMapMonocular函数完成的，本文基于该函数的流程出发，目的是为了结合代码流程，把单目初始化的上下两篇的知识点和ORB-SLAM3整个系统的知识点串联起来，系统化零碎的知识，告诉你平时学到的各个小知识应用在SLAM系统中的什么位置，达到快速高效学习的效果。

二、CreateInitialMapMonocular 函数的总体流程

1. 将初始关键帧，当前关键帧的描述子转为BoW；
2. 将关键帧插入到地图；
3. 用三角测量初始化得到的3D点来生成地图点，更新关键帧间的连接关系；
4. 全局BA优化，同时优化所有位姿和三维点；
5. 取场景的中值深度，用于尺度归一化；（和《3-3-3 尺度不变性》呼应，用到尺度不变性原理）
6. 将两帧之间的变换归一化到平均深度1的尺度下；
7. 把3D点的尺度也归一化到1；
8. 将关键帧插入局部地图，更新归一化后的位姿、局部地图点。

三、必备知识

1. 为什么单目需要专门策略生成初始化地图

根据论文《ORB-SLAM: a Versatile and Accurate Monocular SLAM System》，即ORB-SLAM1的论文（中文翻译版ORB-SLAM: a Versatile and Accurate Monocular SLAM System）可知：

1. 单目SLAM系统需要设计专门的策略来生成初始化地图，这也是为什么代码中单独设计一个CreateInitialMapMonocular()函数来实现单目初始化，也是我们这篇文章要讨论的。为什么要单独设计呢？就是因为单目没有深度信息。
2. 怎么解决单目没有深度信息问题？有2种，论文用的是第二种，用一个具有高不确定度的逆深度参数来初始化点的深度信息，该参数会在后期逐渐收敛到真值（虽然目前还不知道在哪里收敛）。
3. 说了ORB-SLAM为什么要同时计算基础矩阵F和单应矩阵H的原因：这两种摄像头位姿重构方法在低视差下都没有很好的约束，所以提出了一个新的基于模型选择的自动初始化方法，对平面场景算法选择单应性矩阵，而对于非平面场景，算法选择基础矩阵。
4. 说了ORB-SLAM初始化容易失败的原因：（条件比较苛刻）在平面的情况下，为了保险起见，如果最终存在双重歧义，则算法避免进行初始化，因为可能会因为错误选择而导致算法崩溃。因此，我们会延迟初始化过程，直到所选的模型在明显的视差下产生唯一的解。

2. 共视图 Covisibility Graph

共视图非常的关键，需要先理解共视图，才能更好的理解后续程序中如何设置顶点和边。

2.1 共视图定义

共视图是无向加权图，每个节点是关键帧，如果两个关键帧之间满足一定的共视关系（至少15个共同观测地图点）他们就连成一条边，边的权重就是共视地图点数目。

2.2 共视图作用

2.2.1 跟踪局部地图，扩大搜索范围

• Tracking::UpdateLocalKeyFrames()

2.2.2 局部建图里关键帧之间新建地图点

• LocalMapping::CreateNewMapPoints() 
• LocalMapping::SearchInNeighbors()

2.2.3 闭环检测、重定位检测

• LoopClosing::DetectLoop()、LoopClosing::CorrectLoop()
• KeyFrameDatabase::DetectLoopCandidates
• KeyFrameDatabase::DetectRelocalizationCandidates

2.2.4 优化

• Optimizer::OptimizeEssentialGraph

3. 地图点 MapPoint 和关键帧 KeyFrame

对每个地图点云$p_i$保存以下信息：

1. 它在世界坐标系中的3D坐标$X_w^i$
2. 视图方向$n_i$，即所有视图方向的平均单位向量（该方向是指连接该点云和其对应观测关键帧光心的射线方向）
3. ORB特征描述子$D_i$，与其他所有能观测到该点云的关键帧中ORB描述子相比，该描述子的汉明距离最小
4. 根据ORB特征尺度不变性约束，可观测的点云的最大距离$d_{max}$和最小距离$d_{min}$

对每个关键帧$K_i$保存以下信息：

1. 相机位姿T_(i,w)，从世界坐标系转换到相机坐标系下的变换矩阵
2. 相机内参，包括主点和焦距
3. 从图像帧提取的所有ORB特征，不管其是否已经关联了地图云点， 这些ORB特征点都经过畸变模型矫正过

4. 图优化 Graph SLAM

可先看看这些资料《计算机视觉大型攻略 —— SLAM(2) Graph-based SLAM（基于图优化的算法)》，还有《概率机器人学》的第11章，深入理解图优化的概念。

• ORBSLAM中一共有五种优化方式，分别是局部BA优化，全局BA优化，闭环位姿优化，全局位姿优化，单帧BA优化（这个名字是我自己取的，方便理解），这里是全局BA优化，不仅带位姿（$T_{cw}$），还带观测（$P_w$）。
• 在全局优化中，所有的关键帧（除了第一帧，因为第一帧作为锚定的帧，是不变的，对应vSE3->setFixed(pKF->mnId==0)）和三维点都参与优化。
• 全局优化的图结构示意如下，节点是当前帧的Tcw（即SE(3)）和地图点三维坐标$P_w$，边是地图点与当前帧的投影关系，即公式$Z_C\left[\begin{array}{c}u\\ v\\ 1\end{array}\right]=\underset{\text{内参}}{\underbrace{\left[\begin{array}{cccc}{f}_x& 0& u_0& 0\\ 0& f_y& v_0& 0\\ 0& 0& 1& 0\end{array}\right]}}\underset{\text{外参}}{\underbrace{\left[\begin{array}{cc}R& t\\ 0& 1\end{array}\right]}}\left[\begin{array}{c}{X}_C\\ Y_C\\ Z_C\\ 1\end{array}\right]$，信息矩阵与特征点所在的尺度有关（后续再展开说明）。
  
• 误差模型是重投影误差（3D(x,y,z)-2D(u,v)误差），在EdgeSE3ProjectXYZ中，将3D地图点投影到相机坐标系下的相机平面，与观测的像素点产生的重投影误差，表示成：$$e=(u,v)-f_{project}(SE(3),P_w)$$

我们在文章开头说过，单目初始化结果得到了三角测量初始化得到的3D地图点$P_w$，计算得到了初始两帧图像之间的相对位姿（相当于得到了SE(3)），通过相机坐标系$P_c$和世界坐标系$P_w$之间的公式，（参考《像素坐标系、图像坐标系、相机坐标系和世界坐标系的关系（简单易懂版）》）
$$\left[\begin{array}{c}{X}_c\\ Y_c\\ Z_c\end{array}\right]=R\left[\begin{array}{c}{X}_w\\ Y_w\\ Z_w\end{array}\right]+T$$
得到相机坐标系的坐标$P_c$，但是这样还是不能和像素坐标比较。我们接着通过相机坐标系$P_c$和像素坐标系$P_{u,v}$之间的公式$$\left[\begin{array}{c}u\\ v\\ 1\end{array}\right]=\left[\begin{array}{ccc}{f}_x& 0& u_0\\ 0& f_y& v_0\\ 0& 0& 1\end{array}\right]\left[\begin{array}{c}\frac{X_C}{Z_C}\\ \frac{Y_C}{Z_C}\\ 1\end{array}\right]$$
可计算得到直接和观测值能比较的值$P_{u,v}^{\prime }$，这样就可以和特征点的坐标$P_{u,v}$进行比较，得到观测误差。
$$e=P_{u,v}-P_{u,v}^{\prime }$$

回顾一下SLAM的状态估计问题：
$$e_{v,k}=x_k-f(x_{k-1},u_k)$$

$$e_{v,j,k}=z_{k,j}-f(x_k,y_j)$$
该误差的平方之和：
$$J(x)=\sum _k{e}_{v,k}^T{R}_k^{-1}{e}_{v,k}+\sum _k\sum _j{e}_{v,j,k}^T{Q}_{(}k,j{)}^{-1}{e}_{v,j,k}$$
其中，$x_k$是k时刻的状态变量，$f(x_{k-1},u_k)$是由k-1时刻的状态变量$x_{k-1}$结合状态方程$f(x_k)$和过程噪声$u_k$计算得到的估计值，它们之间的差值就是误差项。z_{k,j}是k时刻的测量值，$f(x_k,y_j)$是由k时刻的状态变量$x_k,y_j$结合观测模型$f(x_k,y_j)$计算得到的估计观测值，它们之间的差值就是误差项。这样就得到一个总体意义下的最小二乘问题，它的最优解等价于状态的最大似然估计。由于噪声的存在，当我们把估计的轨迹与地图代入SLAM的运动、观测方程时，并不会很完美，可以对状态进行微调，使得整体的误差下降到一个极小值，这就是优化的目的。

5. g2o使用方法

关于g2o库的使用方法，可以参考《G2O图优化基础和SLAM的Bundle Adjustment(光束法平差)》和《理解图优化，一步步带你看懂g2o代码》。一般来说，g2o的使用流程如下：

• A.创建一个线性求解器LinearSolver
• B.创建BlockSolver，并用上面定义的线性求解器LinearSolver初始化
• C.创建总求解器solver，并从GN, LM, DogLeg 中选一个，再用上述块求解器BlockSolver初始化
• D.创建终极大boss 稀疏优化器（SparseOptimizer），并用已定义的总求解器solver作为求解方法
• E.定义图的顶点和边，并添加到稀疏优化器（SparseOptimizer）中
• F.设置优化参数，开始执行优化
  

四、代码

1. 将初始关键帧，当前关键帧的描述子转为BoW

    pKFini->ComputeBoW();
    pKFcur->ComputeBoW();

不展开词袋BoW，只需要知道一点，就是我们在回环检测的时候，需要用到词袋向量mBowVec和特征点向量mFeatVec，所以这里要计算。

2. 向地图添加关键帧

    mpAtlas->AddKeyFrame(pKFini);
    mpAtlas->AddKeyFrame(pKFcur);

3. 生成地图点，更新图（节点和边）

3.1 遍历

    for(size_t i=0; i<mvIniMatches.size();i++)

因为要用三角测量初始化得到的3D点，所以外围是一个大的循环，遍历三角测量初始化得到的3D点mvIniP3D。

3.2 检查

    if(mvIniMatches[i]<0)
        continue;

没有匹配的点，则跳过。

3.3 构造点

    cv::Mat worldPos(mvIniP3D[i]);

用三角测量初始化得到的3D点mvIniP3D[i]作为空间点的世界坐标 worldPos。

    MapPoint* pMP = new MapPoint(worldPos,pKFcur,mpAtlas->GetCurrentMap());

然后用空间点的世界坐标 worldPos 构造地图点 pMP。

3.4 修改点属性

3.4.1 添加可以观测到该地图点 pMP 的关键帧

    pMP->AddObservation(pKFini,i);
    pMP->AddObservation(pKFcur,mvIniMatches[i]);

3.4.2 计算该地图点 pMP 的描述子

    pMP->ComputeDistinctiveDescriptors();

因为ORBSLAM是特征点方法，描述子非常重要，但是一个地图点有非常多能观测到该点的关键帧，每个关键帧都有相对该地图点的值（距离和角度）不一样的描述子，在这么多的描述子中，如何选取一个最能代表该点的描述子呢？这里作者用了距离中值法，意思就是说，最能代表该地图点的描述子，应该是与其他描述子具有最小的距离中值。

举个栗子，现有描述子A、B、C、D、E、F、G，它们之间的距离分别是1、1、2、3、4、5，求最小距离中值的描述子：

把它们的距离做成2维vector行列的形式，如下：

对每个关键帧得到的描述子与其他描述子的距离进行排序。然后，中位数是median = vDists[0.5*(N-1)]=0.5×(7-1)=3，得到：

可以看到，描述子B具有最小距离中值，所以选择描述子B作为该地图点的描述子。

上述例子比较容易理解，但实际问题是，描述子是一个值，如何描述一个值和另一个值的距离呢？我们可以把两个值看成是两个二进制串，而描述两个二进制串之间的距离可以用汉明距离，指的是其不同位数的个数。这样，我们就可以求出两个描述子之间的距离了。

3.4.3 更新该地图点 pMP 的平均观测方向和深度范围

    pMP->UpdateNormalAndDepth();

由于一个地图点会被许多相机观测到，因此在插入关键帧后，需要更新相应变量，创建新的关键帧的时候会调用该地图点。而图像提取描述子是使用金字塔分层提取，所以计算法向量和深度可以知道该地图点在对应的关键帧的金字塔哪一层可以提取到。明确了目的，下一步就是方法问题，所谓的法向量，就是也就是说相机光心指向地图点的方向，计算这个方向方法很简单，只需要用地图点的三维坐标mWorldPos减去相机光心的三维坐标Ow就可以。可是，能观测到该地图点的不止一个关键帧，是个关键帧集合，每个关键帧都有各自的相机光心Owi，我们可以假设一下，有地图点P，有3个关键帧可观测到该点，3个关键帧的相机光心分别是A、B、C，则三个光心到地图点的向量分别是$\vec{a}、\vec{b}、\vec{c}$，因为通过向量加法可以抵消掉朝向地图点的其他方向上的量，所以最终该地图点的法向量就是三个光心到该地图点的向量的和$\vec{a}+\vec{b}+\vec{c}$。而法向量长度为一，要对$\vec{a}+\vec{b}+\vec{c}$做归一化处理$\frac{\vec{a}+\vec{b}+\vec{c}}{|\vec{a}+\vec{b}+\vec{c}|}$，示意图如下：

知道方法之后，我们看程序里面MapPoint::UpdateNormalAndDepth()如何实现：

3.4.3.1 获取地图点信息

    observations=mObservations; // 获得观测到该地图点的所有关键帧
    pRefKF=mpRefKF;             // 观测到该点的参考关键帧（第一次创建时的关键帧）
    Pos = mWorldPos.clone();    // 地图点在世界坐标系中的位置

我们要获得观测到该地图点的所有关键帧，用来找到每个关键帧的光心Owi。还要获得观测到该点的参考关键帧（即第一次创建时的关键帧），因为这里只是更新观测方向，距离还是用参考关键帧到该地图点的距离，体现在后面dist = cv::norm(Pos - pRefKF->GetCameraCenter())。还要获得地图点在世界坐标系中的位置，用来计算法向量。

3.4.3.2 计算该地图点的法向量

    cv::Mat normal = cv::Mat::zeros(3,1,CV_32F);
    int n=0;
    for(map<KeyFrame*,size_t>::iterator mit=observations.begin(), mend=observations.end(); mit!=mend; mit++)
    {
        KeyFrame* pKF = mit->first;
        tuple<int,int> indexes = mit -> second;
        int leftIndex = get<0>(indexes), rightIndex = get<1>(indexes);
        if(leftIndex != -1){
            cv::Mat Owi = pKF->GetCameraCenter();
            cv::Mat normali = mWorldPos - Owi;
            normal = normal + normali/cv::norm(normali);
            n++;
        }
        if(rightIndex != -1){
            cv::Mat Owi = pKF->GetRightCameraCenter();
            cv::Mat normali = mWorldPos - Owi;
            normal = normal + normali/cv::norm(normali);
            n++;
        }
    } 

遍历能观测到该地图点的所有关键帧，获取不同的光心坐标Owi。用地图点的三维坐标mWorldPos减去相机光心的三维坐标Ow计算向量，然后归一化为单位向量，对初始单位向量normal和归一化后的单位向量normali/cv::norm(normali)进行求和，最终除以总数n得到该地图点的朝向mNormalVector = normal/n。ORB-SLAM3在这里添加了双目相机的光心坐标计算。

3.4.3.3 计算该地图点到图像的距离

    cv::Mat PC = Pos - pRefKF->GetCameraCenter();
    const float dist = cv::norm(PC);

计算参考关键帧相机指向地图点的向量，利用该向量求该地图点的距离。

3.4.3.4 更新该地图点的距离上下限

    // 观测到该地图点的当前帧的特征点在金字塔的第几层
    tuple<int ,int> indexes = observations[pRefKF];
    int leftIndex = get<0>(indexes), rightIndex = get<1>(indexes);
    int level;
    if(pRefKF -> NLeft == -1){
        level = pRefKF->mvKeysUn[leftIndex].octave;
    }
    else if(leftIndex != -1){
        level = pRefKF -> mvKeys[leftIndex].octave;
    }
    else{
        level = pRefKF -> mvKeysRight[rightIndex - pRefKF -> NLeft].octave;
    }
    //const int level = pRefKF->mvKeysUn[observations[pRefKF]].octave;        
    const float levelScaleFactor =  pRefKF->mvScaleFactors[level];          // 当前金字塔层对应的缩放倍数
    const int nLevels = pRefKF->mnScaleLevels;                              // 金字塔层数
    {
        unique_lock<mutex> lock3(mMutexPos);
        // 使用方法见PredictScale函数前的注释
        mfMaxDistance = dist*levelScaleFactor;                              // 观测到该点的距离上限
        mfMinDistance = mfMaxDistance/pRefKF->mvScaleFactors[nLevels-1];    // 观测到该点的距离下限
        mNormalVector = normal/n;                                           // 获得地图点平均的观测方向
    }

回顾之前的知识：

假设第m层上有一特征点$F_m$，其空间位置与拍摄时相机中心的位置为$d_m$ ，显然这是原始图像缩放$1/1.2^m$ 倍后得到的特征点patch，考虑“物远像小”的成像特点，要使得该第m层特征点对应patch变为图像金字塔第0层中同样大小的patch，其相机与空间点的距离$d=d_m\ast 1.2^m$ ,即尺度不变的最大物距$d_{max}=d_m\ast 1.2^m$ 。

要求尺度不变的最小物距则这样考虑：根据“物近像大”的成像特点，使得当前第m层的特征点移到第7层上，则真实相机成像图像得放大$1.2^{7-m}$倍，故对应最小物距
$$d_{min}=d_m\ast 1.2^{m-7}=d_m\ast 1.2^{m-(8-1)}=\frac{d_m\ast 1.2^m}{1.2^{(8-1)}}=\frac{d_{max}}{scal{e}^{nlevels-1}}$$

从上我们知道，【地图点的距离上限】 = 【该地图点到图像的距离】（在3中我们计算了，就是该地图点到参考关键帧的距离）× 【观测到该地图点的当前帧的特征点所在金字塔层数的缩放倍数】 的 【观测到该地图点的当前帧的特征点在金字塔的层数】 次幂，就是上面例子中的$d_{max}=d_m\ast 1.2^m$公式的描述，代码是mfMaxDistance = dist*levelScaleFactor。
从上我们还知道，【地图点的距离下限】 = 【地图点的距离上限】/【观测到该地图点的当前帧的特征点所在金字塔层数的缩放倍数】，代码是mfMinDistance = mfMaxDistance/pRefKF->mvScaleFactors[nLevels-1]。

3.5 添加地图点到地图

    mpAtlas->AddMapPoint(pMP);

3.6 更新图

非常重要的知识点，好好琢磨，该过程由函数UpdateConnections完成，深入其中看看有什么奥妙。

3.6.1 统计共视帧

// 遍历每一个地图点
for(vector<MapPoint*>::iterator vit=vpMP.begin(), vend=vpMP.end(); vit!=vend; vit++)
...
     // 统计与当前关键帧存在共视关系的其他帧
     map<KeyFrame*,size_t> observations = pMP->GetObservations();
     for(map<KeyFrame*,size_t>::iterator mit=observations.begin(), mend=observations.end(); mit!=mend; mit++)
     ...
     // 体现了作者的编程功底，很强
     KFcounter[mit->first]++;
...

这里代码主要是想完成遍历每一个地图点，统计与当前关键帧存在共视关系的其他帧，统计结果放在KFcounter。看代码有点费劲，举个栗子：已知有一关键帧F1，上面有四个地图点ABCD，其中，能观测到点A的关键帧是有3个，分别是帧F1、F2、F3。能观测到点B的关键帧是有4个，分别是帧F1、F2、F3、F4。能观测到点C的关键帧是有5个，分别是帧F1、F2、F3、F4、F5。能观测到点D的关键帧是有6个，分别是帧F1、F2、F3、F4、F5、F6。对应关系如下：

总而言之，代码想统计的就是与当前关键帧存在共视关系的其他帧，共视关系是通过能看到同个特征点来描述的，所以，当前帧F1与帧F2的共视关系为4，当前帧F1与帧F3的共视关系为4，当前帧F1与帧F4的共视关系为3，当前帧F1与帧F5的共视关系为2，当前帧F1与帧F6的共视关系为1。

3.6.2 更新共视关系大于一定阈值的边，并找到共视程度最高的关键帧

    for(map<KeyFrame*,int>::iterator mit=KFcounter.begin(), mend=KFcounter.end(); mit!=mend; mit++)
    {
        ...
        // 找到共视程度最高的关键帧
        if(mit->second>nmax)
        {
            nmax=mit->second;
            pKFmax=mit->first;
        }
        if(mit->second>=th)
        {
            // 更新共视关系大于一定阈值的边
            vPairs.push_back(make_pair(mit->second,mit->first));
            // 更新其它关键帧与当前帧的连接权重
            (mit->first)->AddConnection(this,mit->second);
        }
    }

假设共视关系阈值为1，在上面这个例子中，只要和当前帧有共视关系的帧都需要更新边，由于在这之前，关键帧只和地图点之间有连接关系，和其他帧没有连接关系，要构建共视图（以帧为节点，以共视关系为边）就要一个个更新节点之间的边的值。
(mit->first)->AddConnection(this,mit->second)的含义是更新其他帧Fi和当前帧F1的边（因为当前帧F1也被当做其他帧Fi的有共视关系的一个）。在遍历查找共视关系最大帧的时候同步做这个事情，可以加速计算和高效利用代码。mit->first在这里，代表和当前帧有共视关系的F2…F6（因为遍历的是KFcounter，存储着与当前帧F1有共视关系的帧F2…F6）。举个栗子，当处理当前帧F1和共视帧F2时，更新与帧F2有共视关系的帧F1，以此类推，当处理当前帧F1和共视帧F3时，更新与帧F3有共视关系的帧F1…。

3.6.3 如果没有连接到关键帧（没有超过阈值的权重），则连接权重最大的关键帧

    if(vPairs.empty())
    {
        vPairs.push_back(make_pair(nmax,pKFmax));
        pKFmax->AddConnection(this,nmax);
    }

如果每个关键帧与它共视的关键帧的个数都少于给定的阈值，那就只更新与其它关键帧共视程度最高的关键帧的 mConnectedKeyFrameWeights，以免之前这个阈值可能过高造成当前帧没有共视帧，容易造成跟踪失败？（自己猜的）

3.6.4 对共视程度比较高的关键帧对更新连接关系及权重（从大到小）

    sort(vPairs.begin(),vPairs.end());
    // 将排序后的结果分别组织成为两种数据类型
    list<KeyFrame*> lKFs;
    list<int> lWs;
    for(size_t i=0; i<vPairs.size();i++)
    {
        // push_front 后变成了从大到小顺序
        lKFs.push_front(vPairs[i].second);
        lWs.push_front(vPairs[i].first);
    }

3.6.5 更新当前帧的信息

   ...
   mConnectedKeyFrameWeights = KFcounter;
   mvpOrderedConnectedKeyFrames = vector<KeyFrame*>(lKFs.begin(),lKFs.end());
   mvOrderedWeights = vector<int>(lWs.begin(), lWs.end());

3.6.1.6 更新生成树的连接

    ...
   if(mbFirstConnection && mnId!=mpMap->GetInitKFid())
   {
        // 初始化该关键帧的父关键帧为共视程度最高的那个关键帧
        mpParent = mvpOrderedConnectedKeyFrames.front();
        // 建立双向连接关系，将当前关键帧作为其子关键帧
        mpParent->AddChild(this);
        mbFirstConnection = false;
   }

4. 全局BA

全局BA主要是由函数GlobalBundleAdjustemnt完成的，其调用了函数BundleAdjustment，建议开始阅读之前复习一下文章前面的《二、4. 图优化 Graph SLAM》和《二、5. g2o使用方法》，下文直接从函数BundleAdjustment展开叙述。

// 调用
    Optimizer::GlobalBundleAdjustemnt(mpAtlas->GetCurrentMap(),20);
// 定义
void Optimizer::GlobalBundleAdjustemnt(Map* pMap, int nIterations, bool* pbStopFlag, const unsigned long nLoopKF, const bool bRobust)
//调用
    vector<KeyFrame*> vpKFs = pMap->GetAllKeyFrames();
    vector<MapPoint*> vpMP = pMap->GetAllMapPoints();
    BundleAdjustment(vpKFs,vpMP,nIterations,pbStopFlag, nLoopKF, bRobust);
// 定义
void Optimizer::BundleAdjustment(const vector<KeyFrame *> &vpKFs, const vector<MapPoint *> &vpMP, int nIterations, bool* pbStopFlag, const unsigned long nLoopKF, const bool bRobust)

4.1 方程求解器 LinearSolver

    g2o::BlockSolver_6_3::LinearSolverType * linearSolver;
    linearSolver = new g2o::LinearSolverEigen<g2o::BlockSolver_6_3::PoseMatrixType>();

创建方程求解器LinearSolver，求解增量方程$H\Delta x=g$。通常情况下想到的方法就是直接求逆，也就是$\Delta x=-H^{-1}\cdot g$。看起来好像很简单，但这有个前提，就是H的维度较小，此时只需要矩阵的求逆就能解决问题。但是当H的维度较大时，矩阵求逆变得很困难，求解问题也变得很复杂。g2o里面已经集成了很多方法的函数比如sparse cholesky分解法的LinearSolverCholmod，CSparse法的LinearSolverCSparse，preconditioned conjugate gradient法的LinearSolverPCG，dense cholesky分解法的LinearSolverDense，eigen中sparse Cholesky法的LinearSolverEigen。很明显，ORB-SLAM使用了eigen中sparse Cholesky法，感兴趣的可自行百度原理。

4.2 矩阵求解器 BlockSolver

    g2o::BlockSolver_6_3 * solver_ptr = new g2o::BlockSolver_6_3(linearSolver);

用LinearSolver创建矩阵求解器BlockSolver，求解雅克比Jacobian和海森Hessian矩阵，BlockSolver_6_3指矩阵的维度是6×3，即SE(3)是关键帧的位姿$\left[\begin{array}{cc}R& t\\ 0^T& 1\end{array}\right]$，包含(x,y,z,roll,pitch,yaw)，维度是6。地图点的位姿是三维坐标（x,y,z），维度是3。

typedef BlockSolver< BlockSolverTraits<6, 3> > BlockSolver_6_3;

4.3 算法求解器 AlgorithmSolver

    g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg(solver_ptr);

用BlockSolver创建方法求解器solver，选择非线性最小二乘解法（高斯牛顿GN、LM、狗腿DogLeg等），AlgorithmSolver是我自己想出来的名字，方便记忆。

4.4 稀疏优化器 SparseOptimizer

    g2o::SparseOptimizer optimizer;
    optimizer.setAlgorithm(solver);// 用前面定义好的求解器作为求解方法
    optimizer.setVerbose(false);// 设置优化过程输出信息用的

用solver创建稀疏优化器SparseOptimizer。

4.5 定义图的顶点和边，添加到稀疏优化器SparseOptimizer

在开始看具体步骤前，注意两点，一是ORB-SLAM3中图的定义，二是其误差模型，理解之后才可能明白为什么初始化过程中要操作这些变量。

4.5.1 图的定义

4.5.1.1 图的节点和边

因为在全局优化中所有的关键帧（除了第一帧）和三维点都参与优化，所以图的节点是关键帧位姿（即SE(3)）和地图点位姿（即3DPose）。其中，SE(3)表示为$SE3=\left[\begin{array}{cc}R& t\\ 0^T& 1\end{array}\right]$，即相机位姿，

回想一下《视觉十四讲》，这个SE(3)就是我们常说的特殊欧氏群，即n维欧氏变换，表示的是三维变换关系，而SO(3)是特殊正交群，表示的是三维旋转关系。

也就是当前帧的$T_{cw}$，地图点的位姿是三维坐标（x,y,z），也就是$P_w$。图的边是地图点和关键帧之间观测的关系，即3D空间点-2D像素点的投影关系，就是公式
$$Z_C\left[\begin{array}{c}u\\ v\\ 1\end{array}\right]=\left[\begin{array}{cccc}{f}_x& 0& 0& u_0\\ 0& f_y& 0& v_0\\ 0& 0& 1& 0\end{array}\right]\left[\begin{array}{cc}R& t\\ 0& 1\end{array}\right]\left[\begin{array}{c}{X}_C\\ Y_C\\ Z_C\\ 1\end{array}\right]$$
的关系，代码中，首先会计算世界坐标系坐标$P_w$（即地图点坐标）转换到相机坐标系下的坐标$P_c$，利用如下公式，map函数实现（se3quat.h）：
$$\underset{\text{公式}}{\underbrace{\left[\begin{array}{c}{X}_c\\ Y_c\\ Z_c\end{array}\right]=R\left[\begin{array}{c}{X}_w\\ Y_w\\ Z_w\end{array}\right]+T}}\iff \underset{\text{代码}}{\underbrace{P_c=\_r\cdot vertex2.estimate+\_t}}$$

再计算相机坐标系坐标$P_c$转换到像素坐标系下的坐标$P_{u,v}$，利用如下公式，EdgeSE3ProjectXYZ::cam_project函数实现（types_six_dof_expmap.cpp）：
$$\left[\begin{array}{c}u\\ v\\ 1\end{array}\right]=\underset{\text{公式}}{\underbrace{\left[\begin{array}{ccc}{f}_x& 0& u_0\\ 0& f_y& v_0\\ 0& 0& 1\end{array}\right]\left[\begin{array}{c}\frac{X_c}{Z_c}\\ \frac{Y_c}{Z_c}\\ 1\end{array}\right]}}\iff \underset{\text{代码}}{\underbrace{\left[\begin{array}{ccc}fx& 0& cx\\ 0& fy& cy\\ 0& 0& 1\end{array}\right]\left[\begin{array}{c}\frac{v(0)}{v(2)}\\ \frac{v(1)}{v(2)}\\ 1\end{array}\right]}}$$
结合代码，可以看看下图的示意（点击查看高清大图）：

4.5.1.2 设置节点和边的步骤

和把大象放冰箱的步骤一样的简单，设置顶点和边的步骤总共分三步：

1. 设置类型是关键帧位姿的节点信息：位姿（SE3）、编号（setId(pKF->mnId)）、最大编号（maxKFid）；
2. 设置类型是地图点坐标的节点信息：位姿（3dPos）、编号（setId(pMp->mnId+maxKFid+1)）、计算的变量（setMarginalized）；【为什么要设置setMarginalized，感兴趣的同学可以自己参考一下这篇文章《g2o：非线性优化与图论的结合》，这里就不过多赘述了】
3. 设置边的信息：连接的节点（setVertex）、信息矩阵（setInformation）、计算观测值的相关参数（setMeasurement/fx/fy/cx/cy）、核函数（setRobustKernel）。【引入鲁棒核函数是人为降低过大的误差项，可以更加稳健地优化，具体请参考《视觉十四讲》第10讲】

4.5.1.3 ORB-SLAM3新增部分

ORB-SLAM3中新增了单独记录边、地图点和关键帧的容器，比如单目中的vpEdgesMono、vpEdgeKFMono和vpMapPointEdgeMono，分别记录的是误差值、关键帧和地图点，目的是在获取优化后的关键帧位姿时，使用该误差值vpEdgesMono[i]，对地图点vpMapPointEdgeMono[i]进行自由度为2的卡方检验e->chi2()>5.991，以此排除外点，选出质量好的地图点，见源码Optimizer.cc#L337。为了不打断图优化思路，不过多展开ORB-SLAM2和3的区别，感兴趣的同学可自行研究源码。

4.5.2 误差模型

SLAM中要计算的误差如下示意：
$$J(x)=\sum _k{e}_{v,k}^T{R}_k^{-1}{e}_{v,k}+\sum _k\sum _j{e}_{v,j,k}^T{Q}_{(}k,j{)}^{-1}{e}_{v,j,k}$$
其中，$e_{v,j,k}$是观测误差，对应到代码中就是，用观测值【即校正后的特征点坐标mvKeysUn，是Frame类的UndistortKeyPoints函数获取的】，减去其估计值【即地图点mvIniP3D，该点是ReconstructF或者ReconstructH中，利用三角测量得到空间点坐标，之后把该地图点mvIniP3D投影到图像上，得到估计的特征点坐标$P_{u,v}$】。Q是观测噪声，对应到代码中就是协方差矩阵sigma（而且还和特征点所在金字塔层数有关，层数越高，噪声越大）。

4.5.3 步骤一，添加关键帧位姿顶点

    // 对于当前地图中的所有关键帧
    for(size_t i=0; i<vpKFs.size(); i++)
    {
        KeyFrame* pKF = vpKFs[i];
        // 去除无效的
        if(pKF->isBad())
            continue;
        // 对于每一个能用的关键帧构造SE3顶点,其实就是当前关键帧的位姿
        g2o::VertexSE3Expmap * vSE3 = new g2o::VertexSE3Expmap();
        vSE3->setEstimate(Converter::toSE3Quat(pKF->GetPose()));
        vSE3->setId(pKF->mnId);
        // 只有第0帧关键帧不优化（参考基准）
        vSE3->setFixed(pKF->mnId==0);
        // 向优化器中添加顶点，并且更新maxKFid
        optimizer.addVertex(vSE3);
        if(pKF->mnId>maxKFid)
            maxKFid=pKF->mnId;
    }

注意三点：

• 第0帧关键帧作为参考基准，不优化
• 只需设置SE(3)和Id即可
• 需要更新maxKFid，以便下方添加观测值（相机3D位姿）时使用

4.5.4 步骤二，添加地图点位姿顶点

    // 卡方分布 95% 以上可信度的时候的阈值
    const float thHuber2D = sqrt(5.99);     // 自由度为2
    const float thHuber3D = sqrt(7.815);    // 自由度为3
    // Set MapPoint vertices
    // Step 2.2：向优化器添加MapPoints顶点
    // 遍历地图中的所有地图点
    for(size_t i=0; i<vpMP.size(); i++)
    {
        MapPoint* pMP = vpMP[i];
        // 跳过无效地图点
        if(pMP->isBad())
            continue;
        // 创建顶
        g2o::VertexSBAPointXYZ* vPoint = new g2o::VertexSBAPointXYZ();
        // 注意由于地图点的位置是使用cv::Mat数据类型表示的,这里需要转换成为Eigen::Vector3d类型
        vPoint->setEstimate(Converter::toVector3d(pMP->GetWorldPos()));
        // 前面记录maxKFid 是在这里使用的
        const int id = pMP->mnId+maxKFid+1;
        vPoint->setId(id);
        // g2o在做BA的优化时必须将其所有地图点全部schur掉，否则会出错。
        // 原因是使用了g2o::LinearSolver<BalBlockSolver::PoseMatrixType>这个类型来指定linearsolver,
        // 其中模板参数当中的位姿矩阵类型在程序中为相机姿态参数的维度，于是BA当中schur消元后解得线性方程组必须是只含有相机姿态变量。
        // Ceres库则没有这样的限制
        vPoint->setMarginalized(true);
        optimizer.addVertex(vPoint);

4.5.5 步骤三，添加边

        // 边的关系，其实就是点和关键帧之间观测的关系
        const map<KeyFrame*,size_t> observations = pMP->GetObservations();
        // 边计数
        int nEdges = 0;
        //SET EDGES
        // Step 3：向优化器添加投影边（是在遍历地图点、添加地图点的顶点的时候顺便添加的）
        // 遍历观察到当前地图点的所有关键帧
        for(map<KeyFrame*,size_t>::const_iterator mit=observations.begin(); mit!=observations.end(); mit++)
        {
            KeyFrame* pKF = mit->first;
            // 滤出不合法的关键帧
            if(pKF->isBad() || pKF->mnId>maxKFid)
                continue;
            nEdges++;
            const cv::KeyPoint &kpUn = pKF->mvKeysUn[mit->second];
            if(pKF->mvuRight[mit->second]<0)
            {
                // 如果是单目相机按照下面操作
                // 构造观测
                Eigen::Matrix<double,2,1> obs;
                obs << kpUn.pt.x, kpUn.pt.y;
                // 创建边
                g2o::EdgeSE3ProjectXYZ* e = new g2o::EdgeSE3ProjectXYZ();
                // 填充数据，构造约束关系
                e->setVertex(0, dynamic_cast<g2o::OptimizableGraph::Vertex*>(optimizer.vertex(id)));
                e->setVertex(1, dynamic_cast<g2o::OptimizableGraph::Vertex*>(optimizer.vertex(pKF->mnId)));
                e->setMeasurement(obs);
                // 信息矩阵，也是协方差，表明了这个约束的观测在各个维度（x,y）上的可信程度，在我们这里对于具体的一个点，两个坐标的可信程度都是相同的，
                // 其可信程度受到特征点在图像金字塔中的图层有关，图层越高，可信度越差
                // 为了避免出现信息矩阵中元素为负数的情况，这里使用的是sigma^(-2)
                const float &invSigma2 = pKF->mvInvLevelSigma2[kpUn.octave];
                e->setInformation(Eigen::Matrix2d::Identity()*invSigma2);
                // 如果要使用鲁棒核函数
                if(bRobust)
                {
                    g2o::RobustKernelHuber* rk = new g2o::RobustKernelHuber;
                    e->setRobustKernel(rk);
                    // 这里的重投影误差，自由度为2，所以这里设置为卡方分布中自由度为2的阈值，如果重投影的误差大约大于1个像素的时候，就认为不太靠谱的点了，
                    // 核函数是为了避免其误差的平方项出现数值上过大的增长
                    rk->setDelta(thHuber2D);
                }
                // 设置相机内参
                // ORB-SLAM2的做法
                //e->fx = pKF->fx;
                //e->fy = pKF->fy;
                //e->cx = pKF->cx;
                //e->cy = pKF->cy;
                // ORB-SLAM3的改变
				e->pCamera = pKF->mpCamera;
                optimizer.addEdge(e);
            }
            else
            {
                // 双目或RGBD相机按照下面操作
                ......这里忽略，只讲单目
            }  
        } // 向优化器添加投影边,也就是遍历所有观测到当前地图点的关键帧
        // 如果因为一些特殊原因,实际上并没有任何关键帧观测到当前的这个地图点,那么就删除掉这个顶点,并且这个地图点也就不参与优化
        if(nEdges==0)
        {
            optimizer.removeVertex(vPoint);
            vbNotIncludedMP[i]=true;
        }
        else
        {
            vbNotIncludedMP[i]=false;
        }
    }    

4.5.6 优化

    optimizer.initializeOptimization();
    optimizer.optimize(nIterations);

添加边设置优化参数，开始执行优化。

5. 计算深度中值

    float medianDepth = pKFini->ComputeSceneMedianDepth(2);
    float invMedianDepth = 1.0f/medianDepth;

这里开始，5到7是比较关键的转换，要理解这部分背后的含义，我们需要回顾一下相机模型，复习一下各个坐标系之间的转换关系，再看代码就简单多了。

5.1 相机模型与坐标系转换

很多人看了n遍相机模型，小孔成像原理烂熟于心，但那只是烂熟，并没有真正应用到实际，想真正掌握，认真看下去。先复习一下相机投影的过程，也可参考该文《像素坐标系、图像坐标系、相机坐标系和世界坐标系的关系（简单易懂版）》，如图（点击查看高清大图）：

再来弄清楚各个坐标系之间的转换关系，认真研究下图，懂了之后能解决很多心里的疑问（点击查看高清大图）：

总之，图像上的像素坐标和世界坐标的关系是：

其中，$z_c$是相机坐标系下的坐标；$d_x$和$d_y$分别表示每个像素在横轴x和纵轴y的物理尺寸，单位为毫米/像素；$u_0,v_0$表示的是图像的中心像素坐标和图像圆点像素坐标之间相差的横向和纵向像素数；$f$是相机的焦距，$R,T$是旋转矩阵和平移矩阵，$x_w,y_w,z_w$是世界坐标系下的坐标。

5.2 归一化平面

讲归一化平面的资料比较少，可参考性不高。大家也不要把这个东西看的有多玄乎，其实就是一个数学技巧，主要是为了方便计算。从上面的公式可以看到，左边还有个$z_c$的因数，除掉这个因数的过程其实就可以叫归一化。代码中接下来要讲的几步其实都可以归结为以下这个公式：

6. 归一化两帧变换到平均深度为1

    cv::Mat Tc2w = pKFcur->GetPose();
    // x/z y/z 将z归一化到1 
    Tc2w.col(3).rowRange(0,3) = Tc2w.col(3).rowRange(0,3)*invMedianDepth;
    pKFcur->SetPose(Tc2w);

7. 3D点的尺度归一化

    vector<MapPointPtr> vpAllMapPoints = pKFini->GetMapPointMatches();
    for (size_t iMP = 0; iMP < vpAllMapPoints.size(); iMP++)
    {
        if (vpAllMapPoints[iMP])
        {
            MapPointPtr pMP = vpAllMapPoints[iMP];
            if(!pMP->isBad())
                pMP->SetWorldPos(pMP->GetWorldPos() * invMedianDepth);
        }
    }

8. 将关键帧插入局部地图

    mpLocalMapper->InsertKeyFrame(pKFini);
    mpLocalMapper->InsertKeyFrame(pKFcur);
    mCurrentFrame.SetPose(pKFcur->GetPose());
    mnLastKeyFrameId = pKFcur->mnId;
    mnLastKeyFrameFrameId=mCurrentFrame.mnId;
    mpLastKeyFrame = pKFcur;
    mvpLocalKeyFrames.push_back(pKFcur);
    mvpLocalKeyFrames.push_back(pKFini);
    mvpLocalMapPoints = mpMap->GetAllMapPoints();
    mpReferenceKF = pKFcur;
    mCurrentFrame.mpReferenceKF = pKFcur;
    mLastFrame = Frame(mCurrentFrame);
    mpMap->SetReferenceMapPoints(mvpLocalMapPoints);
    {
        unique_lock<mutex> lock(mMutexState);
        mState = eTrackingState::OK;
    }
    mpMap->calculateAvgZ();
    // 初始化成功，至此，初始化过程完成

五、总结

总之，初始化地图部分，重要的支撑在于两个点：

1. 理解图优化的概念，包括ORB-SLAM3是如何定义图的，顶点和边到底是什么，他们有什么关系，产生这种关系背后的公式是什么，搞清楚这些，图优化就算入门了吧，也可以看得懂地图初始化部分了；
2. 相机模型，以及各个坐标系之间的关系，大多数人还是停留在大概理解的层面，需要结合代码实际来加深对它的理解，因为整个视觉SLAM就是多视图几何理论的天下，不懂这些就扎近茫茫代码中，很容易迷失。

至此，初始化过程完结了。我们通过初始化过程认识了ORB-SLAM3系统，但只是管中窥豹，看不到全面，想要更加深入的挖掘，还是要多多拆分源码，一个个模块掌握，然后才能转化成自己的东西。以上都是各人见解，如有纰漏，请各位不吝赐教，O(∩_∩)O谢谢。

六、参考

1. ORB-SLAM: a Versatile and Accurate Monocular SLAM System
2. ORB-SLAM3 细读单目初始化过程（上）
3. 理解图优化，一步步带你看懂g2o代码
4. ORB-SLAM2 代码解读（三）：优化 1（概述）
5. 视觉slam十四讲 6.非线性优化
6. 《视觉十四讲》 高翔
7. Mur-Artal R , Tardos J D . ORB-SLAM2: an Open-Source SLAM System for Monocular, Stereo and RGB-D Cameras[J]. IEEE Transactions on Robotics, 2017, 33(5):1255-1262.
8. Campos C , Elvira R , Juan J. Gómez Rodríguez, et al. ORB-SLAM3: An Accurate Open-Source Library for Visual, Visual-Inertial and Multi-Map SLAM[J]. 2020.
9. 《概率机器人》 [美] Sebastian Thrun / [德] Wolfram Burgard / [美] Dieter Fox 机械工业出版社