視覺SLAM筆記

1. Pose Graph 的意義

帶有相機位姿和空間點的圖優化稱爲 BA，能夠有效地求解大規模的定位與建圖問題
但是，隨着時間的流逝，機器人的運動軌跡將越來越長，地圖規模也將不斷增長
像 BA 這樣的方法，計算效率就會（令人擔憂地）不斷下降

根據前面的討論，發現特徵點在優化問題中佔據了絕大多數部分
而實際上，經過若干次觀測之後，那些收斂的特徵點，空間位置估計就會收斂至一個值保持不動
而發散的外點則通常看不到了
對收斂點再進行優化，似乎是有些費力不討好的

因此，更傾向於在優化幾次之後就把特徵點固定住，只把它們看作位姿估計的約束
而不再實際地優化它們的位置估計

沿着這個思路往下走，會發現：是否能夠完全不管路標，而只管軌跡呢？
完全可以構建一個只有軌跡的圖優化
而位姿節點之間的邊，可以由兩個關鍵幀之間通過特徵匹配之後得到的運動估計來給定初始值
不同的是，一旦初始估計完成，就不再優化那些路標點的位置，而只關心所有的相機位姿之間的聯繫了
通過這種方式，省去了大量的特徵點優化的計算，只保留了關鍵幀的軌跡
從而構建了所謂的位姿（Pose Graph）

在 BA 中，特徵點數量遠大於位姿節點的數量，一個關鍵幀往往關聯了數百個關鍵點
而實時 BA 的最大計算規模，即使利用稀疏性，在當前的主流 CPU 上一般也就是幾萬個點左右
這就限制了 SLAM 應用場景
所以，當機器人在更長的時間和空間中運動時，必須考慮一些解決方式：

要麼像滑動窗口法那樣，丟棄一些歷史數據
要麼像 Pose Graph 的做法那樣，捨棄對路標點的優化，只保留 Pose 之間的邊

2. Pose Graph 的優化

關於 Pose Graph 圖優化中的節點和邊
這裏的節點表示相機位姿，以 ξ1,……,ξn 來表達
而邊，則是兩個位姿節點之間相對運動的估計，該估計可能來自於特徵點法或直接法
但不管如何，估計了，比如說 ξi 和 ξj 之間的一個運動 ∆ξij
該運動可以有若干種表達方式，取比較自然的一種：

或按李羣的寫法：

按照圖優化的思路來看，實際當中該等式不會精確地成立
因此設立最小二乘誤差，然後和以往一樣，討論誤差關於優化變量的導數

這裏，把上式的 ∆Tij 移至等式右側，構建誤差 eij：

注意優化變量有兩個： ξi 和 ξj
因此求 eij 關於這兩個變量的導數
按照李代數的求導方式，給 ξi 和 ξj 各一個左擾動： δξi 和 δξj
於是誤差變爲：

該式中，兩個擾動項被夾在了中間
爲了利用 BCH 近似，希望把擾動項移至式子左側或右側
暫時把它當作想當然的東西：

稍加改變，有：

該式表明，通過引入一個伴隨項，能夠“交換”擾動項左右側的 T
利用它，可以將擾動挪到最右（當然最左亦可），導出右乘形式的雅可比矩陣（挪到左邊時形成左乘）：