估计曲线

绘制阻尼因子随迭代变化的曲线

我的思路是保存代码中的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;// 统计优化变量的维数，为构建 H 矩阵做准备SetOrdering();// 遍历edge, 构建 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();// 优化退出条件1： delta_x_ 很小则退出if (delta_x_.squaredNorm() <= 1e-6 || false_cnt > 10) {stop = true;break;}// 更新状态量 X = X+ delta_xUpdateStates();// 判断当前步是否可行以及 LM 的 lambda 怎么更新oneStepSuccess = IsGoodStepInLM();// 后续处理，if (oneStepSuccess) {// 在新线性化点 构建 hessianMakeHessian();// TODO:: 这个判断条件可以丢掉，条件 b_max <= 1e-12 很难达到，这里的阈值条件不应该用绝对值，而是相对值
//                double b_max = 0.0;
//                for (int i = 0; i < b_.size(); ++i) {//                    b_max = max(fabs(b_(i)), b_max);
//                }
//                // 优化退出条件2： 如果残差 b_max 已经很小了，那就退出
//                stop = (b_max <= 1e-12);false_cnt = 0;} else {false_cnt++;RollbackStates();   // 误差没下降，回滚}}iter++;// 优化退出条件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,即可在工作区间下生成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变化图

在工作区新建一个文件夹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 所有字符串读进datafor line in data:  odom = line.split()        #将单个数据分隔开存好  numbers_float = map(float, odom) #转化为浮点数  y.append( numbers_float[0] )plt.plot(y)
plt.show()

运行该脚本

将曲线参数改成y=ax^2 +bx+c

代码修改如下：

残差计算

// 计算曲线模型误差virtual void ComputeResidual() override{Vec3 abc = verticies_[0]->Parameters();  // 估计的参数//residual_(0) = std::exp( abc(0)*x_*x_ + abc(1)*x_ + abc(2) ) - y_;  // 构建残差residual_(0) = abc(0)*x_*x_ + abc(1)*x_ + abc(2)  - y_;  // 构建残差}

雅克比计算

// 计算残差对变量的雅克比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维，状态量 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函数

主要是把观测值的计算公式改一下

// 构造 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结果不是很好，为了更好的结果，将数据点的个数N改为1000
结果：

其他阻尼因子更新策略

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// 统计所有的残差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;}
}

公式推导

参考公式推导

从零手写VIO(三)——LM算法相关推荐

从零手写VIO(7)
从零手写VIO(7) 文章目录从零手写VIO(7) 前言一.VINS-Course代码解析二.作业(7) 1.simulation-test.cpp修改 2.Sysytem.cpp修改 3.co ...
[从零手写VIO|第五节]——后端优化实践——单目BA求解代码解析
长篇警告⚠⚠⚠ 目录 solver 全流程回顾 Solver三要素 Solver求解中的疑问核心问题代码解析 1. TestMonoBA.cpp 2. 后端部分: 2.1 顶点 2.2 边(残差) ...
手写VIO --学习笔记 - Part1
目录一.VIO文献阅读二.四元数和李代数更新三.其他导数参考一.VIO文献阅读阅读VIO 相关综述文献,回答以下问题: 1.视觉与IMU进行融合之后有何优势? IMU:(Inertial ...
《视觉SLAM进阶：从零开始手写VIO》第二讲作业-IMU仿真、MU imu_utils标定
<视觉SLAM进阶:从零开始手写VIO>第二讲作业-IMU仿真.MU imu_utils标定作业题目: 1 仿真代码解析仿真代码地址:https://github.com/HeYiji ...
深蓝学院《从零开始手写VIO》作业三
深蓝学院<从零开始手写VIO>作业三深蓝学院<从零开始手写VIO>作业三 1. 代码修改 2. 公式推导 3. 公式证明: 深蓝学院<从零开始手写VIO>作业三 ...
从零开始手写VIO第三章作业（含关键点细节及思维过程）
文章目录前言·与同主题博文的不同 1.代码修改 1.1阻尼因子 µ 随着迭代变化的曲线图 1.2完成曲线y = ax^2^ + bx + c的参数估计 1.3实现其他更优秀的阻尼因子策略 2.公式推 ...
乘法逆元 java_java写的三个求乘法逆元的算法
java写的三个求乘法逆元的算法: 第一个是著名的扩展欧拉算法第二个是网上发现的一个牛人写的算法第三个是我这个菜鸟补充的一个最垃圾的算法在对较小的数进行运算的时候,后两种算法都比第一个快,第三个 ...
【图像隐写】基于LSB+DWT+DCT三种算法实现图像和音频水印嵌入提取含Matlab源码
1 简介基于LSB+DWT+DCT三种算法实现图像和音频水印嵌入提取. 1.1 LSB算法根据LSB算法简单易实现的特点,结合在图像置乱技术中很好特性的Arnold变换.利用变化产生影子图像.通过 ...
写了三百篇算法题解，关于如何刷题有些话我想对你说
这篇文章憋了我挺久的,感觉都快憋出内伤,一次次的打开 Typora 写几十个字,一次次的修改删除最后关闭 Typora,如此反复. 为什么会如此纠结? 或许是太狂妄了,我真的想让那些看了这篇文章的人都 ...
《视觉SLAM进阶：从零开始手写VIO》第三讲基于优化的IMU预积分与视觉信息融合作业
<视觉SLAM进阶:从零开始手写VIO>第三讲基于优化的IMU预积分与视觉信息融合作业文章目录 <视觉SLAM进阶:从零开始手写VIO>第三讲基于优化的IMU预积分与视 ...

从零手写VIO(三)——LM算法

文章目录