最近在做基于激光信息的機器人行人跟蹤發現如果單獨利用激光信息很難完成機器人對行人的識別、跟蹤等功能，因此考慮與視覺融合的方法，這樣便可以充分利用激光雷達提供的精確位置信息及視覺提供的豐富紋理、顏色等場景信息。以下是最近調研視覺SLAM中的實現方法的總結，包括三方面內容：姿態計算、閉環檢測、BA優化。

姿態計算

一、通過提取圖像的特征描述子，如ORB、SURF和SIFT等特征描述子，然后通過RANSAC算法進行圖像匹配去除匹配點中的外點，再通過將二維點對映射到三維之后，便可以利用PnP或ICP算法計算相機位姿。基于特征提取的位姿計算算法對場景有一定的要求，在無紋理場景會出現位姿計算失敗的情形。

二、直接圖像匹配方法：直接圖像匹配并不對圖片進行特征提取，核心思想是在旋轉坐標系下，基于相機一致性的假設，在相機的剛體變換已知的情況下，利用相機變換矩陣將目標圖片投影到當前圖片上，其像素之間的差異應該最小，將姿態計算轉換為加權最小二乘問題。直接圖像匹配算法的計算效率很高，不依賴GPU，具有很高的理論和商用價值。

閉環檢測

閉環檢測算法指的是通過檢測算法檢測出之前訪問過的場景。如圖1所示，當機器人在移動過程中，特別是在探索大面積的場景時，由于模型的不確定性以及設備的噪聲，不確定性會逐漸增長。通過引入閉環檢測技術，識別出歷史訪問過的場景以增加位姿之間的約束，可以很好的減少這種不確定性。

閉環檢測前后對比(a) 累計誤差導致的構圖錯誤，(b)閉環約束后的地圖

閉環檢測方法有：

一、最簡單的閉環檢測算法是將新檢測出來的關鍵幀和過去所有的關鍵幀一一進行比較，雖然這種方法能比較好的檢測出當前場景是否在之前出現過，但是在大規模場景下，機器人往往有成千上萬個關鍵幀，這種方法檢測效率及其底下，不能再實際場景中使用。

二、通過將圖像中的特征和整個地圖中的路標對應起來，然后建立一個所有路標的數據庫，通過使用路標分類器來加快場景識別速度。比如講所有路標構建成一棵KD-Tree，當新添加一幀關鍵幀時，將關鍵幀在KD-Tree中進行檢索，從中檢索之前出現過的場景。

三、基于視覺詞袋的閉環檢測方法，這種方法通過將特征描述子抽象成詞匯，通過TF-IDF方法識別出現過的場景。如圖2所示，是閉環檢測中基于詞袋模型流程圖。使用視覺詞袋的方法效率很高，可用在大規模地圖的創建上。

BA優化

一、問題闡述：同時對三維點位置和相機參數進行非線性優化。

BA原理

二、LM法的原理與優勢：

原理：是一種“信賴域”的方法，當收斂速度較快時，增大信賴域使算法趨向于高斯牛頓法；當收斂速度較慢時，減小信賴域使算法趨向于最速下降法。優勢：速度快；可以在距離初始值較遠處得到最優解。

SLAM優化算法對比