算法目的
位姿估計 + 建圖
在建圖的同時不斷優化自身位姿 提高建圖的准確度
為廣大視覺SLAM算法打下了基礎
算法應用場景
- 室內室外均可
- 有明顯的並且數量合理的回環檢測
- 單目、雙目、RGBD
- 算法本身可能無法直接使用 但是為后續算法提供了模版框架
算法優點
- 室內室外均可
- 有明顯的並且數量合理的回環檢測
- 單目、雙目、RGBD
- 算法本身可能無法直接使用 但是為后續算法提供了模版框架
相關概念點
特征點：提取的是ORB特征 包括特征點和它的描述子
關鍵幀：由若干幀中選出的重要的幀（依據選取間隔、特征點數量等）
參考關鍵幀：構建這個地圖點的關鍵幀為參考關鍵幀。
地圖點：由關鍵幀的特征點三角化出的地圖點
BA：對位姿和地圖點同時進行優化（EM）
BOW：詞袋 是特征點的聚類
BOW向量：BOW相似度得分 用一個向量描述整個圖像（圖像中出現狗 貓 但是沒有汽車 記為【1， 1， 0】）得分就是計算兩個圖像之間的差值
ORB特征點：如角點 邊緣點 特征點法中ORB特征點 + 相對應的描述子
描述子：對ORB特征點的位置進行描述
光流法：最小化光度誤差 估計相機的運動 不實用描述子但使用特征點
直接法：先估計相機的運動位姿 計算前后兩幀相對應的像素點的灰度值 通過減小灰度值的差值 優化相機位姿（可以使用任意像素點而不一定使用特征點 更不需要描述子）
內涵算法
三角化：三角測量算法 根據兩幀圖像恢復出深度 即地圖點
對極幾何：根據兩幀單目圖像恢復出相機運動 生成對極約束 對極約束中包括基礎矩陣和本質矩陣 實際中通過求解本質矩陣得到相機變換位姿更多 但本算法使用的是基礎矩陣
單應矩陣：恢復相機運動 場景中的特征點都落在了同一平面上 則可以通過單應矩陣進行求解
PnP：已知一幀圖像的三維點和另一幀圖像的二維點計算出相機位姿
RANSAC：將圖像中的外點刨除 外點會極大影響計算結果 多次迭代擬合保留內點
BA優化：e = z - h(T, p) z為像素坐標 T為相機位姿（外參）對應的李群 p為三維點坐標 優化變量：R、t和p坐標 已知：z
sim3：用三對關鍵幀對應的地圖點進行位姿求解（包括旋轉矩陣、平移矩陣、尺度變換）
算法實現
tracking線程
三大線程：跟蹤線程（主線程）、局部建圖線程、回環檢測線程
步驟
- ORB 特征點提取
- 相機初始化
- 根據前一幀圖片估計出當前幀的位姿
- 基於當前幀的位姿構建出局部地圖 局部地圖的作用是優化當前幀的位姿
- 決定是否生成關鍵幀
單目相機初始化
成功條件
連續兩幀間成功三角化超過100個點，則初始化成功
初始化幀即為參考幀 第二幀的特征點個數要大與100
單目相機初始化器
當特征點共面或者相機發生純旋轉時 基礎矩陣的自由度下降 如果此時繼續使用基礎矩陣進行求解 則求解結果會極大的受到噪聲的影響 為了避免這種情況的發生 本算法采用H和F同時計算的方法 選取卡方檢驗得分較高的那個結果
單目相機初始化步驟：
- 若單目相機初始化器還沒創建，則創建初始化器
- 若上一幀特征點數量足夠但是當前幀的特征點數量過少 則匹配失敗 刪除初始化器
- 在初始化幀和當前幀進行特征匹配
- 若匹配的特征點數目太少，則匹配失敗，刪除初始化器
- 進行單目初始化
- 創建初始化地圖
初始化后續
- 將兩個關鍵幀插入地圖
- 處理所有的地圖點（添加地圖點 構建關鍵幀和地圖點的雙向聯系等）
- 全局BA 優化所有關鍵點位姿和地圖點
- 歸一化平移尺度和坐標點尺度 更新地圖點坐標和位姿
初始位姿估計
初始化成功后 每次傳入一幀新圖像 都會先進行初始位姿估計 然后生成局部地圖 再對局部地圖中的地圖點和當前位姿進行BA
恆速運動模型估計：認為速度恆定 進行匹配 若匹配成功（詞袋匹配點大於10）則認為速度恆定 根據速度恆定模型計算初始化位姿 + BA
參考幀估計位姿：將當前關鍵幀共視點最多的幀設為關鍵參考幀 與這一個關鍵參考幀幀進行匹配 若匹配成功則使用上一幀的位姿作為初始化的位姿 + BA
重定位估計位姿：根據詞袋相似度尋找到與當前幀相近的關鍵幀（設為參考關鍵幀）
對每一個參考關鍵幀都初始化一個PnPsolver 保存成指針向量
每一個PnPsolver都進行最多三次特征匹配
第一次特征匹配：計算初始位姿 + BA只優化位姿（優化位姿過程中對內點和外點根據進行區分 如果g2o :: chi2誤差較大 此邊的值和和其他邊不符 認為是外點）如果內點數量達不到要求 進行第二次特征匹配
第二次特征匹配：通過投影的方式將關鍵幀中未匹配的地圖點投影到當前幀中, 生成新的匹配 重復上述操作
第三次特征匹配：如果第二次內點數量在30-50之間 進行最后一次搶救 重新投影匹配 使用更嚴格的描述子閾值
步驟：
- 檢查是否初始化
- 檢查恆速運動莫模型估計是否成功
- 若沒成功則使用參考幀估計位姿
- 若還沒成功則使用重定位估計位姿
- 若還沒成功則標記跟蹤lose
跟蹤局部地圖
成功估計當前幀的初始化位姿后 基於當前位姿更新局部地圖並優化當前幀位姿 流程：
- 更新局部地圖
- 將局部地圖點投影到當前幀特征點上
- BA 優化當前幀位姿
- 更新地圖點觀測數值 統計內點個數
- 根據內點個數判斷是否跟蹤成功
關鍵幀的創建
關鍵幀的要求：
當前幀的質量 當前幀的地圖點要足夠多 同時與參考關鍵幀的重合程度不能太大
時間間隔
是否進行過重定位 如果進行過重定位 重定位后的位姿不會太准 不能當作參考幀
參考關鍵幀
根據參考關鍵幀估計初始位姿
根據參考幀估計地圖點的平均觀測距離
參考幀的指定：
創建新關鍵幀后會將新創建的關鍵幀設為參考關鍵幀
將當前關鍵幀共視程度最高的關鍵幀設為參考關鍵幀
局部建圖線程
構建地圖步驟
run函數：死循環
- 檢查緩沖隊列中是否含有關鍵幀
- 處理緩沖隊列中的第一個關鍵幀
- 剔除壞點
- 創建新地圖點補充壞點（對極幾何或三角化）
- 將當前關鍵幀與共視關鍵幀地圖點融合
- 局部BA優化
- 剔除冗余關鍵幀
- 將當前關鍵幀加入閉環檢測中
好的地圖點標准
如果地圖點經過了連續三個關鍵幀未被剔除 則認為是好的地圖點
剔除准則：
召回率 < 0.25 && 創建的三幀內觀測數目少於2
創建新地圖點
將當前關鍵幀分別與共視程度最高的前10(單目相機取20)個共視關鍵幀兩兩進行特征匹配,生成地圖點.
對於雙目相機的匹配特征點對,可以根據某幀特征點深度恢復地圖點,也可以根據兩幀間對極幾何三角化地圖點,這里取視差角最大的方式來生成地圖點.
地圖點融合
在將地圖點反投影到幀中的過程中,存在以下兩種情況:
- 若地圖點反投影對應位置上不存在地圖點，則直接添加觀測
- 若地圖點反投影位置上存在對應地圖點，則將兩個地圖點合並到其中觀測較多的那個（替換）
冗余關鍵幀的標准
關鍵幀90%的地圖點可以被超過3個其他關鍵幀觀測到
回環檢測線程
步驟
- 取出緩沖隊列頭部關鍵幀 作為當前檢測閉環關鍵幀
- 如果距離上次回環檢測時間太短 則不進行回環檢測
- 計算當前關鍵幀與共視關鍵幀間的最大相似度（后面有詳細解釋）
- 根據相似度尋找當前關鍵幀的閉環候選關鍵幀
- 在當前的關鍵幀組和之前的連續關鍵幀組之間尋找匹配
- 維護循環變量 當前幀的閉環候選關鍵幀組當作下一幀之前的閉環候選關鍵幀組
回環匹配
- 尋找當前關鍵幀的共視關鍵幀並計算當前幀和共視關鍵幀的BOW相似度得分 求出最低得分
- 選取閉環候選關鍵幀與當前幀的共視關系（BOW相似度得分）要大於最低得分 則認為匹配成功
- 將所有匹配成功的量層層過濾 選取過濾之后相似度得分最高的關鍵幀作為最終的閉環候選幀
- 對最終的閉環候選幀做連續性檢測
連續性檢測
- 每個閉環候選關鍵幀將與共視關鍵幀構成一個子候選組
- 檢測“子候選組”中每一個關鍵幀是否存在於“連續組”，如果存在 nCurrentConsistency++，則將該“子候選組”放入“當前連續組vCurrentConsistentGroups”
- 如果nCurrentConsistency大於等於3，那么該“子候選組”代表的候選關鍵幀過關
通過連續性檢測的候選關鍵幀為准備進入閉環矯正的關鍵幀
更詳細的解釋
連續的概念：不同組之間有一個及以上共同關鍵幀
連續鏈：1--2--3--4 防止形成網狀結構 采用菊花鏈的方式
對連續組進行維護 存的是子候選組 每個子候選組都維護 ‘連續鏈’ 這個變量
連續組更新方式：所有被檢測到與當前子候選組有關聯的 都會在連續組鏈后面 + 1
sim3 + 閉環矯正
矯正前停掉局部建圖線程和局部BA線程
- 尋找兩關鍵幀之間的粗匹配來粗略估計sim3
- 由粗略估計出的sim3來尋找兩關鍵幀之間更多的匹配
- 構建圖優化模型，固定地圖點通過兩關鍵幀的地圖點到兩幀的重投影誤差來優化sim3
- 本質圖BA優化 優化所有地圖點和關鍵幀位姿
算法對比
PTAM
PTAM最大的貢獻是提出了tracking、mapping雙線程的架構
tracking線程只需要逐幀更新相機位置姿態，可以很容易實現實時計算
而mapping線程並不需要逐幀更新，有更長的處理時間 這種基於優化的算法比濾波法在單位計算時間可以得到更高的精度。
這種多線程的處理方式也更順應現代CPU的發展趨勢。之后的視覺SLAM算法幾乎全部沿用了這一思想。
SVO SLAM
SVO算法：結合了特征點法和直接法的視覺里程計
SVO算法首先使用了圖像中的特征點 但是沒有計算描述子（描述子的計算時間漫長 但可以描述特征點所在位置）通過初始化相機位姿來大約估計兩幀圖像之間匹配同一地圖點的兩個像素點 計算兩個像素點的灰度值並構建灰度誤差函數 通過最小化灰度誤差函數來優化兩幀間的相機位姿（相機位姿如果不夠好 兩個像素點的灰度值會相差很大）
SVO只是一種里程計 高博將SVO和ORBSLAM算法相結合 跟蹤速度是原來的3倍
VIO SLAM
視覺慣性里程計/視覺慣性系統
將相機和IMU數據進行融合 ORB-SLAM中運動估計采用構建重投影誤差方程來優化運動估計 在VIO SLAM中直接使用IMU的數據進行運動估計
根據是否把圖像特征加入特征向量中 分為緊耦合算法和松耦合算法
松耦合：視覺運動估計和慣導運動估計分為兩個進程 將最后的結果進行融合
緊耦合：充分利用了傳感器數據 但是算法整體的維度特別高 需要很高的計算量
緊耦合包括算法：MSCKF、ROVIO
DS-SLAM
在動態環境中對運動的物體進行了更好的處理 和ORB SLAM相比具有更高的精度和魯棒性
Dyna-SLAM
同Dyna-SLAM 處理了動態環境中對運動的物體 Mask R-CNN + 多視圖幾何檢測動態物體
二者融合后便可以檢測出那些運動的沒有先驗知識的物體（例如，被人推動的椅子，拿着的書本）。最后只使用靜態區域ORB特征點進行相機位姿估計。由動態區域的Mask篩選得到靜態區域的特征點，進而進行跟蹤和建圖，此過程與ORB-SLAM完全相同。
DSO-SLAM
使用直接法 是傳統特征點法的五倍速度 但保證了原始的精度
ORB-SLAM3
增加了IMU和魚眼相機 提高了魯棒性和精度 以及以下的細節：
- 保存了很多瑣碎的地圖：主要的作用發揮在Tracking線程Lost時，之前Lost后需要回到原先的位置進行Relocalization()，但是現在，如果tracking線程丟失，ORBSLAM3會在之前的所有小地圖中進行查詢匹配，如果匹配成功，則tracking線程繼續；如果匹配不成功，則重新開辟一個小的地圖；
- 新增最大后驗概率，用於初始化並且提煉IMU數據
- loop and map merging 線程：因為有了很多瑣碎的地圖，因此新增了map merging的部分，如果兩個地圖有重合部分，且重合處屬於當前Active的Map，則對其進行LoopCorrection，類似於之前的檢測到回環。如果兩個地圖重合的部分屬於別的地圖，這就類似於找到了其他兩個地圖的重合點，因此就將他們merge到一起。
算法缺點
- 初始化時最好保持低速運動，對准特征和幾何紋理豐富的物體。
- 旋轉時比較容易丟幀，特別是對於純旋轉，對噪聲敏感，不具備尺度不變性。
- 如果使用純視覺 slam 用於機器人導航，可能會精度不高，或者產生累積誤差，漂移，盡管可以使用 DBoW 詞袋可以用來回環檢測。最好使用 VSLAM+IMU 進行融合，可以提高精度上去，適用於實際應用中機器人的導航。
- 代碼中的bug和性能有待提升等問題
其他
共視圖、擴展樹、本質圖
共視圖
如果兩個關鍵幀的共視地圖點超過了15個 就增加一條邊 視為共視。所有的這些邊連在一起形成了共視圖。
共視圖是無向加權圖 邊的權重就是共視地圖點的數目
作用：
跟蹤局部地圖擴大搜索范圍
局部建圖里關鍵幀之間的新建地圖點
閉環檢測 重定位檢測
優化
感性認識共視圖的作用：重定位進行候選關鍵幀的選取時 得到匹配好的候選關鍵幀 並將和這個關鍵幀具有共視關系的關鍵幀全部拿出來 選出評分最高的關鍵幀作為最終選擇出來的候選關鍵幀
擴展樹
對於所有的關鍵幀 每一個關鍵幀都能夠得到和當前關鍵幀共視程度最大的關鍵幀 將當前關鍵幀和共視程度最大的關鍵幀連在一起就形成了擴展樹
本質圖
本質圖為擴展樹的連接關系
共視關系好的（大與100）的連接關系
形成閉環的連接關系 閉環后的地圖點變動后新增加的連接關系
本質圖中節點也是所有關鍵幀
本質圖是濃縮版的共視圖 只保留權重大與100的邊 就是保留了共視程度更高的關鍵幀
作用：
檢測回環時 利用本質圖對相似變換sim3進行位姿圖優化
零散項
地圖點相關
地圖信息 = 關鍵幀數組 + 特征點數組
地圖點在關鍵幀的索引 = 地圖點對應的特征點在關鍵幀的索引
地圖點對應特征點 特征點不一定能三角化出地圖點
地圖點map：關鍵幀和該地圖點在這個關鍵幀里對應的特征點的索引
參考關鍵幀：構建這個地圖點的關鍵幀為參考關鍵幀，如果這個參考關鍵幀被刪除了，則指定第一個觀測到這個地圖點的關鍵幀為參考關鍵幀
雙目特征點相關
雙目特征點匹配步驟：
粗匹配：根據兩個特征點的描述子和金字塔層級進行配對 兩個特征點的描述子的距離不能太遠 金字塔層級不能相差太多
精匹配：在兩個特征點的附近進行窗口滑動 計算兩個窗口之間的距離的大小 距離不能過大
亞像素插值：在右目相機的特征點的左右各選取一個特征點 分別計算左目特征點分別到右目特征點以及附近特征點的距離 並擬合成二次函數 求二次函數的最小值 即為最好的匹配結果
刪除離群點：匹配距離大於平均匹配距離2.1倍的特征點被認為誤匹配