二、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建立一個線性求解器LinearSolver5.2建立BlockSolver，並用上面定義的線性求解器LinearSolver初始化5.3建立總求解器solver，並從GN, LM, DogLeg 中選一個，再用上述塊求解器BlockSolver初始化5.4建立終極大boss 稀疏最佳化器（SparseOptimizer），並用已定義的總求解器solver作為求解方法5.5定義圖的頂點和邊，並新增到稀疏最佳化器（SparseOptimizer）中5.6設定最佳化引數，開始執行最佳化四、程式碼1. 將初始關鍵幀，當前關鍵幀的描述子轉為BoWpKFini->ComputeBoW();pKFcur->ComputeBoW();不展開詞袋BoW，只需要知道一點，就是我們在迴環檢測的時候，需要用到詞袋向量mBowVec和特徵點向量mFeatVec，所以這裡要計算。
2. 向地圖新增關鍵幀mpAtlas->AddKeyFrame(pKFini);mpAtlas->AddKeyFrame(pKFcur);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]);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++;}}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);非常重要的知識點，好好琢磨，該過程由函式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); } ... mConnectedKeyFrameWeights = KFcounter; mvpOrderedConnectedKeyFrames = vector<KeyFrame*>(lKFs.begin(),lKFs.end()); mvOrderedWeights = vector<int>(lWs.begin(), lWs.end()); ... if(mbFirstConnection && mnId!=mpMap->GetInitKFid()) { // 初始化該關鍵幀的父關鍵幀為共視程度最高的那個關鍵幀 mpParent = mvpOrderedConnectedKeyFrames.front(); // 建立雙向連線關係，將當前關鍵幀作為其子關鍵幀 mpParent->AddChild(this); mbFirstConnection = false; }全域性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) g2o::BlockSolver_6_3::LinearSolverType * linearSolver; linearSolver = new g2o::LinearSolverEigen<g2o::BlockSolver_6_3::PoseMatrixType>();g2o::BlockSolver_6_3 * solver_ptr = new g2o::BlockSolver_6_3(linearSolver);typedef BlockSolver< BlockSolverTraits<6, 3> > BlockSolver_6_3;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); // 邊的關係，其實就是點和關鍵幀之間觀測的關係 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; } } 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); 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); } } 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視覺學習社群