第十講（第一版第十一講）-位姿圖優化部分

誤差關於位姿的雅克比矩陣推導

今天看到後端優化的關於位姿圖優化部分，對於誤差關於兩個位姿的雅克比矩陣推導部分理解遇到了一些困難。書中直接給$T_i$和$T_j$各乘了一個左擾動，然後就開始推導了。
倒數第三行到倒數第二行的近似運用了Taylor Expansion，倒數第二行到倒數第一行的近似是什麼原理就不太清楚了。（希望如果有知道其中原理的大神看到了，能在評論裏給出解釋或者推導過程的連接，謝謝！）

現在我給出我自己的理解，分別求對兩個位姿的雅克比矩陣。令$T_{ij}^{-1}T_i^{-1}T_j$的李代數爲$\xi_e$，給李羣$T_i$一個左擾動，那麼誤差可以表示爲
$\hat{\xi_e} = \ln \Big( T_{ij}^{-1}T_i^{-1} \exp\big((-\delta\xi_i)^{\wedge}\big) T_j \Big)^{\vee}$
注意，書中誤差用$e_{ij}$表示，按照定義的形式應該是一個$6\times 1$維的向量，是個李代數，所以用$\xi_e$表示感覺不會存在誤導。利用SE(3)上的伴隨性質
$\hat{\xi_e} = \ln \Big( T_{ij}^{-1}T_i^{-1}T_j \exp\big((-\mathrm{Ad}(T_j^{-1}) \delta\xi_i)^{\wedge}\big) \Big)^{\vee}$
利用BCH右乘近似
$\hat{\xi_e} = \ln \Big( \exp\big( \xi_e^\wedge \big) \exp\big((-\mathrm{Ad}(T_j^{-1}) \delta\xi_i)^{\wedge}\big) \Big)^{\vee} = -J_r^{-1}\mathrm{Ad}(T_j^{-1}) \delta\xi_i + \xi_e$
再根據李代數的求導
$\frac{\partial e_{ij}}{\partial \delta\xi_i} = \lim_{\delta\xi_i\rightarrow 0} \frac{-J_r^{-1}\mathrm{Ad}(T_j^{-1}) \delta\xi_i + \xi_e - \xi_e}{ \delta\xi_i} = -J_r^{-1}\mathrm{Ad}(T_j^{-1})$
同理，給李羣$T_j$一個左擾動，在根據李代數求導，可以求得
$\frac{\partial e_{ij}}{\partial \delta\xi_i} = J_r^{-1}\mathrm{Ad}(T_j^{-1})$

課後習題1

根據上面的思路，如果誤差定義爲$\Delta\xi_{ij}=\xi_i\circ\xi_j^{-1}$，左擾動雅克比矩陣可以表示爲
$\frac{\partial e_{ij}}{\partial \delta\xi_i} = J_r^{-1}\mathrm{Ad}(T_j) \mathrm{Ad}(T_i^{-1})$
$\frac{\partial e_{ij}}{\partial \delta\xi_i} = -J_r^{-1}$
按照此結果改寫pose_graph_g2o_lie.cpp中誤差和雅克比矩陣

//error
_error = (_measurement.inverse() * v1 * v2.inverse()  ).log();
//jacobian
_jacobianOplusXi = J * v2.Adj() * v1.inverse().Adj();
_jacobianOplusXj = -J;

結果沒有符合預期，誤差是下降了，但是優化結束了還是很大，g2o_viewer顯示的結果也不對。不過將linearizeOplus()函數註釋掉，採用數值方式求解的雅克比矩陣，最後結果還是一樣。個人猜測可能是這樣誤差定義成這樣可能和g2o內部代碼設定不匹配，當然也可能是上面的雅克比矩陣沒求對，所以希望有這樣嘗試過的也可以分享下經驗。 分析之後，應該是sphere.g2o中保存邊的觀測值（即位姿間的相對運動）應該是按照$T_i^{-1}T_j$定義的，而不是按照$T_iT_j^{-1}$定義，這樣用此種方式定義的誤差進行優化將得不到預期的結果。
還有一個問題就是左乘擾動和右乘擾動推導的雅克比矩陣不一樣，用右乘擾動推導的雅克比矩陣優化，得不到預期的結果。這應該是位姿矩陣左乘和右乘不同導致的，一般SLAM裏求的位姿指的都是相對於世界座標系的絕對位姿，即左乘；而右乘是相對於動座標系的位姿，可以理解爲對前一幀圖像的相對位姿。sphere.g2o中保存的頂點應該是相對於世界座標系下的位姿。因此，如果圖優化的頂點的是關鍵幀之間的相對位姿，則應該用右擾動模型求解的結果；如果頂點保存的是相對於固定座標的位姿，則應該使用左擾動模型求解的結果。（沒有在實際代碼中驗證過）

optimizing ...
iteration= 0     chi2= 15212918696.119320        time= 0.928117  cumTime= 0.928117       edges= 9799     schur= 0        lambda= 81.350858   levenbergIter= 1
iteration= 1     chi2= 11386678732.912115        time= 0.978504  cumTime= 1.90662        edges= 9799     schur= 0        lambda= 54.233905   levenbergIter= 1
iteration= 2     chi2= 5105775457.680527         time= 1.01027   cumTime= 2.91689        edges= 9799     schur= 0        lambda= 36.155937   levenbergIter= 1
iteration= 3     chi2= 3636537945.099213         time= 1.00972   cumTime= 3.92661        edges= 9799     schur= 0        lambda= 24.103958   levenbergIter= 1
iteration= 4     chi2= 675549387.141574  time= 0.992889  cumTime= 4.9195         edges= 9799     schur= 0 lambda= 16.069305  levenbergIter= 1
iteration= 5     chi2= 249002379.412620  time= 0.971525  cumTime= 5.89102        edges= 9799     schur= 0 lambda= 10.712870  levenbergIter= 1
iteration= 6     chi2= 147527240.236920  time= 1.00219   cumTime= 6.89322        edges= 9799     schur= 0 lambda= 7.141913   levenbergIter= 1
iteration= 7     chi2= 79441445.003757   time= 0.966643  cumTime= 7.85986        edges= 9799     schur= 0 lambda= 4.445619   levenbergIter= 1
iteration= 8     chi2= 65455378.789084   time= 1.00436   cumTime= 8.86422        edges= 9799     schur= 0 lambda= 1.906949   levenbergIter= 1
iteration= 9     chi2= 61760426.506496   time= 0.940925  cumTime= 9.80515        edges= 9799     schur= 0 lambda= 0.635650   levenbergIter= 1
iteration= 10    chi2= 59864696.972565   time= 1.00284   cumTime= 10.808         edges= 9799     schur= 0 lambda= 0.211883   levenbergIter= 1
iteration= 11    chi2= 59741695.565524   time= 1.00289   cumTime= 11.8109        edges= 9799     schur= 0 lambda= 0.141255   levenbergIter= 1
iteration= 12    chi2= 59624909.837083   time= 0.955693  cumTime= 12.7666        edges= 9799     schur= 0 lambda= 0.094170   levenbergIter= 1
iteration= 13    chi2= 57542133.582117   time= 1.9179    cumTime= 14.6845        edges= 9799     schur= 0 lambda= 0.125560   levenbergIter= 2
iteration= 14    chi2= 56043735.219484   time= 1.92567   cumTime= 16.6101        edges= 9799     schur= 0 lambda= 0.167052   levenbergIter= 2
iteration= 15    chi2= 55731974.147674   time= 1.00317   cumTime= 17.6133        edges= 9799     schur= 0 lambda= 0.111368   levenbergIter= 1
iteration= 16    chi2= 54104140.413337   time= 1.92121   cumTime= 19.5345        edges= 9799     schur= 0 lambda= 0.141591   levenbergIter= 2
iteration= 17    chi2= 53804036.461193   time= 1.00997   cumTime= 20.5445        edges= 9799     schur= 0 lambda= 0.094394   levenbergIter= 1
iteration= 18    chi2= 52174388.910775   time= 1.82121   cumTime= 22.3657        edges= 9799     schur= 0 lambda= 0.125859   levenbergIter= 2
iteration= 19    chi2= 51901913.851112   time= 1.01241   cumTime= 23.3781        edges= 9799     schur= 0 lambda= 0.083906   levenbergIter= 1
iteration= 20    chi2= 50389317.789719   time= 1.90794   cumTime= 25.286         edges= 9799     schur= 0 lambda= 0.111875   levenbergIter= 2
iteration= 21    chi2= 50188802.496751   time= 1.0043    cumTime= 26.2903        edges= 9799     schur= 0 lambda= 0.074583   levenbergIter= 1
iteration= 22    chi2= 49380588.813712   time= 0.940104  cumTime= 27.2304        edges= 9799     schur= 0 lambda= 0.049722   levenbergIter= 1
iteration= 23    chi2= 48552200.604526   time= 0.99819   cumTime= 28.2286        edges= 9799     schur= 0 lambda= 0.033148   levenbergIter= 1
iteration= 24    chi2= 48405897.723593   time= 1.00536   cumTime= 29.234         edges= 9799     schur= 0 lambda= 0.022099   levenbergIter= 1
iteration= 25    chi2= 48285647.351651   time= 1.01389   cumTime= 30.2479        edges= 9799     schur= 0 lambda= 0.014732   levenbergIter= 1
iteration= 26    chi2= 48144893.438943   time= 1.00391   cumTime= 31.2518        edges= 9799     schur= 0 lambda= 0.009822   levenbergIter= 1
iteration= 27    chi2= 48008186.430056   time= 0.851683  cumTime= 32.1035        edges= 9799     schur= 0 lambda= 0.006548   levenbergIter= 1
iteration= 28    chi2= 47901148.863662   time= 1.00447   cumTime= 33.1079        edges= 9799     schur= 0 lambda= 0.004365   levenbergIter= 1
iteration= 29    chi2= 47839926.122896   time= 1.00723   cumTime= 34.1152        edges= 9799     schur= 0 lambda= 0.001455   levenbergIter= 1
saving optimization results ...