本節的學習要點：
- 初始化的目的(單目/雙目)
- 初始化的兩種方法
- 初始化過程
初始化的目的
單目SLAM初始化的目的是 ==構建初始的三維點雲地圖（空間點）並為之后的計算提供初始值==。
由於僅從單幀的圖像不能得到深度信息，因此需要從圖像序列中選取兩幀以上的圖像以估計相機機姿態並重建出初始的三維點雲。
常見的方法
方法一
追蹤一個已知物體。單幀圖像的每一個點都對應於空間的一條射線。通過不同角度不同位置掃描同一個物體，期望能夠將三維點的不確定性縮小到可接受的范圍。
方法二
基於假設空間存在一個平面物體，選取兩幀不同位置的圖像，通過計算單應矩陣來估計位姿。這類方法在視差較小或者平面上的點靠近某個主點時效果不好。
方法三
根據兩幀之間的特征點匹配計算基礎矩陣，進一步估計位姿。這種方法要求存在不共面的特征點。
單目初始化
方法流程
- 匹配初始幀；
- 位姿計算；
- 三角測量和地圖創建；
- BA優化。
基本步驟
1. 匹配初始幀
該階段工作是，根據連續兩幀圖片中能夠匹配的特征點數量來判斷其是否可以作為初始幀，即只有連續兩幀中能夠匹配的特征點的數量大於某個值時才認為該幀（前一幀）為初始幀；
在ORM-SLAM2中認為連續幀匹配點的數量大於100時可以將前一幀作為初始幀並記錄兩幀的匹配關系；
以下是在ORB-SLAM2中相關部分的代碼，功能為對兩幀圖片進行ORB特征點提取並進行匹配，當匹配的點的數量大於100時認為前一幀可以作為初始幀；可以通過修改代碼中的參數來調整判斷是否能夠作為初始幀的條件。
int nmatches = matcher.SearchForInitialization(mInitialFrame,mCurrentFrame,mvbPrevMatched,mvIniMatches,100);
2. 位姿計算
得到超過100對匹配點后，ORB-SLAM2同時計算適用於平面場景的單應矩陣H和適用於普通場景的基礎矩陣F;
方法是：首先由抽樣點計算出單次抽樣的H（四對點）和F矩陣（八點法）,通過若干次RANSAC抽樣計算出最優的H和F矩陣；然后選擇最合適的結果作為相機的初始位姿。
2.1. 八點法
相機位姿估計問題是為了求解本質矩陣E或者基礎矩陣F，然后求解旋轉R和平移t。
對於E矩陣認為是一個3*3的矩陣，因為任意常數乘以E不變，所以E矩陣的自由度是8。實際上E矩陣的自由度是5（反對稱），但是對於SLAM運算中八點法和五點法區別不大且會增加麻煩所以我們只考慮E矩陣的尺度等價性用八點法來計算），從上式可以看出一對點可以確定一個關於E矩陣的方程，8個自由度就需要8對點來求解E矩陣，這就是八點法。
2.2 計算位姿
在同時計算單應矩陣和基礎矩陣后對兩個模型進行打分選擇得分高的那個模型用來位姿計算，打分是用求得的E矩陣和F矩陣將前一幀上的特征點投影到下一幀並將下一幀的特征點投影到前一幀來計算重投影誤差的和，代碼如下：
cv::Mat Hn = ComputeH21(vPn1i,vPn2i); //計算單應矩陣
currentScore = CheckHomography(H21i, H12i, vbCurrentInliers, mSigma); //進行評分
cv::Mat Fn = ComputeF21(vPn1i,vPn2i);//計算基礎矩陣
currentScore = CheckFundamental(F21i, vbCurrentInliers, mSigma);// 進行評分
float RH = SH/(SH+SF);//計算評分比,如果RH>0.4，選擇單應矩陣來恢復相機位姿，否則選擇基礎矩陣來恢復相機位姿。
在計算完E矩陣和F矩陣並確定模型后，如果是本質矩陣E，進行SVD分解會得到4組可能的R，t，對這些解進行檢查求出唯一真正的解；如果是單應矩陣進行分解（數值法、解析法）得到4組解，利用先驗信息進行排除得到唯一解。
3. 三角測量和地圖創建
已知位姿后通過三角測量可以計算出特征點對應的深度從而生成點雲；並以第一幀為世界坐標系創建地圖並進行數據關聯。
三角測量是指通過在兩處觀察同一個點的夾角從而確定該點的距離。數學上可以從上式進行求解，設$x_1$,$x_2$是兩個特征點的歸一化坐標，那么存在
$$
s_1x_1=s_2Rx_2+t
$$
現在已知$R$,$t$要求解$s_1$,$s_2$,先求解$s_2$,對上式左乘$x_1$^:
數據關聯
- 地圖點與關鍵幀關聯
一個地圖點可被多個關鍵幀觀測到，將觀測到這個地圖點的關鍵幀與這個地圖點進行關聯，同時記錄關鍵幀上哪一個特征點與這個地圖點有關聯。對於單目初始化來說，地圖點需要關聯第一步創建的兩個關鍵幀；地圖點與關鍵幀上的特征點關聯后，計算最合適的描述子來描述該地圖點，用於之后跟蹤的匹配。
- 關鍵幀與地圖點關聯
一個關鍵幀上的特征點由多個地圖點投影而成，將關鍵幀與地圖點關聯。
- 關鍵幀與關鍵幀關聯
關鍵幀之間會共視一個地圖點，如果共視的地圖點個數越多，說明這兩個關鍵幀之間的聯系越緊密。對於某個關鍵幀，統計其與其他關鍵幀共視的特征點個數，如果大於某個閾值，那么將這兩個關鍵幀進行關聯。
- 將關鍵幀和地圖點加入到地圖中
4. BA優化。
初始化的最后一步將對只有兩個關鍵幀的地圖進行BA優化來優化位姿和路標點，以優化后的結果來重新生成點雲地圖。
同局部BA優化來最小化重投影誤差不同，全局BA優化是在求解觀測誤差的最小二乘。由於觀測誤差的最小二乘是非線性的，利用了雅克比矩陣和H矩陣的稀疏性進行邊緣化來簡化運算，其中也使用了圖優化理論。
實際上求解觀測誤差的最小二乘的過程是較復雜的，在這里就不多贅述了。
值得一提的是，由於單目沒有尺度，因此在地圖尺寸初始化時選擇生成點深度的中位數作為單位尺寸1當作基准來進行地圖的尺寸初始化。
雙目初始化
由於雙目和RGB-D相機不需要通過兩個相鄰幀來恢復地圖點深度，所以初始化過程極其相似。
只要得到兩個滿足條件的關鍵幀即可開始初始化。
雙目/RGB-D相機已知若干個特征點的深度（通過雙目匹配、結構光或者飛行時間等深度計算方法），可以求解二維點對應的世界坐標系下的空間點，即已知若干個3D空間點及其投影的位置；此時使用PnP來估計相機運動;PnP問題的求解方法有很多種，包括P3P, DLT,EPnP,UPnP, BA等，其中ORM-SLAM2使用的PnP方法是EPnP,具體計算方不作贅述。
RGB-D初始化
- 初始化的目的是建立三維的空間點和地圖並為之后的計算提供初始值；
- 同雙目/RGB-D SLAM不同，單目SLAM無法從一幀圖片中計算出深度，因此初始化需要兩幀連續滿足條件的圖片來進行初始化；
- 單目SLAM初始化計算位姿是一個對極約束問題，而雙目/RGB-D SLAM的初始化計算位姿是一個PnP問題；
- 在初始化成功后單目SLAM和雙目SLAM一樣是通過PnP來求解相機位姿的；
- 單目SLAM尺度不確定性的原因是因為在通過SVD分解E矩陣求解R，t時計算的t是沒有單位的；
- 單目SLAM初始化的過程中對t進行了歸一化來固定尺度，即以求解的初始幀的t為單位1，而后的軌跡和平移都將以這個t為單位；
- 單目SLAM的初始化一定要有一定程度的平移，純旋轉是無法完成初始化的。