SLAM後端：位姿圖優化（Pose Graph）

BA優化時間跟特徵點數量有關，特徵點數量越多BA消耗時間越長。折衷做法是，在進行幾次優化後，將特徵點位置固定，不再優化特徵點，只優化相機位姿。位姿圖優化是一中方法。場景如下：

                                   

三角形節點表示相機位姿，用表示，藍色的邊表示兩相機位姿之間的相對運動，表示之間的相對位姿變化。圖優化的目的是爲了優化，方法是利用與之間的相對關係構建最小二乘問題進行優化求解。

首先根據兩幀的位姿可以得到相對位姿變換：

                                                                             

李羣表達式：

                                                                               

實際的，即值根據i,j兩幀圖像利用2D-2D（對極幾何）得到。優化的目的就是讓

                                                                       

可以寫成：

                    

求解該優化問題一般採用LM法，需要計算偏導數並求解H，以通過增量方程求解優化量。

給分別左乘以一個擾動：

                                               

套用伴隨性質（附在後文）得到：

                                  

泰勒展開合併得到：

                                   

則的偏導數如下：

                                                                 

得到偏導數即可求解增量方程解決優化問題。

伴隨性質：

旋轉羣伴隨性質：

                                                                    

變換羣伴隨性質：

                                                               

其中：