網(wǎng)上有很多關(guān)于pos機(jī)一直顯示網(wǎng)絡(luò)正在初始化,SLAM3 單目地圖初始化的知識，也有很多人為大家解答關(guān)于pos機(jī)一直顯示網(wǎng)絡(luò)正在初始化的問題，今天pos機(jī)之家(www.shineka.com)為大家整理了關(guān)于這方面的知識，讓我們一起來看下吧!
本文目錄一覽：
1、pos機(jī)一直顯示網(wǎng)絡(luò)正在初始化
pos機(jī)一直顯示網(wǎng)絡(luò)正在初始化
來源：公眾號|計(jì)算機(jī)視覺工坊（系投稿）
作者：喬不思
「3D視覺工坊」技術(shù)交流群已經(jīng)成立，目前大約有12000人，方向主要涉及3D視覺、CV&深度學(xué)習(xí)、SLAM、三維重建、點(diǎn)云后處理、自動駕駛、CV入門、三維測量、VR/AR、3D人臉識別、醫(yī)療影像、缺陷檢測、行人重識別、目標(biāo)跟蹤、視覺產(chǎn)品落地、視覺競賽、車牌識別、硬件選型、學(xué)術(shù)交流、求職交流、ORB-SLAM系列源碼交流、深度估計(jì)等。工坊致力于干貨輸出，不做搬運(yùn)工，為計(jì)算機(jī)視覺領(lǐng)域貢獻(xiàn)自己的力量！歡迎大家一起交流成長~
一、前言請閱讀本文之前最好把ORB-SLAM3的單目初始化過程再過一遍（ORB-SLAM3 細(xì)讀單目初始化過程(上)、超詳細(xì)解讀ORB-SLAM3單目初始化（下篇）），以提高學(xué)習(xí)效率。單目初始化過程中最重要的是兩個(gè)函數(shù)實(shí)現(xiàn)，分別是構(gòu)建幀（Frame）和初始化（Track）。接下來，就是完成初始化過程的最后一步：地圖的初始化，是由CreateInitialMapMonocular函數(shù)完成的，本文基于該函數(shù)的流程出發(fā)，目的是為了結(jié)合代碼流程，把單目初始化的上下兩篇的知識點(diǎn)和ORB-SLAM3整個(gè)系統(tǒng)的知識點(diǎn)串聯(lián)起來，系統(tǒng)化零碎的知識，告訴你平時(shí)學(xué)到的各個(gè)小知識應(yīng)用在SLAM系統(tǒng)中的什么位置，達(dá)到快速高效學(xué)習(xí)的效果。
二、CreateInitialMapMonocular 函數(shù)的總體流程1. 將初始關(guān)鍵幀，當(dāng)前關(guān)鍵幀的描述子轉(zhuǎn)為BoW；2. 將關(guān)鍵幀插入到地圖；3. 用三角測量初始化得到的3D點(diǎn)來生成地圖點(diǎn)，更新關(guān)鍵幀間的連接關(guān)系；4. 全局BA優(yōu)化，同時(shí)優(yōu)化所有位姿和三維點(diǎn)；5. 取場景的中值深度，用于尺度歸一化；6. 將兩幀之間的變換歸一化到平均深度1的尺度下；7. 把3D點(diǎn)的尺度也歸一化到1；8. 將關(guān)鍵幀插入局部地圖，更新歸一化后的位姿、局部地圖點(diǎn)。三、必備知識1. 為什么單目需要專門策略生成初始化地圖根據(jù)論文《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系統(tǒng)需要設(shè)計(jì)專門的策略來生成初始化地圖，這也是為什么代碼中單獨(dú)設(shè)計(jì)一個(gè)CreateInitialMapMonocular()函數(shù)來實(shí)現(xiàn)單目初始化，也是我們這篇文章要討論的。為什么要單獨(dú)設(shè)計(jì)呢？就是因?yàn)閱文繘]有深度信息。2） 怎么解決單目沒有深度信息問題？有2種，論文用的是第二種，用一個(gè)具有高不確定度的逆深度參數(shù)來初始化點(diǎn)的深度信息，該參數(shù)會在后期逐漸收斂到真值。3） 說了ORB-SLAM為什么要同時(shí)計(jì)算基礎(chǔ)矩陣F和單應(yīng)矩陣H的原因：這兩種攝像頭位姿重構(gòu)方法在低視差下都沒有很好的約束，所以提出了一個(gè)新的基于模型選擇的自動初始化方法，對平面場景算法選擇單應(yīng)性矩陣，而對于非平面場景，算法選擇基礎(chǔ)矩陣。4）說了ORB-SLAM初始化容易失敗的原因：（條件比較苛刻）在平面的情況下，為了保險(xiǎn)起見，如果最終存在雙重歧義，則算法避免進(jìn)行初始化，因?yàn)榭赡軙驗(yàn)殄e(cuò)誤選擇而導(dǎo)致算法崩潰。因此，我們會延遲初始化過程，直到所選的模型在明顯的視差下產(chǎn)生唯一的解。2. 共視圖 Covisibility Graph共視圖非常的關(guān)鍵，需要先理解共視圖，才能更好的理解后續(xù)程序中如何設(shè)置頂點(diǎn)和邊。
2.1 共視圖定義共視圖是無向加權(quán)圖，每個(gè)節(jié)點(diǎn)是關(guān)鍵幀，如果兩個(gè)關(guān)鍵幀之間滿足一定的共視關(guān)系（至少15個(gè)共同觀測地圖點(diǎn)）他們就連成一條邊，邊的權(quán)重就是共視地圖點(diǎn)數(shù)目。
2.2 共視圖作用2.2.1 跟蹤局部地圖，擴(kuò)大搜索范圍? Tracking::UpdateLocalKeyFrames()
2.2.2 局部建圖里關(guān)鍵幀之間新建地圖點(diǎn)? LocalMapping::CreateNewMapPoints()
? LocalMapping::SearchInNeighbors()
2.2.3 閉環(huán)檢測、重定位檢測? LoopClosing::DetectLoop()、LoopClosing::CorrectLoop()
? KeyFrameDatabase::DetectLoopCandidates
? KeyFrameDatabase::DetectRelocalizationCandidates
2.2.4 優(yōu)化? Optimizer::OptimizeEssentialGraph
3. 地圖點(diǎn) MapPoint 和關(guān)鍵幀 KeyFrame地圖點(diǎn)云保存以下信息：
1）它在世界坐標(biāo)系中的3D坐標(biāo)
2） 視圖方向，即所有視圖方向的平均單位向量（該方向是指連接該點(diǎn)云和其對應(yīng)觀測關(guān)鍵幀光心的射線方向）
3）ORB特征描述子，與其他所有能觀測到該點(diǎn)云的關(guān)鍵幀中ORB描述子相比，該描述子的漢明距離最小
4）根據(jù)ORB特征尺度不變性約束，可觀測的點(diǎn)云的最大距離和最小距離
4. 圖優(yōu)化 Graph SLAM可先看看這些資料[《計(jì)算機(jī)視覺大型攻略 —— SLAM(2) Graph-based SLAM（基于圖優(yōu)化的算法)》](https://blog.csdn.net/plateros/article/details/103498039)，還有《概率機(jī)器人學(xué)》的第11章，深入理解圖優(yōu)化的概念。
我們在文章開頭說過，單目初始化結(jié)果得到了三角測量初始化得到的3D地圖點(diǎn)Pw，計(jì)算得到了初始兩幀圖像之間的相對位姿（相當(dāng)于得到了SE(3)），通過相機(jī)坐標(biāo)系Pc和世界坐標(biāo)系Pw之間的公式，（參考[《像素坐標(biāo)系、圖像坐標(biāo)系、相機(jī)坐標(biāo)系和世界坐標(biāo)系的關(guān)系（簡單易懂版）》](https://blog.csdn.net/shanpenghui/article/details/110481140)）
得到相機(jī)坐標(biāo)系的坐標(biāo)Pc，但是這樣還是不能和像素坐標(biāo)比較。我們接著通過相機(jī)坐標(biāo)系Pc和像素坐標(biāo)系P(u,v)之間的公式
5. g2o使用方法關(guān)于g2o庫的使用方法，可以參考[《G2O圖優(yōu)化基礎(chǔ)和SLAM的Bundle Adjustment(光束平差)》](http://zhaoxuhui.top/blog/2018/04/10/g2o&bundle_adjustment.html#2g2o庫簡介與編譯安裝)和[《理解圖優(yōu)化，一步步帶你看懂g2o代碼》](https://www.cnblogs.com/CV-life/p/10286037.html)。一般來說，g2o的使用流程如下：
5.1創(chuàng)建一個(gè)線性求解器LinearSolver5.2創(chuàng)建BlockSolver，并用上面定義的線性求解器LinearSolver初始化5.3創(chuàng)建總求解器solver，并從GN, LM, DogLeg 中選一個(gè)，再用上述塊求解器BlockSolver初始化5.4創(chuàng)建終極大boss 稀疏優(yōu)化器（SparseOptimizer），并用已定義的總求解器solver作為求解方法5.5定義圖的頂點(diǎn)和邊，并添加到稀疏優(yōu)化器（SparseOptimizer）中5.6設(shè)置優(yōu)化參數(shù)，開始執(zhí)行優(yōu)化四、代碼1. 將初始關(guān)鍵幀，當(dāng)前關(guān)鍵幀的描述子轉(zhuǎn)為BoWpKFini->ComputeBoW();pKFcur->ComputeBoW();
不展開詞袋BoW，只需要知道一點(diǎn)，就是我們在回環(huán)檢測的時(shí)候，需要用到詞袋向量mBowVec和特征點(diǎn)向量mFeatVec，所以這里要計(jì)算。
2. 向地圖添加關(guān)鍵幀mpAtlas->AddKeyFrame(pKFini);mpAtlas->AddKeyFrame(pKFcur);3. 生成地圖點(diǎn)，更新圖（節(jié)點(diǎn)和邊）3.1 遍歷
for(size_t i=0; i<mvIniMatches.size();i++)
因?yàn)橐萌菧y量初始化得到的3D點(diǎn)，所以外圍是一個(gè)大的循環(huán)，遍歷三角測量初始化得到的3D點(diǎn)mvIniP3D。
3.2 檢查if(mvIniMatches[i]<0)continue;
沒有匹配的點(diǎn)，則跳過。
3.3 構(gòu)造點(diǎn)cv::Mat worldPos(mvIniP3D[i]);
用三角測量初始化得到的3D點(diǎn)mvIniP3D[i]作為空間點(diǎn)的世界坐標(biāo) worldPos。
MapPoint* pMP = new MapPoint(worldPos,pKFcur,mpAtlas->GetCurrentMap());
然后用空間點(diǎn)的世界坐標(biāo) worldPos構(gòu)造地圖點(diǎn) pMP。
3.4 修改點(diǎn)屬性3.4.1 添加可以觀測到該地圖點(diǎn)pMP的關(guān)鍵幀pMP->AddObservation(pKFini,i);pMP->AddObservation(pKFcur,mvIniMatches[i]);3.4.2 計(jì)算該地圖點(diǎn)pMP的描述子
pMP->ComputeDistinctiveDescriptors();
因?yàn)镺RBSLAM是特征點(diǎn)方法，描述子非常重要，但是一個(gè)地圖點(diǎn)有非常多能觀測到該點(diǎn)的關(guān)鍵幀，每個(gè)關(guān)鍵幀都有相對該地圖點(diǎn)的值（距離和角度）不一樣的描述子，在這么多的描述子中，如何選取一個(gè)最能代表該點(diǎn)的描述子呢？這里作者用了距離中值法，意思就是說，最能代表該地圖點(diǎn)的描述子，應(yīng)該是與其他描述子具有最小的距離中值。
舉個(gè)栗子，現(xiàn)有描述子A、B、C、D、E、F、G，它們之間的距離分別是1、1、2、3、4、5，求最小距離中值的描述子：
把它們的距離做成2維vector行列的形式，如下：
對每個(gè)關(guān)鍵幀得到的描述子與其他描述子的距離進(jìn)行排序。然后，中位數(shù)是median = vDists[0.5*(N-1)]=0.5×(7-1)=3，得到：
可以看到，描述子B具有最小距離中值，所以選擇描述子B作為該地圖點(diǎn)的描述子。
上述例子比較容易理解，但實(shí)際問題是，描述子是一個(gè)值，如何描述一個(gè)值和另一個(gè)值的距離呢？我們可以把兩個(gè)值看成是兩個(gè)二進(jìn)制串，而描述兩個(gè)二進(jìn)制串之間的距離可以用漢明距離，指的是其不同位數(shù)的個(gè)數(shù)。這樣，我們就可以求出兩個(gè)描述子之間的距離了。
3.4.3 更新該地圖點(diǎn)pMP的平均觀測方向和深度范圍pMP->UpdateNormalAndDepth();
知道方法之后，我們看程序里面MapPoint::UpdateNormalAndDepth()如何實(shí)現(xiàn)：
3.4.3.1 獲取地圖點(diǎn)信息observations=mObservations; // 獲得觀測到該地圖點(diǎn)的所有關(guān)鍵幀pRefKF=mpRefKF; // 觀測到該點(diǎn)的參考關(guān)鍵幀（第一次創(chuàng)建時(shí)的關(guān)鍵幀）Pos = mWorldPos.clone(); // 地圖點(diǎn)在世界坐標(biāo)系中的位置
我們要獲得觀測到該地圖點(diǎn)的所有關(guān)鍵幀，用來找到每個(gè)關(guān)鍵幀的光心Owi。還要獲得觀測到該點(diǎn)的參考關(guān)鍵幀（即第一次創(chuàng)建時(shí)的關(guān)鍵幀），因?yàn)檫@里只是更新觀測方向，距離還是用參考關(guān)鍵幀到該地圖點(diǎn)的距離，體現(xiàn)在后面dist = cv::norm(Pos - pRefKF->GetCameraCenter())。還要獲得地圖點(diǎn)在世界坐標(biāo)系中的位置，用來計(jì)算法向量。
3.4.3.2 計(jì)算該地圖點(diǎn)的法向量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 計(jì)算該地圖點(diǎn)到圖像的距離
cv::Mat PC = Pos - pRefKF->GetCameraCenter();const float dist = cv::norm(PC);
計(jì)算參考關(guān)鍵幀相機(jī)指向地圖點(diǎn)的向量，利用該向量求該地圖點(diǎn)的距離。
3.4.3.4 更新該地圖點(diǎn)的距離上下限// 觀測到該地圖點(diǎn)的當(dāng)前幀的特征點(diǎn)在金字塔的第幾層 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]; // 當(dāng)前金字塔層對應(yīng)的縮放倍數(shù) const int nLevels = pRefKF->mnScaleLevels; // 金字塔層數(shù) { unique_lock<mutex> lock3(mMutexPos); // 使用方法見PredictScale函數(shù)前的注釋 mfMaxDistance = dist*levelScaleFactor; // 觀測到該點(diǎn)的距離上限 mfMinDistance = mfMaxDistance/pRefKF->mvScaleFactors[nLevels-1]; // 觀測到該點(diǎn)的距離下限 mNormalVector = normal/n; // 獲得地圖點(diǎn)平均的觀測方向 }
回顧之前的知識：
3.5 添加地圖點(diǎn)到地圖mpAtlas->AddMapPoint(pMP);3.6 更新圖
非常重要的知識點(diǎn)，好好琢磨，該過程由函數(shù)UpdateConnections完成，深入其中看看有什么奧妙。
3.6.1 統(tǒng)計(jì)共視幀// 遍歷每一個(gè)地圖點(diǎn)for(vector<MapPoint*>::iterator vit=vpMP.begin(), vend=vpMP.end(); vit!=vend; vit++)... // 統(tǒng)計(jì)與當(dāng)前關(guān)鍵幀存在共視關(guān)系的其他幀 map<KeyFrame*,size_t> observations = pMP->GetObservations(); for(map<KeyFrame*,size_t>::iterator mit=observations.begin(), mend=observations.end(); mit!=mend; mit++) ... // 體現(xiàn)了作者的編程功底，很強(qiáng) KFcounter[mit->first]++;
這里代碼主要是想完成遍歷每一個(gè)地圖點(diǎn)，統(tǒng)計(jì)與當(dāng)前關(guān)鍵幀存在共視關(guān)系的其他幀，統(tǒng)計(jì)結(jié)果放在KFcounter。看代碼有點(diǎn)費(fèi)勁，舉個(gè)栗子：已知有一關(guān)鍵幀F(xiàn)1，上面有四個(gè)地圖點(diǎn)ABCD，其中，能觀測到點(diǎn)A的關(guān)鍵幀是有3個(gè)，分別是幀F(xiàn)1、F2、F3。能觀測到點(diǎn)B的關(guān)鍵幀是有4個(gè)，分別是幀F(xiàn)1、F2、F3、F4。能觀測到點(diǎn)C的關(guān)鍵幀是有5個(gè)，分別是幀F(xiàn)1、F2、F3、F4、F5。能觀測到點(diǎn)D的關(guān)鍵幀是有6個(gè)，分別是幀F(xiàn)1、F2、F3、F4、F5、F6。對應(yīng)關(guān)系如下：
總而言之，代碼想統(tǒng)計(jì)的就是與當(dāng)前關(guān)鍵幀存在共視關(guān)系的其他幀，共視關(guān)系是通過能看到同個(gè)特征點(diǎn)來描述的，所以，當(dāng)前幀F(xiàn)1與幀F(xiàn)2的共視關(guān)系為4，當(dāng)前幀F(xiàn)1與幀F(xiàn)3的共視關(guān)系為4，當(dāng)前幀F(xiàn)1與幀F(xiàn)4的共視關(guān)系為3，當(dāng)前幀F(xiàn)1與幀F(xiàn)5的共視關(guān)系為2，當(dāng)前幀F(xiàn)1與幀F(xiàn)6的共視關(guān)系為1。
3.6.2 更新共視關(guān)系大于一定閾值的邊，并找到共視程度最高的關(guān)鍵幀for(map<KeyFrame*,int>::iterator mit=KFcounter.begin(), mend=KFcounter.end(); mit!=mend; mit++) { ... // 找到共視程度最高的關(guān)鍵幀 if(mit->second>nmax) { nmax=mit->second; pKFmax=mit->first; } if(mit->second>=th) { // 更新共視關(guān)系大于一定閾值的邊 vPairs.push_back(make_pair(mit->second,mit->first)); // 更新其它關(guān)鍵幀與當(dāng)前幀的連接權(quán)重 (mit->first)->AddConnection(this,mit->second); } }
假設(shè)共視關(guān)系閾值為1，在上面這個(gè)例子中，只要和當(dāng)前幀有共視關(guān)系的幀都需要更新邊，由于在這之前，關(guān)鍵幀只和地圖點(diǎn)之間有連接關(guān)系，和其他幀沒有連接關(guān)系，要構(gòu)建共視圖（以幀為節(jié)點(diǎn)，以共視關(guān)系為邊）就要一個(gè)個(gè)更新節(jié)點(diǎn)之間的邊的值。
(mit->first)->AddConnection(this,mit->second)的含義是更新其他幀F(xiàn)i和當(dāng)前幀F(xiàn)1的邊（因?yàn)楫?dāng)前幀F(xiàn)1也被當(dāng)做其他幀F(xiàn)i的有共視關(guān)系的一個(gè)）。在遍歷查找共視關(guān)系最大幀的時(shí)候同步做這個(gè)事情，可以加速計(jì)算和高效利用代碼。mit->first在這里，代表和當(dāng)前幀有共視關(guān)系的F2...F6（因?yàn)楸闅v的是KFcounter，存儲著與當(dāng)前幀F(xiàn)1有共視關(guān)系的幀F(xiàn)2...F6）。舉個(gè)栗子，當(dāng)處理當(dāng)前幀F(xiàn)1和共視幀F(xiàn)2時(shí)，更新與幀F(xiàn)2有共視關(guān)系的幀F(xiàn)1，以此類推，當(dāng)處理當(dāng)前幀F(xiàn)1和共視幀F(xiàn)3時(shí)，更新與幀F(xiàn)3有共視關(guān)系的幀F(xiàn)1....。
3.6.3 如果沒有連接到關(guān)鍵幀（沒有超過閾值的權(quán)重），則連接權(quán)重最大的關(guān)鍵幀if(vPairs.empty()) { vPairs.push_back(make_pair(nmax,pKFmax)); pKFmax->AddConnection(this,nmax); }
如果每個(gè)關(guān)鍵幀與它共視的關(guān)鍵幀的個(gè)數(shù)都少于給定的閾值，那就只更新與其它關(guān)鍵幀共視程度最高的關(guān)鍵幀的 mConnectedKeyFrameWeights，以免之前這個(gè)閾值可能過高造成當(dāng)前幀沒有共視幀，容易造成跟蹤失??？（自己猜的）
3.6.4 對共視程度比較高的關(guān)鍵幀對更新連接關(guān)系及權(quán)重（從大到?。?/strong>sort(vPairs.begin(),vPairs.end()); // 將排序后的結(jié)果分別組織成為兩種數(shù)據(jù)類型 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 更新當(dāng)前幀的信息
... mConnectedKeyFrameWeights = KFcounter; mvpOrderedConnectedKeyFrames = vector<KeyFrame*>(lKFs.begin(),lKFs.end()); mvOrderedWeights = vector<int>(lWs.begin(), lWs.end());3.6.6 更新生成樹的連接
... if(mbFirstConnection && mnId!=mpMap->GetInitKFid()) { // 初始化該關(guān)鍵幀的父關(guān)鍵幀為共視程度最高的那個(gè)關(guān)鍵幀 mpParent = mvpOrderedConnectedKeyFrames.front(); // 建立雙向連接關(guān)系，將當(dāng)前關(guān)鍵幀作為其子關(guān)鍵幀 mpParent->AddChild(this); mbFirstConnection = false; }4. 全局BA
全局BA主要是由函數(shù)GlobalBundleAdjustemnt完成的，其調(diào)用了函數(shù)BundleAdjustment，建議開始閱讀之前復(fù)習(xí)一下文章前面的《二、4. 圖優(yōu)化 Graph SLAM》和《二、5. g2o使用方法》，下文直接從函數(shù)BundleAdjustment展開敘述。
// 調(diào)用 Optimizer::GlobalBundleAdjustemnt(mpAtlas->GetCurrentMap(),20);// 定義void Optimizer::GlobalBundleAdjustemnt(Map* pMap, int nIterations, bool* pbStopFlag, const unsigned long nLoopKF, const bool bRobust)//調(diào)用 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創(chuàng)建方法求解器solver，選擇非線性最小二乘解法（高斯牛頓GN、LM、狗腿DogLeg等），AlgorithmSolver是我自己想出來的名字，方便記憶。
4.4 稀疏優(yōu)化器 SparseOptimizerg2o::SparseOptimizer optimizer; optimizer.setAlgorithm(solver);// 用前面定義好的求解器作為求解方法 optimizer.setVerbose(false);// 設(shè)置優(yōu)化過程輸出信息用的
用solver創(chuàng)建稀疏優(yōu)化器SparseOptimizer。
4.5 定義圖的頂點(diǎn)和邊，添加到稀疏優(yōu)化器SparseOptimizer在開始看具體步驟前，注意兩點(diǎn)，一是ORB-SLAM3中圖的定義，二是其誤差模型，理解之后才可能明白為什么初始化過程中要操作這些變量。
4.5.1 圖的定義4.5.1.1 圖的節(jié)點(diǎn)和邊再計(jì)算相機(jī)坐標(biāo)系坐標(biāo)Pc轉(zhuǎn)換到像素坐標(biāo)系下的坐標(biāo)P(u,v)，利用如下公式，EdgeSE3ProjectXYZ::cam_project函數(shù)實(shí)現(xiàn)（types_six_dof_expmap.cpp）：
結(jié)合代碼，可以看看下圖的示意（點(diǎn)擊查看高清大圖）：
4.5.1.2 設(shè)置節(jié)點(diǎn)和邊的步驟和把大象放冰箱的步驟一樣的簡單，設(shè)置頂點(diǎn)和邊的步驟總共分三步：
1. 設(shè)置類型是關(guān)鍵幀位姿的節(jié)點(diǎn)信息：位姿（SE3）、編號（setId(pKF->mnId)）、最大編號（maxKFid）；
2. 設(shè)置類型是地圖點(diǎn)坐標(biāo)的節(jié)點(diǎn)信息：位姿（3dPos）、編號（setId(pMp->mnId+maxKFid+1)）、計(jì)算的變量（setMarginalized）；【為什么要設(shè)置setMarginalized，感興趣的同學(xué)可以自己參考一下這篇文章[《g2o：非線性優(yōu)化與圖論的結(jié)合》](https://zhuanlan.zhihu.com/p/37843131)，這里就不過多贅述了】
3. 設(shè)置邊的信息：連接的節(jié)點(diǎn)（setVertex）、信息矩陣（setInformation）、計(jì)算觀測值的相關(guān)參數(shù)（setMeasurement/fx/fy/cx/cy）、核函數(shù)（setRobustKernel）。【引入魯棒核函數(shù)是人為降低過大的誤差項(xiàng)，可以更加穩(wěn)健地優(yōu)化，具體請參考《視覺十四講》第10講】
4.5.1.3 ORB-SLAM3新增部分ORB-SLAM3中新增了單獨(dú)記錄邊、地圖點(diǎn)和關(guān)鍵幀的容器，比如單目中的vpEdgesMono、vpEdgeKFMono和vpMapPointEdgeMono，分別記錄的是誤差值、關(guān)鍵幀和地圖點(diǎn)，目的是在獲取優(yōu)化后的關(guān)鍵幀位姿時(shí)，使用該誤差值vpEdgesMono[i]，對地圖點(diǎn)vpMapPointEdgeMono[i]進(jìn)行自由度為2的卡方檢驗(yàn)e->chi2()>5.991，以此排除外點(diǎn)，選出質(zhì)量好的地圖點(diǎn)，見源碼[Optimizer.cc#L337](https://github.com/UZ-SLAMLab/ORB_SLAM3/blob/ef9784101fbd28506b52f233315541ef8ba7af57/src/Optimizer.cc#L337)。為了不打斷圖優(yōu)化思路，不過多展開ORB-SLAM2和3的區(qū)別，感興趣的同學(xué)可自行研究源碼。
4.5.2 誤差模型SLAM中要計(jì)算的誤差如下示意：
其中，
是觀測誤差，對應(yīng)到代碼中就是，用觀測值【即校正后的特征點(diǎn)坐標(biāo)mvKeysUn，是Frame類的UndistortKeyPoints函數(shù)獲取的】，減去其估計(jì)值【即地圖點(diǎn)mvIniP3D，該點(diǎn)是ReconstructF或者ReconstructH中，利用三角測量得到空間點(diǎn)坐標(biāo)，之后把該地圖點(diǎn)mvIniP3D投影到圖像上，得到估計(jì)的特征點(diǎn)坐標(biāo)P(u,v)】。Q是觀測噪聲，對應(yīng)到代碼中就是協(xié)方差矩陣sigma（而且還和特征點(diǎn)所在金字塔層數(shù)有關(guān)，層數(shù)越高，噪聲越大）。
4.5.3 步驟一，添加關(guān)鍵幀位姿頂點(diǎn)// 對于當(dāng)前地圖中的所有關(guān)鍵幀 for(size_t i=0; i<vpKFs.size(); i++) { KeyFrame* pKF = vpKFs[i]; // 去除無效的 if(pKF->isBad()) continue; // 對于每一個(gè)能用的關(guān)鍵幀構(gòu)造SE3頂點(diǎn),其實(shí)就是當(dāng)前關(guān)鍵幀的位姿 g2o::VertexSE3Expmap * vSE3 = new g2o::VertexSE3Expmap(); vSE3->setEstimate(Converter::toSE3Quat(pKF->GetPose())); vSE3->setId(pKF->mnId); // 只有第0幀關(guān)鍵幀不優(yōu)化（參考基準(zhǔn)） vSE3->setFixed(pKF->mnId==0); // 向優(yōu)化器中添加頂點(diǎn)，并且更新maxKFid optimizer.addVertex(vSE3); if(pKF->mnId>maxKFid) maxKFid=pKF->mnId; }
注意三點(diǎn)：
- 第0幀關(guān)鍵幀作為參考基準(zhǔn)，不優(yōu)化
- 只需設(shè)置SE(3)和Id即可
- 需要更新maxKFid，以便下方添加觀測值（相機(jī)3D位姿）時(shí)使用
4.5.4 步驟二，添加地圖點(diǎn)位姿頂點(diǎn)// 卡方分布 95% 以上可信度的時(shí)候的閾值 const float thHuber2D = sqrt(5.99); // 自由度為2 const float thHuber3D = sqrt(7.815); // 自由度為3 // Set MapPoint vertices // Step 2.2：向優(yōu)化器添加MapPoints頂點(diǎn) // 遍歷地圖中的所有地圖點(diǎn) for(size_t i=0; i<vpMP.size(); i++) { MapPoint* pMP = vpMP[i]; // 跳過無效地圖點(diǎn) if(pMP->isBad()) continue; // 創(chuàng)建頂 g2o::VertexSBAPointXYZ* vPoint = new g2o::VertexSBAPointXYZ(); // 注意由于地圖點(diǎn)的位置是使用cv::Mat數(shù)據(jù)類型表示的,這里需要轉(zhuǎn)換成為Eigen::Vector3d類型 vPoint->setEstimate(Converter::toVector3d(pMP->GetWorldPos())); // 前面記錄maxKFid 是在這里使用的 const int id = pMP->mnId+maxKFid+1; vPoint->setId(id); // g2o在做BA的優(yōu)化時(shí)必須將其所有地圖點(diǎn)全部schur掉，否則會出錯(cuò)。 // 原因是使用了g2o::LinearSolver<BalBlockSolver::PoseMatrixType>這個(gè)類型來指定linearsolver, // 其中模板參數(shù)當(dāng)中的位姿矩陣類型在程序中為相機(jī)姿態(tài)參數(shù)的維度，于是BA當(dāng)中schur消元后解得線性方程組必須是只含有相機(jī)姿態(tài)變量。 // Ceres庫則沒有這樣的限制 vPoint->setMarginalized(true); optimizer.addVertex(vPoint);4.5.5 步驟三，添加邊
// 邊的關(guān)系，其實(shí)就是點(diǎn)和關(guān)鍵幀之間觀測的關(guān)系 const map<KeyFrame*,size_t> observations = pMP->GetObservations(); // 邊計(jì)數(shù) int nEdges = 0; //SET EDGES // Step 3：向優(yōu)化器添加投影邊（是在遍歷地圖點(diǎn)、添加地圖點(diǎn)的頂點(diǎn)的時(shí)候順便添加的） // 遍歷觀察到當(dāng)前地圖點(diǎn)的所有關(guān)鍵幀 for(map<KeyFrame*,size_t>::const_iterator mit=observations.begin(); mit!=observations.end(); mit++) { KeyFrame* pKF = mit->first; // 濾出不合法的關(guān)鍵幀 if(pKF->isBad() || pKF->mnId>maxKFid) continue; nEdges++; const cv::KeyPoint &kpUn = pKF->mvKeysUn[mit->second]; if(pKF->mvuRight[mit->second]<0) { // 如果是單目相機(jī)按照下面操作 // 構(gòu)造觀測 Eigen::Matrix<double,2,1> obs; obs << kpUn.pt.x, kpUn.pt.y; // 創(chuàng)建邊 g2o::EdgeSE3ProjectXYZ* e = new g2o::EdgeSE3ProjectXYZ(); // 填充數(shù)據(jù)，構(gòu)造約束關(guān)系 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); // 信息矩陣，也是協(xié)方差，表明了這個(gè)約束的觀測在各個(gè)維度（x,y）上的可信程度，在我們這里對于具體的一個(gè)點(diǎn)，兩個(gè)坐標(biāo)的可信程度都是相同的， // 其可信程度受到特征點(diǎn)在圖像金字塔中的圖層有關(guān)，圖層越高，可信度越差 // 為了避免出現(xiàn)信息矩陣中元素為負(fù)數(shù)的情況，這里使用的是sigma^(-2) const float &invSigma2 = pKF->mvInvLevelSigma2[kpUn.octave]; e->setInformation(Eigen::Matrix2d::Identity()*invSigma2); // 如果要使用魯棒核函數(shù) if(bRobust) { g2o::RobustKernelHuber* rk = new g2o::RobustKernelHuber; e->setRobustKernel(rk); // 這里的重投影誤差，自由度為2，所以這里設(shè)置為卡方分布中自由度為2的閾值，如果重投影的誤差大約大于1個(gè)像素的時(shí)候，就認(rèn)為不太靠譜的點(diǎn)了， // 核函數(shù)是為了避免其誤差的平方項(xiàng)出現(xiàn)數(shù)值上過大的增長 rk->setDelta(thHuber2D); } // 設(shè)置相機(jī)內(nèi)參 // 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相機(jī)按照下面操作 ......這里忽略，只講單目 } } // 向優(yōu)化器添加投影邊,也就是遍歷所有觀測到當(dāng)前地圖點(diǎn)的關(guān)鍵幀 // 如果因?yàn)橐恍┨厥庠?實(shí)際上并沒有任何關(guān)鍵幀觀測到當(dāng)前的這個(gè)地圖點(diǎn),那么就刪除掉這個(gè)頂點(diǎn),并且這個(gè)地圖點(diǎn)也就不參與優(yōu)化 if(nEdges==0) { optimizer.removeVertex(vPoint); vbNotIncludedMP[i]=true; } else { vbNotIncludedMP[i]=false; } } 4.5.6 優(yōu)化
optimizer.initializeOptimization(); optimizer.optimize(nIterations);
添加邊設(shè)置優(yōu)化參數(shù)，開始執(zhí)行優(yōu)化。
5. 計(jì)算深度中值float medianDepth = pKFini->ComputeSceneMedianDepth(2); float invMedianDepth = 1.0f/medianDepth;
這里開始，5到7是比較關(guān)鍵的轉(zhuǎn)換，要理解這部分背后的含義，我們需要回顧一下相機(jī)模型，復(fù)習(xí)一下各個(gè)坐標(biāo)系之間的轉(zhuǎn)換關(guān)系，再看代碼就簡單多了。
5.1 相機(jī)模型與坐標(biāo)系轉(zhuǎn)換很多人看了n遍相機(jī)模型，小孔成像原理爛熟于心，但那只是爛熟，并沒有真正應(yīng)用到實(shí)際，想真正掌握，認(rèn)真看下去。先復(fù)習(xí)一下相機(jī)投影的過程，也可參考該文[《像素坐標(biāo)系、圖像坐標(biāo)系、相機(jī)坐標(biāo)系和世界坐標(biāo)系的關(guān)系（簡單易懂版）》](https://blog.csdn.net/shanpenghui/article/details/110481140)，如圖（點(diǎn)擊查看高清大圖）：
再來弄清楚各個(gè)坐標(biāo)系之間的轉(zhuǎn)換關(guān)系，認(rèn)真研究下圖，懂了之后能解決很多心里的疑問（點(diǎn)擊查看高清大圖）：
總之，圖像上的像素坐標(biāo)和世界坐標(biāo)的關(guān)系是：
其中，zc是相機(jī)坐標(biāo)系下的坐標(biāo)；dx和dy分別表示每個(gè)像素在橫軸x和縱軸y的物理尺寸，單位為毫米/像素；u0,v0表示的是圖像的中心像素坐標(biāo)和圖像圓點(diǎn)像素坐標(biāo)之間相差的橫向和縱向像素?cái)?shù)；f是相機(jī)的焦距，R,T是旋轉(zhuǎn)矩陣和平移矩陣，xw,yw,zw是世界坐標(biāo)系下的坐標(biāo)。
5.2 歸一化平面講歸一化平面的資料比較少，可參考性不高。大家也不要把這個(gè)東西看的有多玄乎，其實(shí)就是一個(gè)數(shù)學(xué)技巧，主要是為了方便計(jì)算。從上面的公式可以看到，左邊還有個(gè)zc的因數(shù)，除掉這個(gè)因數(shù)的過程其實(shí)就可以叫歸一化。代碼中接下來要講的幾步其實(shí)都可以歸結(jié)為以下這個(gè)公式：
6. 歸一化兩幀變換到平均深度為1cv::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點(diǎn)的尺度歸一化
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. 將關(guān)鍵幀插入局部地圖
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(); // 初始化成功，至此，初始化過程完成五、總結(jié)
總之，初始化地圖部分，重要的支撐在于兩個(gè)點(diǎn)：
1. 理解圖優(yōu)化的概念，包括ORB-SLAM3是如何定義圖的，頂點(diǎn)和邊到底是什么，他們有什么關(guān)系，產(chǎn)生這種關(guān)系背后的公式是什么，搞清楚這些，圖優(yōu)化就算入門了吧，也可以看得懂地圖初始化部分了；2. 相機(jī)模型，以及各個(gè)坐標(biāo)系之間的關(guān)系，大多數(shù)人還是停留在大概理解的層面，需要結(jié)合代碼實(shí)際來加深對它的理解，因?yàn)檎麄€(gè)視覺SLAM就是多視圖幾何理論的天下，不懂這些就扎近茫茫代碼中，很容易迷失。至此，初始化過程完結(jié)了。我們通過初始化過程認(rèn)識了ORB-SLAM3系統(tǒng)，但只是管中窺豹，看不到全面，想要更加深入的挖掘，還是要多多拆分源碼，一個(gè)個(gè)模塊掌握，然后才能轉(zhuǎn)化成自己的東西。以上都是各人見解，如有紕漏，請各位不吝賜教，O(∩_∩)O謝謝。
六、參考1. [ORB-SLAM: a Versatile and Accurate Monocular SLAM System](https://blog.csdn.net/weixin_42905141/article/details/102857958)
2. [ORB-SLAM3 細(xì)讀單目初始化過程（上）](https://blog.csdn.net/shanpenghui/article/details/109809723#t10)
3. [理解圖優(yōu)化，一步步帶你看懂g2o代碼](https://www.cnblogs.com/CV-life/p/10286037.html)
4. [ORB-SLAM2 代碼解讀（三）：優(yōu)化 1（概述）](https://wym.netlify.app/2019-07-03-orb-slam2-optimization1/)
5. [視覺slam十四講 6.非線性優(yōu)化](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. 《概率機(jī)器人》 [美] Sebastian Thrun / [德] Wolfram Burgard / [美] Dieter Fox 機(jī)械工業(yè)出版社
備注：作者也是我們「3D視覺從入門到精通」特邀嘉賓：一個(gè)超干貨的3D視覺學(xué)習(xí)社區(qū)
本文僅做學(xué)術(shù)分享，如有侵權(quán)，請聯(lián)系刪文。
以上就是關(guān)于pos機(jī)一直顯示網(wǎng)絡(luò)正在初始化,SLAM3 單目地圖初始化的知識，后面我們會繼續(xù)為大家整理關(guān)于pos機(jī)一直顯示網(wǎng)絡(luò)正在初始化的知識，希望能夠幫助到大家！
