从零手写VIO的第三课的作业，在此记录
我总结的一份从零学VIO第三讲的思维导图从零手写VIO（三）
文章目录
绘制阻尼因子随迭代变化的曲线
我的思路是保存代码中的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, 直到成功迭代一步
{
// setLambda
AddLambdatoHessianLM();
// 第四步，解线性方程 H X = B
SolveLinearSystem();
//
RemoveLambdaHessianLM();
// 优化退出条件1： delta_x_ 很小则退出
if (delta_x_.squaredNorm() <= 1e-6 || false_cnt > 10) {
stop = true;
break;
}
// 更新状态量 X = X+ delta_x
UpdateStates();
// 判断当前步是否可行以及 LM 的 lambda 怎么更新
oneStepSuccess = IsGoodStepInLM();
// 后续处理，
if (oneStepSuccess) {
// 在新线性化点 构建 hessian
MakeHessian();
// 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 os
filepath = os.path.abspath('.')
y = []
with open(filepath + '/lambada.txt','r') as f:
data = f.readlines() #txt 所有字符串读进data
for 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);
// 观测 y
double 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;
}
}
