ORB-SLAM-Mono論文要點總結（六）——— 附錄-非線性優化 Nonlinear Optimizations

非線性優化

1. BA優化[1]
   地圖點的3-D位置座標爲$X_{w,j}\in R^3$，關鍵幀位姿變換$T_{iw}\in SE(3)$(變換空間)。$w$表示世界參考系，優化使得地圖點和與之匹配的特徵點$x_{i,j}\in R^2$間的重投影誤差最小。在關鍵幀$i$中一個地圖點$j$的觀測誤差爲：
   $e_{i，j} = x_{i,j} - \pi_i({T_{iw}，X_{w,j}})$
   這裏$\pi$爲投影函數：
   
   圖中世界繫到$i$關鍵幀的旋轉矩陣$R_{iw}\in SO(3)$旋轉矩陣李羣空間，位移變換$t_{iw}\in R^3$，分爲爲變換矩陣$T_{iw}$的一部分。$（f_{i,u}，f_{i,v}）$和$(c_{i,u}，c_{i,v})$分別表示與相機$i$對應的焦距長度和主點座標。最小化代價函數爲：
   $C = \sum_{i,j}\rho_h(e_i^T\Omega_{i,j}^{-1}e_{i,j})$上式中$\rho_h$是huber魯棒損失函數，$\Omega_{i,j} = \sigma_{i,j}^2I_{2*2}$爲檢測到的特徵點與尺度相關的協方差矩陣。
   下面是基於以上重投影誤差原理和建立的代價函數在不同工作模塊工作時，對應的優化方式，固定不變的狀態，以及每種優化方式待優化的狀態：

工作模塊	優化方式	固定項	待優化項
地圖初始化	full BA	第一個關鍵幀	所有地圖點和關鍵幀
局部建圖	local BA	關鍵幀	局部地圖裏所有的地圖點
跟蹤	位姿優化/motion-only BA	所有地圖點	相機位姿

2. 基於相似變換空間Sim3約束的位姿圖優化[6]
   基於兩個邊的位姿圖（見VII-D）, 可以定義一邊的誤差爲：
   $e_{i,j} = log_{Sim(3)}(S_{ij}S_{jw}S_{iw}^{-1})$上式中$S_{ij}$是圖優化之前基於SE(3)位姿計算的兩個關鍵幀的相對顯示變換，令尺度係數爲1？。在迴環邊，基於Horn[42]方法計算相對變換。$log_{Sim(3)}$[48]轉換到切空間爲了將誤差轉換爲7維向量？。優化相似變換空間的的關鍵幀位姿時最小化的代價函數爲：
   $C = \sum_{i,j}(e_{i,j}^T\Lambda_{i,j}e_{i,j})$上式中$\Lambda_{i,j}$如[48]中所述的信息矩陣，作者將其設爲單位陣。爲了固定規範7自由度，固定迴環關鍵幀。儘管此方法是一個大致的全局BA（full BA），在實驗VIII-E中證明比BA能夠更快更好的收斂。
3. 關鍵幀間的相似變換優化
   基於關鍵幀1和關鍵幀2間給定的一組n個匹配 $i\Rightarrow j$(特徵點和它們對應的3D地圖點)，爲了優化相對位姿$S_{12}$(見VII-B)，可以最小化兩幀圖像間的重投影誤差爲：
   
   代價函數爲：
   
   上式中$\Omega_{1,i}$和$\Omega_{2,j}$爲與圖像1和圖像2中特徵點尺度相關的協方差度量。在此優化過程中，地圖點時固定的。