傳統ORB-SLam中位姿優化中雅克比矩陣講解

由於之前的魚眼orbslam只有單目部分，所以在優化時也只是用了單目位姿優化和三維座標點優化，並沒有將雙目的優化添加進去，不知道是否對結果有影響；

這裏添加雙目的優化部分，主要是將添加雅克比矩陣；

orbslam中的優化部分使用了g2o庫，具體的詳細講解可以參考https://zhuanlan.zhihu.com/p/58521241 講解的很詳細

這裏不加贅述，主要從單目的雅克比矩陣講解，延伸到雙目的雅克比矩陣，以及帶有魚眼EUCM模型的魚眼雙目的雅克比矩陣的推導

(雅克比矩陣即爲誤差函數關於優化項的導數，所以depends on 優化項的個數， 雅克比矩陣的個數也不同)

1. 單目的雅克比矩陣

首先以單目位姿優化爲例， 因爲只對位姿做優化，所以這裏採用一元邊BaseUnaryEdge​ 進行邊的定義，如下：

BaseUnaryEdge<2, Vector2d, VertexSE3Expmap>

誤差函數e的公式不在推導，可以參見SLAM十四講P163的講解，這裏假設空間點P 經過位姿變換後在相機座標系下的座標爲P' =[X',Y',Z']T 
根據相機的投影公式，將座標P‘ 轉換到像素座標系下公式如圖：

 

 

 

 

 接下來要計算的誤差函數就是將 實際的特徵點的二維座標 與該投影值u,v 進行比較，求差，因爲理想狀態下兩者應該是完全一致的，但是由於位姿的誤差等，導致二者不同，因此誤差就是需要經二者的差值最小化，也就是最小化重投影誤差函數；

首先考慮誤差函數e關於擾動量的導數，利用鏈式法則可以寫成如下形式：

因爲上面u，v的公式已經明確，那麼e關於P’ 的導數就可以寫成

（公式1）

同時，第二項P‘ 關於擾動的導數按照之前的推導爲：

（這裏需要注意，在g2o中的形式一般爲[-P'^ , I ], 需要在寫雅克比矩陣的時候注意一下）

這裏P '^ 爲三維座標P‘ 座標的反對成矩陣，因此 —P'^w爲在反對成矩陣的基礎上添加負號即可；

           |0   z    -y|

-P’^ = |z   0    -x|

            |-y   x    0|

最終的誤差關於擾動量的導數公式如下：

該公式也就對應了ｇ２０庫中types_six_dof_expmap.cpp 文件中　EdgeSE3ProjectXYZOnlyPose類　對應的linearizeOplus() 中的雅克比矩陣

  //這裏要注意的就是ｇ２０中是Ｐ'^在前，Ｉ矩陣在後
  _jacobianOplusXi(0,0) =  x*y*invz_2 *fx;
  _jacobianOplusXi(0,1) = -(1+(x*x*invz_2)) *fx;
  _jacobianOplusXi(0,2) = y*invz *fx;
  _jacobianOplusXi(0,3) = -invz *fx;
  _jacobianOplusXi(0,4) = 0;
  _jacobianOplusXi(0,5) = x*invz_2 *fx;

  _jacobianOplusXi(1,0) = (1+y*y*invz_2) *fy;
  _jacobianOplusXi(1,1) = -x*y*invz_2 *fy;
  _jacobianOplusXi(1,2) = -x*invz *fy;
  _jacobianOplusXi(1,3) = 0;
  _jacobianOplusXi(1,4) = -invz *fy;
  _jacobianOplusXi(1,5) = y*invz_2 *fy;

因爲只優化位姿，所以這裏定義了一元邊，如果是將位姿和三維座標點一起優化，這裏需要定義二元邊

BaseBinaryEdge<2, Vector2D, VertexSBAPointXYZ, VertexSE3Expmap>
其中的參數分別是　定義誤差函數的維數，座標形式，頂點０的類型(這裏是三維點)，頂點１的類型(這裏爲相機的位姿)

定義完成二元邊後，只需要添加誤差函數ｅ關於三維空間點Ｐ（ＸＹＺ）的導數即可，根據ｓｌａｍ四講的推導，這裏只需啊喲對之前Ｐ’的求導乘以Ｒ矩陣即可，如下：

具體的代碼部分可以參考ｇ２０庫中types_six_dof_expmap.cpp 文件中EdgeSE3ProjectXYZ::linearizeOplus()　函數：只需要添加一個雅克比項即可，但是需要注意的這裏要先添加對三維點的優化，再添加對位姿的優化：

Matrix<double,2,3> tmp;
  tmp(0,0) = fx;
  tmp(0,1) = 0;
  tmp(0,2) = -x/z*fx;

  tmp(1,0) = 0;
  tmp(1,1) = fy;
  tmp(1,2) = -y/z*fy;

  _jacobianOplusXi =  -1./z * tmp * T.rotation().toRotationMatrix();

２．　普通相機雙目優化公式：

普通相機的雙目優化時，　在誤差項求解時，出來左相機中的特徵點的測量值和觀測值之間的差值，還包括右目中對應特徵點的橫座標的觀測值與測量值之間的關係；

因此測量值相關公式由兩個變成三個，如下：

公式改變帶來的主要是公式1中誤差函數關於相機座標系下的座標點的導數，雅克比矩陣也由最開是2x6變成3x6的矩陣

只需要按照這個順序依次求導帶入即可，對應的相關代碼爲：ｇ２０庫中types_six_dof_expmap.cpp 文件中　EdgeStereoSE3ProjectXYZ::linearizeOplus() 函數對應的雅克比矩陣的下半部分：

  _jacobianOplusXj(2,0) = _jacobianOplusXj(0,0)-bf*y/z_2;
  _jacobianOplusXj(2,1) = _jacobianOplusXj(0,1)+bf*x/z_2;
  _jacobianOplusXj(2,2) = _jacobianOplusXj(0,2);
  _jacobianOplusXj(2,3) = _jacobianOplusXj(0,3);
  _jacobianOplusXj(2,4) = 0;
  _jacobianOplusXj(2,5) = _jacobianOplusXj(0,5)-bf/z_2;

同理，對座標點的優化也要相應的變成3x3；