一、前言
請閱讀本文之前最好把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視覺學習社區