深藍學院《從零開始手寫VIO》作業五

• 1. 完成Bundle Adjustment求解器
• 2. 完成測試函數
• 3. 論文總結

1. 完成Bundle Adjustment求解器

完成單目 Bundle Adjustment 求解器 problem.cc 中的部分代碼。
? 完成 Problem::MakeHessian() 中信息矩陣 H 的計算。
? 完成 Problem::SolveLinearSystem() 中 SLAM 問題的求解

MakeHessian() 補充代碼如下：

// TODO:: home work. 完成 H index 的填寫. H.block(index_i,index_j, dim_i, dim_j).noalias() += hessian; if (j != i) {// 對稱的下三角// TODO:: home work. 完成 H index 的填寫.H.block(index_j,index_i, dim_j, dim_i).noalias() += hessian.transpose();

SolveLinearSystem()補充代碼如下：

// TODO:: home work. 完成矩陣塊取值，Hmm，Hpm，Hmp，bpp，bmm MatXX Hmm = Hessian_.block(reserve_size,reserve_size, marg_size, marg_size); MatXX Hmp = Hessian_.block(reserve_size,0, marg_size, reserve_size); MatXX Hpm = Hessian_.block(0, reserve_size, reserve_size, marg_size); VecX bpp = b_.segment(0,reserve_size); VecX bmm = b_.segment(reserve_size,marg_size); // TODO:: home work. 完成舒爾補 Hpp, bpp 代碼 MatXX tempH = Hpm * Hmm_inv; H_pp_schur_ = Hessian_.block(0,0,reserve_size,reserve_size) - tempH * Hmp; b_pp_schur_ = bpp - tempH * bmm; // TODO:: home work. step3: solve landmark VecX delta_x_ll(marg_size); delta_x_ll = Hmm_inv*(bmm-Hmp*delta_x_pp); delta_x_.tail(marg_size) = delta_x_ll;

運行結果如下：

0 order: 0 1 order: 6 2 order: 12ordered_landmark_vertices_ size : 20 iter: 0 , chi= 5.35099 , Lambda= 0.00597396 iter: 1 , chi= 0.0289048 , Lambda= 0.00199132 iter: 2 , chi= 0.000109162 , Lambda= 0.000663774 problem solve cost: 22.7924 msmakeHessian cost: 19.2487 msCompare MonoBA results after opt... after opt, point 0 : gt 0.220938 ,noise 0.227057 ,opt 0.220992 after opt, point 1 : gt 0.234336 ,noise 0.314411 ,opt 0.234854 after opt, point 2 : gt 0.142336 ,noise 0.129703 ,opt 0.142666 after opt, point 3 : gt 0.214315 ,noise 0.278486 ,opt 0.214502 after opt, point 4 : gt 0.130629 ,noise 0.130064 ,opt 0.130562 after opt, point 5 : gt 0.191377 ,noise 0.167501 ,opt 0.191892 after opt, point 6 : gt 0.166836 ,noise 0.165906 ,opt 0.167247 after opt, point 7 : gt 0.201627 ,noise 0.225581 ,opt 0.202172 after opt, point 8 : gt 0.167953 ,noise 0.155846 ,opt 0.168029 after opt, point 9 : gt 0.21891 ,noise 0.209697 ,opt 0.219314 after opt, point 10 : gt 0.205719 ,noise 0.14315 ,opt 0.205995 after opt, point 11 : gt 0.127916 ,noise 0.122109 ,opt 0.127908 after opt, point 12 : gt 0.167904 ,noise 0.143334 ,opt 0.168228 after opt, point 13 : gt 0.216712 ,noise 0.18526 ,opt 0.216866 after opt, point 14 : gt 0.180009 ,noise 0.184249 ,opt 0.180036 after opt, point 15 : gt 0.226935 ,noise 0.245716 ,opt 0.227491 after opt, point 16 : gt 0.157432 ,noise 0.176529 ,opt 0.157589 after opt, point 17 : gt 0.182452 ,noise 0.14729 ,opt 0.182444 after opt, point 18 : gt 0.155701 ,noise 0.182258 ,opt 0.155769 after opt, point 19 : gt 0.14646 ,noise 0.240649 ,opt 0.14677 ------------ pose translation ---------------- translation after opt: 0 :-0.000478009 0.00115904 0.000366508 || gt: 0 0 0 translation after opt: 1 :-1.06959 4.00018 0.863877 || gt: -1.0718 4 0.866025 translation after opt: 2 :-4.00232 6.92678 0.867244 || gt: -4 6.9282 0.866025

2. 完成測試函數

完成滑動窗口算法測試函數。
? 完成 Problem::TestMarginalize() 中的代碼,并通過測試。

補充代碼如下：

// TODO:: home work. 將變量移動到右下角 /// 準備工作： move the marg pose to the Hmm bottown right // 將 row i 移動矩陣最下面 Eigen::MatrixXd temp_rows = H_marg.block(idx, 0, dim, reserve_size); Eigen::MatrixXd temp_botRows = H_marg.block(idx + dim, 0, reserve_size - idx - dim, reserve_size); H_marg.block(idx, 0, dim, reserve_size) = temp_botRows; H_marg.block(idx + dim, 0, reserve_size - idx - dim, reserve_size) = temp_rows; // TODO:: home work. 完成舒爾補操作 Eigen::MatrixXd Arm = H_marg.block(0,n2,n2,m2); Eigen::MatrixXd Amr = H_marg.block(n2,0,m2,n2); Eigen::MatrixXd Arr = H_marg.block(0,0,n2,n2);

運行結果如下：

---------- TEST Marg: before marg------------100 -100 0-100 136.111 -11.11110 -11.1111 11.1111 ---------- TEST Marg: 將變量移動到右下角------------100 0 -1000 11.1111 -11.1111-100 -11.1111 136.111 ---------- TEST Marg: after marg------------26.5306 -8.16327 -8.16327 10.2041

3. 論文總結

論文我沒有完全看懂，但是我還是結合賀博的講解嘗試將其中提到的對于H矩陣的不同操作方式總一個總結：
論文和課件中由一丟丟不同的是，課件中討論的是視覺SLAM中的操作方式，而論文中討論的是VIO中的操作方式，這兩者本質不同在于起不可觀量不同，視覺SLAM中不可觀量由七個，而VIO中只有四個，論文中提到的不同操作方式一共由三種，分別是：
（1）gauge fixation
這種方式的操作是固定第一個相機的位置和yaw軸這四個狀態量，在論文中給出的公式為：$${\mathbf{p}}_0={\mathbf{p}}_0^0,\Delta {?}_{0z}?{\mathbf{e}}_z^{?}\Delta {?}_0=0$$類比到課件中的操作方式就是：

（2）gauge prior
這種方式的操作是添加先驗，增加系統可觀性，公式如下：$${\Vert {\mathbf{r}}_0^P\Vert }_{{\Sigma }_0^P}^2,\text{?where?}{\mathbf{r}}_0^P(\mathbf{\theta })?\left({\mathbf{p}}_0?{\mathbf{p}}_0^0,\Delta {?}_{0z}\right)$$類比到課件中的操作方式是：

（3）free gauge
這種才操作方式是不對可觀性進行操作，但是為了防止奇異矩陣的產生，采用偽逆或者增加一些阻尼項（列文伯格法），因此這種方法實施后可能會對可觀性造成影響，也就是會是系統零空間發生變換，對應到課件中操作方式是：

論文中的結論是三種方法的準確性和計算時間是相似的，free gauge的方法會稍微快一些，另外因為free gauge方法會使得零空間發生變化，因此它求解的結果會有很大不同。，下面這幅圖說明了三種方法之間的不同，我的個人理解是，黑線 ${\mathcal{M}}_{\theta }$是最小損失值的曲線， 藍色的線是四個自由度的流形空間的曲線，也是gauge fixation方法代表的曲線，因為gauge fixation中僅僅是固定了第一個狀態的位置，因此它的結果會沿著流形空間變換，而 綠色的線是gauge prior方法代表的曲線，因為其增加了先驗約束，可以說其損失函數發生了些許變換，因此其最后的狀態結果和gauge fixation方法也會由些許不同，而 紅色的線是free gauge方法代表的曲線，對于給定的損失函數，free gauge方法使用偽逆選擇最小尺寸的參數步驟，因此垂直于成本的等值線移動（最后這一點我不太明白），但是可以看出來其結果與前兩者會有很大區別。

與50位技術專家面對面20年技術見證，附贈技術全景圖

總結

以上是生活随笔為你收集整理的深蓝学院《从零开始手写VIO》作业五的全部內容，希望文章能夠幫你解決所遇到的問題。

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