轉載請說明出處：

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投影過程，實際的優化有若干次這樣的迭代。

?

?

?

轉載于:https://www.cnblogs.com/zhubaohua-bupt/p/7182785.html

總結

以上是生活随笔為你收集整理的SLAM优化位姿时，误差函数的雅可比矩阵的推导。的全部內容，希望文章能夠幫你解決所遇到的問題。

如果覺得生活随笔網站內容還不錯，歡迎將生活随笔推薦給好友。