手写VIO作业总结（三）

手写VIO作业总结（三）

手写VIO作业总结（一）
手写VIO作业总结（二）

手写VIO作业总结（四）

1.1pyhon实现绘图

import numpy as np
import matplotlib.pyplot as pl
from pylab import mpl
from pylab import *
x = [0,1,2,3,4,5,6,7,8,9,10]
y1 = [0.001,699.051,1864.14,621.378,207.126,69.042,23.014,7.67134,2.55711,0.852371,0.284124]
plt.plot(x, y1)
plt.xlabel('iter')
plt.ylabel('u')
plt.title('plot')
plt.show()

1.2

#include <iostream>
#include <random>
#include "backend/problem.h"

using namespace myslam::backend;
using namespace std;

// 曲线模型的顶点，模板参数：优化变量维度和数据类型
class CurveFittingVertex: public Vertex
{
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW

    CurveFittingVertex(): Vertex(3) {}  // abc: 三个参数， Vertex 是 3 维的
    virtual std::string TypeInfo() const { return "abc"; }
};

// 误差模型 模板参数：观测值维度，类型，连接顶点类型
class CurveFittingEdge: public Edge
{
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    CurveFittingEdge( double x, double y ): Edge(1,1, std::vector<std::string>{"abc"}) {
        x_ = x;
        y_ = y;
    }
    // 计算曲线模型误差
    virtual void ComputeResidual() override   //纯虚函数，由继承的基类实现
    {
        Vec3 abc = verticies_[0]->Parameters();  // 估计的参数
        residual_(0) =  abc(0)*x_*x_ + abc(1)*x_ + abc(2) - y_;  // 构建残差
    }

    // 计算残差对变量的雅克比
    virtual void ComputeJacobians() override   //纯虚函数，由继承的基类实现
    {
        Vec3 abc = verticies_[0]->Parameters();
        double y =  abc(0)*x_*x_ + abc(1)*x_ + abc(2) ;

        Eigen::Matrix<double, 1, 3> jaco_abc;  // 误差为1维，状态量 3 个（a,b,c），所以是 1x3 的雅克比矩阵
        jaco_abc << x_ * x_ , x_  , 1 ;  //分别对a,b,c求导
        jacobians_[0] = jaco_abc;
        //std::cout<<"jacobians"<<jaco_abc<<std::endl;
    }
    /// 返回边的类型信息
    virtual std::string TypeInfo() const override { return "CurveFittingEdge"; }
public:
    double x_,y_;  // x 值， y 值为 _measurement
};

int main()
{
    double a=10.0, b=20.0, c=10.0;         // 真实参数值
    int N = 100;                          // 数据点个数
    double w_sigma= 1.;                 // 噪声Sigma值

    std::default_random_engine generator;
    std::normal_distribution<double> noise(0.,w_sigma);//设置正态分布的噪声（均值为0,方差为1）

    // 构建 problem
    Problem problem(Problem::ProblemType::GENERIC_PROBLEM);
    shared_ptr< CurveFittingVertex > vertex(new CurveFittingVertex());

    // 设定待估计参数 a, b, c初始值
    vertex->SetParameters(Eigen::Vector3d (0.,0.,0.));
    // 将待估计的参数加入最小二乘问题
    problem.AddVertex(vertex);         //往图中增加    顶点(待估计的参数)

    // 构造 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 );

        // 每个观测对应的残差函数
        shared_ptr< CurveFittingEdge > edge(new CurveFittingEdge(x,y));
        std::vector<std::shared_ptr<Vertex>> edge_vertex;
        edge_vertex.push_back(vertex);
        edge->SetVertex(edge_vertex);

        // 把这个残差添加到最小二乘问题
        problem.AddEdge(edge);
    }

    std::cout<<"\nTest CurveFitting start..."<<std::endl;
    /// 使用 LM 求解
    problem.Solve(100);

    std::cout << "-------After optimization, we got these parameters :" << std::endl;
    std::cout << vertex->Parameters().transpose() << std::endl;
    std::cout << "-------ground truth: " << std::endl;
    std::cout << "10.0,  20.0,  10.0" << std::endl;

    // std
    return 0;
}