：
視覺slam的核心是求取相機的位姿並建圖。
在優化位姿時，其思想是構造乙個關於位姿變化的誤差函式，當這個誤差函式最小時，認為此時估計的位姿最優。
無論是直接法還是特徵點法，位姿的迭代優化都是是求解乙個最小二乘問題，
在優化位姿時
<1>直接法最小二乘的誤差函式是，前後幀所有參與求解位姿畫素塊的灰度差。通常這一步又稱之位影象對齊。
<2>特徵點法最小二乘的
誤差函式
是，重投影誤差(pnp,地圖點到當前影象的投影點與匹配點的距離差和)
本篇部落格是介紹的是，slam優化位姿時，誤差函式對位姿雅可比矩陣的推導。
誤差函式對於位姿擾動的雅可比矩陣，決定著下一步最優迭代估計時，位姿增量的方向。因此十分重要。
下面我們先以直接法雅可比矩陣開始推導，特徵點法類似。
影象最後的雅可比矩陣可以由，所有參與直接法畫素點雅可比矩陣的疊加形成。
至此，我們推導出了直接法中，誤差函式對於李代數字姿的雅克比矩陣。
在特徵點法的slam中，在pnp優化位姿時，誤差函式是重投影誤差。
pnp特徵點法與直接法的雅克比矩陣有什麼不同呢？還得從投影過程說起。pnp是什麼？
這個過程可用影象表示如下
pnp的過程可描述如下，一堆世界座標系下的三維地圖點p(x,y,z)，
<1> 首先經過我們要優化的位姿轉換至當前幀的座標系下
p』(x』,y』,z』)，
<2> 然後透過相機內參投影至當前幀影象座標系下p』(x』,y』)，
<3> 最後縮小地圖點在當前影象幀上投影點p』與匹配點pmatch(xmatch,ymatch)距離誤差，
來優化位姿。
上述的過程就是pnp投影過程，實際的優化有若干次這樣的迭代。
ORBSlam2的位姿優化演算法
orbslam2中使用了對極約束，三角測量，pnp投影和bundleadjustment演算法 本文著重講解在orbslam2中使用到的bundleadjustment優化演算法。區域性ba優化的過程是，通過當前取得的關鍵幀找到附近的區域性關鍵幀，區域性地圖點和可以觀察到這些區域性地圖點但是不是區域...
SLAM第6章 求解位姿變換 2D 2D
第3至5章講了3d 3d和3d 2d問題，為了全面介紹相機位姿的求解方法，本章介紹2d 2d問題。根據3d 3d和3d 2d問題的描述，就可以理解2d 2d問題 相機在兩個位置看到了同樣的3維點，即知道畫素座標，求這兩個相機的位姿關係。對極幾何有極線 極點等概念，而我們不需要這些，只需要知道空間向量...
機械臂位姿三個夾角與位姿矩陣的關係
機械臂位姿三個夾角與位姿矩陣的關係 由姿態矩陣 計算旋轉角度 double sin2 ax double cos2 1 sqrt 1 pow ax,2 double cos2 2 sqrt 1 pow ax,2 std vectorcosvector cosvector.push back cos2...