SLAM優化位姿時，偏差函數的雅可比矩陣的推導。

轉載請說明出處：

http://blog.csdn.net/zhubaohua_bupt/article/details/74011005

視覺SLAM的核心是求取相機的位姿並建圖。

在優化位姿時，其思想是構造一個關於位姿變化的偏差函數，當這個偏差函數最小時，認爲此時估計的位姿最優。

不管是直接法仍是特徵點法，位姿的迭代優化都是是求解一個最小二乘問題，

在優化位姿時

<1>直接法   最小二乘的偏差函數是，先後幀全部參與求解位姿像素塊的灰度差。一般這一步又稱之位圖像對齊。

<2>特徵點法   最小二乘的偏差函數是，重投影偏差(PnP,地圖點到當前圖像的投影點與匹配點的距離差和)

本篇博客是介紹的是，SLAM優化位姿時，偏差函數對位姿雅可比矩陣的推導。

偏差函數對於位姿擾動的雅可比矩陣，決定着下一步最優迭代估計時，位姿增量的方向。所以十分重要。

下面咱們先以直接法雅可比矩陣開始推導，特徵點法相似。

圖像最後的雅可比矩陣能夠由，全部參與直接法像素點雅可比矩陣的疊加造成。

1 直接法：

至此，咱們推導出了直接法中，偏差函數對於李代數位姿的雅克比矩陣。

2 特徵點法

 

在特徵點法的SLAM中，在PnP優化位姿時，偏差函數是重投影偏差。

PnP特徵點法與直接法的雅克比矩陣有什麼不一樣呢？還得從投影過程提及。PnP是什麼？

這個過程可用圖像表示以下

 

 

PnP的過程可描述以下，一堆世界座標系下的三維地圖點P(X,Y,Z)，

<1> 首先通過咱們要優化的位姿轉換至當前幀的座標系下P’(X’,Y’,Z’)，

 

 

<2> 而後透過相機內參投影至當前幀圖像座標系下p’(x’,y’)，

 

 

<3> 最後縮小地圖點在當前圖像幀上投影點p’與匹配點pmatch(xmatch,ymatch)距離偏差，來優化位姿。

上述的過程就是PnP投影過程，實際的優化有若干次這樣的迭代。