ORB-SLAM 完整拆解:三線程架構、詞袋迴圈偵測與位姿圖優化
- orb-slam
- visual-slam
- loop-closure
- bundle-adjustment
- feature-matching
系統定位
ORB-SLAM(Mur-Artal et al., 2015 年首版,隨後有 ORB-SLAM2、ORB-SLAM3)是圖優化路線 SLAM 在視覺 SLAM 領域最具代表性、影響最深遠的開源實作之一。它是特徵點法(feature-based)視覺 SLAM 的標竿系統:不直接處理原始像素灰階值,而是先從每一幀影像中偵測、描述、匹配離散的特徵點,再用這些特徵點的幾何關係推算相機運動與場景結構。整個系統圍繞三個並行執行的線程組織,這個三線程架構是理解 ORB-SLAM 工程設計最關鍵的切入點。
為什麼選 ORB 特徵
ORB(Oriented FAST and Rotated BRIEF)特徵是系統名稱的由來,選擇它而不是 SIFT 或 SURF 有明確的工程理由:
偵測用 FAST 角點偵測器(比較像素與周圍一圈像素的灰階差異,計算量遠低於 SIFT 的尺度空間極值搜尋),並額外算出每個角點的主方向(用灰階質心法,intensity centroid,讓角點具備旋轉不變性,這是「Oriented」的來源——原始 FAST 沒有方向資訊)。描述用 BRIEF 二進位描述子(比較特徵點周圍預先定義的像素對灰階大小,輸出一串二進位碼),並依照前面算出的主方向旋轉取樣模板,讓描述子本身也具備旋轉不變性(這是「Rotated」的來源)。二進位描述子的關鍵優勢是可以用漢明距離(Hamming distance,只需要 XOR 加位元計數)做特徵匹配,比 SIFT/SURF 的浮點描述子用歐氏距離快上一到兩個數量級,這個速度優勢是 ORB-SLAM 能做到即時運行的基礎。
追蹤線程(Tracking)
追蹤線程是唯一需要跟相機幀率同步、即時輸出結果的線程,職責是估計每一幀相機的姿態。流程是:
- 特徵提取:從當前幀提取 ORB 特徵。
- 初始姿態估計:正常情況下用等速運動模型(假設相機運動在短時間內近似等速,用前一幀的速度預測目前姿態),把上一幀的地圖點投影到目前幀,在投影點附近做特徵匹配縮小搜尋範圍;如果運動模型失效(例如運動模型預測的投影匹配數量太少),改用重定位(relocalization)——透過 DBoW2 詞袋模型在整個關鍵幀資料庫裡搜尋外觀最相似的候選關鍵幀,做更大範圍的特徵匹配後用 PnP(Perspective-n-Point)算出姿態。
- 追蹤局部地圖:把目前姿態附近共視圖上相關的局部地圖點投影到目前幀,做更大範圍的特徵匹配與更精細的姿態最佳化(最小化重投影誤差)。
- 決定是否插入新關鍵幀:不是每一幀都存成關鍵幀(那樣地圖會膨脹到無法管理),只有滿足特定條件(例如跟最近一個關鍵幀的共視地圖點數量降到某個門檻以下、或距離上次插入關鍵幀已經過了一定幀數)才觸發插入,這個決策直接影響地圖密度與後續 Bundle Adjustment 的計算負擔。
局部建圖線程(Local Mapping)
當追蹤線程插入一個新關鍵幀,局部建圖線程接手處理:
- 地圖點篩選:對最近建立的地圖點做品質檢查,剔除觀測次數太少、或跨越關鍵幀的可視比例過低的不穩定地圖點(culling)。
- 新地圖點三角化:新關鍵幀跟共視圖上相鄰的關鍵幀之間,尋找還沒有對應地圖點的特徵匹配,透過三角測量(triangulation)建立新的地圖點。
- 局部 Bundle Adjustment:對新關鍵幀、跟它共視的關鍵幀、以及這些關鍵幀觀測到的所有地圖點,聯合最佳化位姿與地圖點座標,最小化所有觀測的重投影誤差——這是圖優化 SLAM非線性最小平方框架在局部範圍的具體應用,只涉及局部一小群關鍵幀,不是全域最佳化,計算量可控,能在背景線程即時完成。
- 冗餘關鍵幀剔除:如果一個關鍵幀觀測到的地圖點,有很高比例也被其他至少 3 個關鍵幀以同樣或更好的品質觀測到,這個關鍵幀對地圖的邊際貢獻就很低,會被剔除,控制關鍵幀數量與地圖規模。
迴圈閉合線程(Loop Closing)
迴圈閉合線程持續監控是否有機會偵測到迴圈:
候選偵測:用 DBoW2 詞袋模型(Bag of Words),把每個關鍵幀的所有 ORB 特徵描述子量化成一個視覺詞彙直方圖(visual vocabulary,這個詞彙表通常是離線用大量影像預先訓練的階層式 k-means 樹),比較目前關鍵幀的詞袋向量跟資料庫裡所有其他關鍵幀(排除掉共視圖上鄰近的關鍵幀,避免把附近幀誤判成迴圈)的相似度,找出外觀高度相似的候選。
幾何驗證:詞袋相似度只代表「外觀看起來像」,不保證幾何上真的一致(見前面 FAQ 討論迴圈閉合誤判的災難性後果),必須進一步驗證——對候選幀做 ORB 特徵匹配,估計候選幀與目前幀之間的相對變換(用 RANSAC 搭配 PnP 或本質矩陣估計排除誤匹配),只有幾何驗證也通過的候選,才真正判定為迴圈閉合。
位姿圖優化:一旦確認迴圈閉合,先在本質圖(見 FAQ 說明)上做一次相對輕量的位姿圖優化(Pose Graph Optimization,只最佳化關鍵幀位姿,不涉及所有地圖點,計算量遠低於完整 Bundle Adjustment),快速把累積漂移沿整個共視圖分散修正。
全域 Bundle Adjustment(可選):位姿圖優化之後,系統會在背景額外觸發一次全域 Bundle Adjustment(同時最佳化所有關鍵幀位姿與所有地圖點),進一步提升精度,因為計算成本高,這一步通常做成可以被新的追蹤請求中斷的背景任務,不阻塞即時運行。
ORB-SLAM3 的關鍵改進
相對 ORB-SLAM2,ORB-SLAM3(2020)帶來兩個實務上影響最大的改進:
緊耦合視覺-慣性融合(Tightly-Coupled Visual-Inertial):把 IMU 預積分(preintegration)直接加入 Bundle Adjustment 的最佳化問題,而不是像早期鬆耦合方案那樣把視覺估計跟慣性估計分開處理再融合。緊耦合能在快速運動、短暫視覺追蹤丟失(例如快速轉身導致運動模糊)時,靠 IMU 提供的慣性資訊撐過視覺資訊不足的空窗期,大幅提升魯棒性。
多地圖系統(Atlas):當追蹤徹底丟失(例如相機被遮擋很長一段時間),ORB-SLAM2 只能嘗試重定位回原本的地圖,失敗就整個系統失效;ORB-SLAM3 在追蹤丟失時會自動開啟一個全新的獨立地圖繼續運行,等到之後偵測到跟舊地圖有迴圈閉合關係,再把兩個地圖無縫合併(map merging),這讓系統對長時間遮擋、光照劇變這類會導致追蹤中斷的情況有更好的容錯能力,不會因為一次追蹤丟失就整個報廢。
特徵點法 vs 直接法
ORB-SLAM 代表的特徵點法,跟 LSD-SLAM、DSO(Direct Sparse Odometry)代表的直接法(direct method),是視覺 SLAM 兩條並行發展的技術路線,差異值得對照理解:特徵點法先做離散特徵偵測與匹配,再用幾何方法求解,優點是對光照變化、旋轉、尺度變化的穩健性較好(因為描述子本身設計成具備這些不變性),有成熟的離群值剔除工具(RANSAC);直接法跳過特徵偵測,直接對像素灰階值做光度誤差(photometric error)最佳化,優點是能利用低紋理區域的微弱資訊、精度上限通常更高,缺點是對光照變化敏感(光度誤差假設同一個 3D 點在不同幀的灰階值不變,這個假設在光照劇烈變化時會被破壞)、且高度依賴好的初始值(最佳化問題的非凸程度更高,容易收斂到錯誤的局部極小值)。兩條路線各有取捨,也是為什麼視覺 SLAM 領域至今沒有單一「最佳」方案。
程式碼範例:ORB 特徵提取與匹配
import cv2
orb = cv2.ORB_create(nfeatures=1000)
img1 = cv2.imread("frame1.png", cv2.IMREAD_GRAYSCALE)
img2 = cv2.imread("frame2.png", cv2.IMREAD_GRAYSCALE)
kp1, des1 = orb.detectAndCompute(img1, None)
kp2, des2 = orb.detectAndCompute(img2, None)
# 二進位描述子用漢明距離做匹配,這正是 ORB 相對 SIFT/SURF
# 能大幅加速匹配的關鍵(漢明距離只需要 XOR + popcount)
bf = cv2.BFMatcher(cv2.NORM_HAMMING, crossCheck=True)
matches = bf.match(des1, des2)
matches = sorted(matches, key=lambda m: m.distance)
# 實務上會再用 RANSAC 估計本質矩陣 (Essential Matrix) 做幾何一致性驗證,
# 剔除外觀相似但幾何上不一致的錯誤匹配
good_matches = matches[:200]
常見錯誤
- 初始化只用純旋轉運動:ORB-SLAM 的地圖初始化依賴三角測量建立初始地圖點,純旋轉運動(相機沒有平移)在幾何上無法三角測量出深度(視差為零),是視覺 SLAM 初始化最常見的失敗場景,實務上系統通常會偵測到這種退化情況並拒絕初始化,等待有足夠平移量的運動。
- 在低紋理場景直接套用特徵點法系統,不評估環境是否適合:如前述 FAQ 討論,特徵點法在大片白牆、光滑地板環境容易特徵不足,選型前應該評估目標場景的視覺紋理豐富度。
- 關鍵幀插入策略設得太寬鬆,關鍵幀數量爆炸:關鍵幀太密集會讓共視圖、局部 Bundle Adjustment 的計算負擔快速膨脹,長時間運行容易耗盡記憶體或讓局部建圖線程跟不上追蹤線程插入新關鍵幀的速度。
- 迴圈閉合的幾何驗證門檻設得太寬鬆:只做詞袋相似度篩選、跳過或簡化幾何驗證,會顯著提高誤判機率,一旦發生會造成地圖結構性錯亂,且通常難以事後自動修復。
- 單目(monocular)配置下忽略尺度漂移:純單目視覺 SLAM 沒有絕對尺度資訊(只能估計到相差一個未知比例常數的相對結構),長時間運行容易出現尺度漂移(同一個物體在地圖不同區域的估計大小不一致),這是單目視覺 SLAM 的結構性限制,通常需要額外的尺度資訊來源(例如已知物體尺寸、IMU、或立體相機基線)才能解決。
延伸閱讀
常見問題
- ORB-SLAM 為什麼要用三個並行線程,而不是一個線程依序處理?
- 因為追蹤(tracking)需要跟相機幀率同步、即時輸出目前姿態(通常要求 30 Hz 以上),但局部建圖(local mapping)裡的局部 Bundle Adjustment 與迴圈閉合裡的全域 Bundle Adjustment 都是計算成本高、耗時可能到幾百毫秒甚至數秒的最佳化問題,如果放在同一個線程依序執行,追蹤線程會被這些耗時的最佳化卡住,直接讓即時姿態輸出的頻率崩潰。把這兩類耗時的最佳化拆到獨立線程背景執行,追蹤線程只需要用目前已知的局部地圖做輕量的姿態估計,就能維持即時性,這是幾乎所有即時 visual SLAM 系統採用多線程(或前後端分離)架構的根本原因,不是工程上的偶然選擇。
- 共視圖(Covisibility Graph)跟本質圖(Essential Graph)差在哪裡,為什麼需要兩個?
- 共視圖記錄所有關鍵幀之間的共視關係——只要兩個關鍵幀共同觀測到夠多的地圖點,之間就有一條邊,這是一個資訊完整但邊數很多、很稠密的圖,適合用來查詢局部相關的關鍵幀(例如局部 Bundle Adjustment 要決定哪些關鍵幀該一起最佳化)。本質圖是從共視圖裡篩選出來的一個稀疏子集:只保留生成樹(spanning tree,確保圖連通)的邊、共視程度特別高的邊、以及迴圈閉合邊,邊數少很多。迴圈閉合之後要做的位姿圖優化只需要在本質圖上求解,因為稀疏圖的最佳化計算量遠低於在完整共視圖上求解,同時本質圖仍然保留了修正全域一致性所需要的關鍵連通性,這是用精度換取效率的典型工程取捨——先用本質圖快速做一次位姿圖優化把明顯的漂移修正掉,再視情況觸發更完整、更耗時的全域 Bundle Adjustment。
- ORB-SLAM 遇到低紋理環境(例如白牆、走廊)為什麼容易失敗?
- ORB-SLAM 是特徵點法(feature-based),整個追蹤與建圖流程都建立在能穩定偵測到足夠數量的 ORB 角點特徵這個前提上。低紋理表面(大片白牆、光滑地板)在影像上幾乎沒有局部灰階梯度變化,FAST 角點偵測器天生就找不到足夠的候選特徵點,即使找到少數幾個,可能也集中在畫面邊緣(例如牆角、窗框),這種空間分布不均的特徵點會讓姿態估計的幾何條件很差(類似只有共線點時三角測量會退化)。這是特徵點法相對直接法(direct method,例如 LSD-SLAM、DSO 直接用像素灰階值做光度誤差最佳化,不需要偵測離散特徵點)在低紋理場景下的結構性劣勢,直接法雖然對光照變化更敏感、初始化通常更脆弱,但在低紋理場景反而能利用大片區域的微弱灰階梯度資訊,這也是為什麼兩條技術路線在 SLAM 領域長期並存、沒有一方完全取代另一方。