从零手写VIO(三)

---

从零手写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;
    }
}

公式推导

参考 公式推导