ORB-SLAM3 单目地图初始化（终结篇）
一、前言
请阅读本文之前最好把ORB-SLAM3的单目初始化过程再过一遍（ORB-SLAM3 细读单目初始化过程(上)、超详细解读ORB-SLAM3单目初始化（下篇）），以提高学习效率。单目初始化过程中最重要的是两个函数实现，分别是构建帧（Frame）和初始化（Track）。接下来，就是完成初始化过程的最后一步：地图的初始化，是由CreateInitialMapMonocular函数完成的，本文基于该函数的流程出发，目的是为了结合代码流程，把单目初始化的上下两篇的知识点和ORB-SLAM3整个系统的知识点串联起来，系统化零碎的知识，告诉你平时学到的各个小知识应用在SLAM系统中的什么位置，达到快速高效学习的效果。
二、CreateInitialMapMonocular 函数的总体流程
1. 将初始关键帧，当前关键帧的描述子转为BoW；
2. 将关键帧插入到地图；
3. 用三角测量初始化得到的3D点来生成地图点，更新关键帧间的连接关系；
4. 全局BA优化，同时优化所有位姿和三维点；
5. 取场景的中值深度，用于尺度归一化；
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](https://blog.csdn.net/weixin_42905141/article/details/102857958)）可知：
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
地图点云保存以下信息：
1）它在世界坐标系中的3D坐标
2） 视图方向，即所有视图方向的平均单位向量（该方向是指连接该点云和其对应观测关键帧光心的射线方向）
3）ORB特征描述子，与其他所有能观测到该点云的关键帧中ORB描述子相比，该描述子的汉明距离最小
4）根据ORB特征尺度不变性约束，可观测的点云的最大距离和最小距离
4. 图优化 Graph SLAM
可先看看这些资料[《计算机视觉大型攻略 —— SLAM(2) Graph-based SLAM（基于图优化的算法)》](https://blog.csdn.net/plateros/article/details/103498039)，还有《概率机器人学》的第11章，深入理解图优化的概念。
我们在文章开头说过，单目初始化结果得到了三角测量初始化得到的3D地图点Pw，计算得到了初始两帧图像之间的相对位姿（相当于得到了SE(3)），通过相机坐标系Pc和世界坐标系Pw之间的公式，（参考[《像素坐标系、图像坐标系、相机坐标系和世界坐标系的关系（简单易懂版）》](https://blog.csdn.net/shanpenghui/article/details/110481140)）
得到相机坐标系的坐标Pc，但是这样还是不能和像素坐标比较。我们接着通过相机坐标系Pc和像素坐标系P(u,v)之间的公式
5. g2o使用方法
关于g2o库的使用方法，可以参考[《G2O图优化基础和SLAM的Bundle Adjustment(光束平差)》](http://zhaoxuhui.top/blog/2018/04/10/g2o&bundle_adjustment.html#2g2o%E5%BA%93%E7%AE%80%E4%BB%8B%E4%B8%8E%E7%BC%96%E8%AF%91%E5%AE%89%E8%A3%85)和[《理解图优化，一步步带你看懂g2o代码》](https://www.cnblogs.com/CV-life/p/10286037.html)。一般来说，g2o的使用流程如下：
5.1创建一个线性求解器LinearSolver
5.2创建BlockSolver，并用上面定义的线性求解器LinearSolver初始化
5.3创建总求解器solver，并从GN, LM, DogLeg 中选一个，再用上述块求解器BlockSolver初始化
5.4创建终极大boss 稀疏优化器（SparseOptimizer），并用已定义的总求解器solver作为求解方法
5.5定义图的顶点和边，并添加到稀疏优化器（SparseOptimizer）中
5.6设置优化参数，开始执行优化
四、代码
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();
知道方法之后，我们看程序里面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++;
}
}
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; // 获得地图点平均的观测方向
}回顾之前的知识：
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.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>();
4.2 矩阵求解器 BlockSolver
g2o::BlockSolver_6_3 * solver_ptr = new g2o::BlockSolver_6_3(linearSolver);
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 图的节点和边
再计算相机坐标系坐标Pc转换到像素坐标系下的坐标P(u,v)，利用如下公式，EdgeSE3ProjectXYZ::cam_project函数实现（types_six_dof_expmap.cpp）：
结合代码，可以看看下图的示意（点击查看高清大图）：
4.5.1.2 设置节点和边的步骤
和把大象放冰箱的步骤一样的简单，设置顶点和边的步骤总共分三步：
1. 设置类型是关键帧位姿的节点信息：位姿（SE3）、编号（setId(pKF->mnId)）、最大编号（maxKFid）；
2. 设置类型是地图点坐标的节点信息：位姿（3dPos）、编号（setId(pMp->mnId+maxKFid+1)）、计算的变量（setMarginalized）；【为什么要设置setMarginalized，感兴趣的同学可以自己参考一下这篇文章[《g2o：非线性优化与图论的结合》](https://zhuanlan.zhihu.com/p/37843131)，这里就不过多赘述了】
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](https://github.com/UZ-SLAMLab/ORB_SLAM3/blob/ef9784101fbd28506b52f233315541ef8ba7af57/src/Optimizer.cc#L337)。为了不打断图优化思路，不过多展开ORB-SLAM2和3的区别，感兴趣的同学可自行研究源码。
4.5.2 误差模型
SLAM中要计算的误差如下示意：
其中，
是观测误差，对应到代码中就是，用观测值【即校正后的特征点坐标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遍相机模型，小孔成像原理烂熟于心，但那只是烂熟，并没有真正应用到实际，想真正掌握，认真看下去。先复习一下相机投影的过程，也可参考该文[《像素坐标系、图像坐标系、相机坐标系和世界坐标系的关系（简单易懂版）》](https://blog.csdn.net/shanpenghui/article/details/110481140)，如图（点击查看高清大图）：
再来弄清楚各个坐标系之间的转换关系，认真研究下图，懂了之后能解决很多心里的疑问（点击查看高清大图）：
总之，图像上的像素坐标和世界坐标的关系是：
其中，zc是相机坐标系下的坐标；dx和dy分别表示每个像素在横轴x和纵轴y的物理尺寸，单位为毫米/像素；u0,v0表示的是图像的中心像素坐标和图像圆点像素坐标之间相差的横向和纵向像素数；f是相机的焦距，R,T是旋转矩阵和平移矩阵，xw,yw,zw是世界坐标系下的坐标。
5.2 归一化平面
讲归一化平面的资料比较少，可参考性不高。大家也不要把这个东西看的有多玄乎，其实就是一个数学技巧，主要是为了方便计算。从上面的公式可以看到，左边还有个zc的因数，除掉这个因数的过程其实就可以叫归一化。代码中接下来要讲的几步其实都可以归结为以下这个公式：
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](https://blog.csdn.net/weixin_42905141/article/details/102857958)
2. [ORB-SLAM3 细读单目初始化过程（上）](https://blog.csdn.net/shanpenghui/article/details/109809723#t10)
3. [理解图优化，一步步带你看懂g2o代码](https://www.cnblogs.com/CV-life/p/10286037.html)
4. [ORB-SLAM2 代码解读（三）：优化 1（概述）](https://wym.netlify.app/2019-07-03-orb-slam2-optimization1/)
5. [视觉slam十四讲 6.非线性优化](https://blog.csdn.net/weixin_42905141/article/details/92993097#2_59)
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 机械工业出版社
备注：作者也是我们「3D视觉从入门到精通」特邀嘉宾：一个超干货的3D视觉学习社区
