---

從零手寫VIO的第三課的作業(yè)，在此記錄
我總結(jié)的一份從零學(xué)VIO第三講的思維導(dǎo)圖從零手寫VIO（三）

---

文章目錄

• 估計曲線
• • 繪制阻尼因子隨迭代變化的曲線
  • • 保存lamada
    • 畫出lambda變化圖
  • 將曲線參數(shù)改成y=ax^2 +bx+c
  • • 殘差計算
    • 雅克比計算
    • main函數(shù)
  • 其他阻尼因子更新策略
• 公式推導(dǎo)

估計曲線

繪制阻尼因子隨迭代變化的曲線

我的思路是保存代碼中的lamada，然后使用Python繪制出圖。

保存lamada

bool Problem::Solve(int iterations) {if (edges_.size() == 0 || verticies_.size() == 0) {std::cerr << "\nCannot solve problem without edges or verticies" << std::endl;return false;}TicToc t_solve;// 統(tǒng)計優(yōu)化變量的維數(shù)，為構(gòu)建 H 矩陣做準(zhǔn)備SetOrdering();// 遍歷edge, 構(gòu)建 H = J^T * J 矩陣MakeHessian();// LM 初始化ComputeLambdaInitLM();// LM 算法迭代求解bool stop = false;int iter = 0;std::vector<double> lambdas;std::string filename = "lambda.txt";std::ofstream save_lambda;save_lambada.open(filename.c_str());while (!stop && (iter < iterations)) {std::cout << "iter: " << iter << " , chi= " << currentChi_ << " , Lambda= " << currentLambda_<< std::endl;lambadas.push_back(currentLambda_);bool oneStepSuccess = false;int false_cnt = 0;while (!oneStepSuccess) // 不斷嘗試 Lambda, 直到成功迭代一步{// setLambdaAddLambdatoHessianLM();// 第四步，解線性方程 H X = BSolveLinearSystem();//RemoveLambdaHessianLM();// 優(yōu)化退出條件1： delta_x_ 很小則退出if (delta_x_.squaredNorm() <= 1e-6 || false_cnt > 10) {stop = true;break;}// 更新狀態(tài)量 X = X+ delta_xUpdateStates();// 判斷當(dāng)前步是否可行以及 LM 的 lambda 怎么更新oneStepSuccess = IsGoodStepInLM();// 后續(xù)處理，if (oneStepSuccess) {// 在新線性化點 構(gòu)建 hessianMakeHessian();// TODO:: 這個判斷條件可以丟掉，條件 b_max <= 1e-12 很難達(dá)到，這里的閾值條件不應(yīng)該用絕對值，而是相對值 // double b_max = 0.0; // for (int i = 0; i < b_.size(); ++i) { // b_max = max(fabs(b_(i)), b_max); // } // // 優(yōu)化退出條件2： 如果殘差 b_max 已經(jīng)很小了，那就退出 // stop = (b_max <= 1e-12);false_cnt = 0;} else {false_cnt++;RollbackStates(); // 誤差沒下降，回滾}}iter++;// 優(yōu)化退出條件3： currentChi_ 跟第一次的chi2相比，下降了 1e6 倍則退出if (sqrt(currentChi_) <= stopThresholdLM_)stop = true;}for(size_t i=0;i < lambdas.size(); i++){save_lambda<<lambdas[i]<<" "<<std::endl;}std::cout << "problem solve cost: " << t_solve.toc() << " ms" << std::endl;std::cout << " makeHessian cost: " << t_hessian_cost_ << " ms" << std::endl;return true; }

重新編譯代碼，運行testCurveFitting,即可在工作區(qū)間下生成lambda.txt

0.001
699.051
1864.14
1242.76
414.252
138.084
46.028
15.3427
5.11423
1.70474
0.568247
0.378832

畫出lambda變化圖

在工作區(qū)新建一個文件夾scripts，在此文件夾下面放置Python腳本
draw_lambda.py

# -*- coding: utf-8 -*- """ Created on Thu june 20 16:23:24 2020@author: hyj """import numpy as np; import matplotlib import matplotlib.pyplot as plt import osfilepath = os.path.abspath('.') y = [] with open(filepath + '/lambada.txt','r') as f:data = f.readlines() #txt 所有字符串讀進(jìn)datafor line in data: odom = line.split() #將單個數(shù)據(jù)分隔開存好 numbers_float = map(float, odom) #轉(zhuǎn)化為浮點數(shù) y.append( numbers_float[0] )plt.plot(y) plt.show()

運行該腳本

將曲線參數(shù)改成y=ax^2 +bx+c

代碼修改如下：

殘差計算

// 計算曲線模型誤差virtual void ComputeResidual() override{Vec3 abc = verticies_[0]->Parameters(); // 估計的參數(shù)//residual_(0) = std::exp( abc(0)*x_*x_ + abc(1)*x_ + abc(2) ) - y_; // 構(gòu)建殘差residual_(0) = abc(0)*x_*x_ + abc(1)*x_ + abc(2) - y_; // 構(gòu)建殘差}

雅克比計算

// 計算殘差對變量的雅克比virtual void ComputeJacobians() override{//Vec3 abc = verticies_[0]->Parameters();//double exp_y = std::exp( abc(0)*x_*x_ + abc(1)*x_ + abc(2) );Eigen::Matrix<double, 1, 3> jaco_abc; // 誤差為1維，狀態(tài)量 3 個，所以是 1x3 的雅克比矩陣//jaco_abc << x_ * x_ * exp_y, x_ * exp_y , 1 * exp_y;jaco_abc << x_ * x_ , x_ , 1 ;jacobians_[0] = jaco_abc;}

main函數(shù)

主要是把觀測值的計算公式改一下

// 構(gòu)造 N 次觀測for (int i = 0; i < N; ++i) {double x = i/100.;double n = noise(generator);// 觀測 ydouble y = a*x*x + b*x + c + n; // double y = std::exp( a*x*x + b*x + c );

N=100的時候，估計出的abc結(jié)果不是很好，為了更好的結(jié)果，將數(shù)據(jù)點的個數(shù)N改為1000
結(jié)果：

其他阻尼因子更新策略

Marquardt 更新策略

Nielsen 更新策略：

Nielsen更新策略代碼為：

bool Problem::IsGoodStepInLM() {double scale = 0;scale = delta_x_.transpose() * (currentLambda_ * delta_x_ + b_);scale += 1e-3; // make sure it's non-zero :)// recompute residuals after update state// 統(tǒng)計所有的殘差double tempChi = 0.0;for (auto edge: edges_) {edge.second->ComputeResidual();tempChi += edge.second->Chi2();}double rho = (currentChi_ - tempChi) / scale;if (rho > 0 && isfinite(tempChi)) // last step was good, 誤差在下降{double alpha = 1. - pow((2 * rho - 1), 3);alpha = std::min(alpha, 2. / 3.);double scaleFactor = (std::max)(1. / 3., alpha);currentLambda_ *= scaleFactor;ni_ = 2;currentChi_ = tempChi;return true;} else {currentLambda_ *= ni_;ni_ *= 2;return false;} }

公式推導(dǎo)

參考 公式推導(dǎo)

總結(jié)

以上是生活随笔為你收集整理的从零手写VIO(三)——LM算法的全部內(nèi)容，希望文章能夠幫你解決所遇到的問題。

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