深藍(lán)學(xué)院《從零開始手寫VIO》作業(yè)五

• 1. 完成Bundle Adjustment求解器
• 2. 完成測試函數(shù)
• 3. 論文總結(jié)

1. 完成Bundle Adjustment求解器

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

MakeHessian() 補(bǔ)充代碼如下：

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

SolveLinearSystem()補(bǔ)充代碼如下：

// 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. 完成舒爾補(bǔ) 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;

運(yùn)行結(jié)果如下：

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. 完成測試函數(shù)

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

補(bǔ)充代碼如下：

// TODO:: home work. 將變量移動(dòng)到右下角 /// 準(zhǔn)備工作： move the marg pose to the Hmm bottown right // 將 row i 移動(dòng)矩陣最下面 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. 完成舒爾補(bǔ)操作 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);

運(yùn)行結(jié)果如下：

---------- TEST Marg: before marg------------100 -100 0-100 136.111 -11.11110 -11.1111 11.1111 ---------- TEST Marg: 將變量移動(dòng)到右下角------------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. 論文總結(jié)

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

（2）gauge prior
這種方式的操作是添加先驗(yàn)，增加系統(tǒng)可觀性，公式如下：$${\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
這種才操作方式是不對(duì)可觀性進(jìn)行操作，但是為了防止奇異矩陣的產(chǎn)生，采用偽逆或者增加一些阻尼項(xiàng)（列文伯格法），因此這種方法實(shí)施后可能會(huì)對(duì)可觀性造成影響，也就是會(huì)是系統(tǒng)零空間發(fā)生變換，對(duì)應(yīng)到課件中操作方式是：

論文中的結(jié)論是三種方法的準(zhǔn)確性和計(jì)算時(shí)間是相似的，free gauge的方法會(huì)稍微快一些，另外因?yàn)閒ree gauge方法會(huì)使得零空間發(fā)生變化，因此它求解的結(jié)果會(huì)有很大不同。，下面這幅圖說明了三種方法之間的不同，我的個(gè)人理解是，黑線 ${\mathcal{M}}_{\theta }$是最小損失值的曲線， 藍(lán)色的線是四個(gè)自由度的流形空間的曲線，也是gauge fixation方法代表的曲線，因?yàn)間auge fixation中僅僅是固定了第一個(gè)狀態(tài)的位置，因此它的結(jié)果會(huì)沿著流形空間變換，而 綠色的線是gauge prior方法代表的曲線，因?yàn)槠湓黾恿讼闰?yàn)約束，可以說其損失函數(shù)發(fā)生了些許變換，因此其最后的狀態(tài)結(jié)果和gauge fixation方法也會(huì)由些許不同，而 紅色的線是free gauge方法代表的曲線，對(duì)于給定的損失函數(shù)，free gauge方法使用偽逆選擇最小尺寸的參數(shù)步驟，因此垂直于成本的等值線移動(dòng)（最后這一點(diǎn)我不太明白），但是可以看出來其結(jié)果與前兩者會(huì)有很大區(qū)別。

與50位技術(shù)專家面對(duì)面20年技術(shù)見證，附贈(zèng)技術(shù)全景圖

總結(jié)

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

如果覺得生活随笔網(wǎng)站內(nèi)容還不錯(cuò)，歡迎將生活随笔推薦給好友。