视觉slam十四讲 6.非线性优化

公式编辑：https://codecogs.com/latex/eqneditor.php

1.状态估计问题

1.1 最大后验与最大似然

1. 经典 SLAM 模型：

   
   

   其中 $x_k$为相机的位姿，$u$ 为输入数据，即为采集到的数据
   假如我们在$x_k$处观测到路标 $y_j$，对应到图像上的像素位置$z_{k,j}$，那么我们的观测方程可以表示为：
   $$s{z}_{k,j}=Kexp\left({\xi }^{\wedge }\right)y_i$$

2. 在运动和观测方程中，通常假设两个噪声项 $w_k$，$v_k$满足零均值的高斯分布：
   $$w_{k}N\left(0,R_k\right),v_{k}N\left(0,Q_{k,j}\right)$$

3. 状态估计问题：通过带噪声的数据 $z$和 $u$ ，推断位姿 $x$ 和地图 $y$（以及它们的概率分布），其中有优化方法有：
   （1）扩展卡尔曼滤波器（EKF）求解，关心的是当前时刻的状态估计，而对直之前的状态没有多加考虑。
   （2）非线性优化，使用所有时刻采集到的数据进行状态估计，被认为优于传统滤波器，成为当前视觉slam的主流。
   非线性优化把所待估计的变量都放在一个状态变量中：
   $$x=\left\{x_1,...,x_N,y_1,...,y_M\right\}$$
   已知：输入读数$u$，观测值$z$。（带噪声）
   目标：$P\left(x|z,u\right)$，只考虑$z$时：$P\left(x|z\right)$

4. 贝叶斯法则：
   $$P\left(x|z\right)=\frac{P\left(z|x\right)P\left(x\right)}{P\left(z\right)}\to P\left(z|x\right)P\left(x\right)$$

5. 贝叶斯法则左侧通常称为后验概率。它右侧的$P\left(z|x\right)$称为似然，另一部分$P\left(x\right)$称为先验.

6. 直接求后验分布较困难，转而求一个状态最优估计，使得在该状态下，后验概率最大化(MPA):
   $$x^{\ast }MAP=argmaxP\left(x|z\right)=argmaxP\left(z\right)$$

   若不知道机器人位姿大概在什么地方，此时就没有了先验。进而可以求解$x$的最大似然估计：
   $$x_{MLE}^{\ast }=argmaxP\left(z|x\right)$$
   最大似然估计，可以理解成：“在什么样的状态下，最可能产生现在观测到的数据”

1.2 最小二乘的引出

1. 对于某一次观测：
   $$z_{k,j}=h\left(y_j,x_k\right)+v_{k,j}$$

   假设噪声项 $v_k$∼$N\left(0,Q_{k,j}\right)$，所以观测数据的条件概率为：
   $$P\left(z_{k,j}|x_k,y_j\right)=N\left(h\left(y_i,x_k\right),Q_{k,j}\right)$$

2. 为了计算使它最大化的$x_k,y_k$,使用最小化负对数的方式，来求一个高斯分布的最大似然。考虑一个任意的高维高斯分布$v_k$∼$N\left(\mu ,\Sigma \right)$ ，它的概率密度函数展开形式为:
   $$P\left(x\right)=\frac{1}{\sqrt{{\left(2\pi \right)}^{N}det(\sum )}}exp(-\frac{1}{2}{\left(x-\mu \right)}^T{\Sigma }^{-1}\left(x-\mu \right))$$
   取负对数：
   $$-\ln \left(P\left(x\right)\right)=\frac{1}{2}\ln \left({\left(2\pi \right)}^{N}det\left(\Sigma \right)\right)+\frac{1}{2}{\left(x-\mu \right)}^T{\Sigma }^{-1}\left(x-\mu \right)$$

3. 第一项与 $x$ 无关，可以略去。代入 SLAM 的观测模型，相当于在求：
   $$x^{\ast }=argmin\left((z_{k,j}-h(x_k,y_j){)}^T{Q}_{k,j}^{-1}(z_{k,j}-h(x_k,y_j))\right)$$ 该式等价于最小化噪声项（即误差）的平方（$\Sigma$ 范数意义下）

4. 对于所有的运动和任意的观测，定义数据与估计值之间的误差：$$\begin{gathered} e_{v,k}=x_k-f\left(x_{k-1},u_k\right) \\ {e}_{v,j,k}=z_{k,j}-f\left(x_k,y_{}\right) \end{gathered}$$

   并求该误差的平方之和：
   $$J(x)=\sum _k{e}_T^{v,k}{R}_k^{-1}{e}_{v,k}+\sum _k\sum _j{e}_{y,k,j}^T{Q}_{k,j}^{-1}{e}_{y,k,j}$$
   这样就得到一个总体意义下的最小二乘问题，它的最优解等价于状态的最大似然估计。由于噪声的存在，当我们把估计的轨迹与地图代入SLAM的运动、观测方程时，并不会很完美，可以对状态进行微调，使得整体的误差下降到一个极小值。

5. SLAM中的最小二乘问题具有一些特定的结构：
   (1)整个问题的目标函数由许多个误差的（加权的）平方和组成。虽然总体的状态变量维数很高，但每个误差项都是简单的，仅与一俩个状态变量有关，运动误差只与$x_{k-1},x_k$有关，观测误差只与$x_k,y_j$有关，每个误差是个小规模约束，把这误差项的小雅克比矩阵放在整体的雅克比矩阵中。称每个误差项对应优化变量为参数快。
   整体误差由很多小误差项之和组成的问题，其增量方程具有一定的稀疏性。
   (2)如果使用李代数表示，那么该问题转换成**无约束最小二乘问题*，用旋转矩阵描述姿态会引入自身的约束
   (3)使用二范数度量误差，相当于欧式空间中距离的平方。

2.非线性最小二乘

最简单的最小二乘问题:
$$\underset{x}{\min }\frac{1}{2}{\Vert f(x)\Vert }_2^2$$
为求其最小值，则需要求其导数，然后得到其求解 x的最优解
对于不方便求解的最小二乘问题，可以用迭代的方法，从初始值出发，不断的跟新当前的优化变量，使目标函数下降，具体步骤有：
（1）给定某个初始值。
（2）对于第k次迭代，寻找增量Δxk, 使得${\Vert f(x_k+\Delta x_k)\Vert }_2^2$这里应该是二范数）达到最小。
（3）若$\Delta x_k$足够小，则停止。
（4）否则,令$x_{k+1}=x_k+\Delta x_k$,返回第2步。

2.1 一阶和二阶梯度法

1. 将目标函数在 $x$ 附近进行泰勒展开：
   $${\Vert f(x+\Delta x)\Vert }_2^2\approx {\Vert f(x)\Vert }_2^2+J(x)\Delta x+\frac{1}{2}\Delta x^{T}H\Delta x$$
   $J$是${\Vert f(x)\Vert }_2^2$关于$x$的导数（雅克比矩阵），$H$是二阶导数（海塞矩阵）
2. 最速梯度下降法：只保留一阶梯度，增量的方向为：
   $$\Delta x^{\ast }=-J^T(x)$$
3. 牛顿法：保留二阶梯度，增量方程为：
   $$\Delta x^{\ast }=argmin{\Vert f(x)\Vert }_2^2+J(x)\Delta x+\frac{1}{2}{x}^{T}H\Delta x$$
   求右侧等式关于 ∆x的导数并令它为零，得到了增量的解：
   $$H\Delta X=-J^T$$
4. 两种方法的问题：
   （1）最速下降法过于贪心，容易走出锯齿路线，反而增加了迭代次数。
   （2）牛顿法则需要计算目标函数的 H矩阵，这在问题规模较大时非常困难，通常倾向于避免 $H$ 的计算。

2.2 高斯牛顿法

1. 高斯牛顿法，它的思想是将$f\left(x\right)$进行泰勒展开（目标函数不是$f\left(x\right)$）：
   $$f(x+\Delta x)\approx f(x)+J(x)\Delta x$$
   $f\left(J\right)$是一个$m\ast n$雅克比矩阵，当前的目标是为了寻找下降矢量 ∆x，使得$\Vert f{\left(x+\Delta x\right)}^2\Vert$达到最小。

2. 为了求 $\Delta x_k$，需要解一个线性的最小二乘问题
   $$\Delta x^{\ast }=arg\underset{\Delta x}{\min }\frac{1}{2}{\Vert f(x)+J(x)\Delta x\Vert }^2$$
   考虑的是$\Delta x_k$的导数（而不是$x$） ，先展开目标函数的平方项
   $$\begin{gathered} \frac{1}{2}{\Vert f(x)+J(x)\Delta x\Vert }^2=\frac{1}{2}(f(x)+J(x)\Delta x{)}^T(f(x)+J(x)\Delta x) \\ =\frac{1}{2}(\Vert f(x)\Vert {)}_2^2+2f(x{)}^{T}J(x)\Delta x+\Delta xJ(x{)}^{T}J(x)\Delta x) \end{gathered}$$

   上式关于$\Delta x$的导数，并令其为零：
   $$J(x{)}^{T}J(x)\Delta x=-J(x{)}^{T}f(x)$$

   这个方程称之为增量方程，也称之为高斯牛顿方程或正规方程，将左边的系数设为$H$，右边的系数设为$g$，则有$H\Delta x=g$。

3. 高斯牛顿法求解的算法步骤可写成：
   （1）给定初始值$x_0$
   （2）对于第$k$次迭代，求出当前的雅克比矩阵$J\left(x_k\right)$ 和误差$f\left(x_k\right)$ 。
   （3）求解增量方程：$H\Delta x_k=g$
   （4）若$\Delta x_k$足够小，则停止。否则，令$x_{k+1}=x_k+\Delta x_k$，返回第二步

4. 高斯牛顿法的缺点：
   （1）要求$H$是可逆的，而且是正定的，如果出现$H$矩阵奇异或者病态，此时增量的稳定性较差，导致算法不收敛
   （2）步长问题，若求出来的$\Delta x_k$太长，则可能出现局部近似不够准确，无法保证迭代收敛。

2.3 列文伯格-马夸尔特方法（阻尼牛顿法)

1. 信赖区域方法 (Trust Region Method)：给 $\Delta x$添加一个信赖区域（Trust Region），不能让它太大而使得近似不准确

2. 考虑使用如下公式来判断泰勒近似是否够好
   $$\rho =\frac{f(x+\Delta x)-\Delta x}{J(x)\Delta x}$$
   （1）如果 $\rho$ 接近于 1，则近似是好的。
   （2）如果 $\rho$太小，说明实际减小的值远少于近似减小的值，则认为近似比较差，需要缩小近似范围。
   （3）如果 $\rho$ 比较大，则说明实际下降的比预计的更大，我们可以放大近似范围。

3. 改良版的非线性优化框架
   
   这里近似范围扩大的倍数和阈值都是经验值，可以替换成其他数值。上述约束中相当于把增量限定在半径为$\mu$的球里面，认为在球内的才有效。带上$D$后成为椭圆，把$D$取成单位阵$I$,相当于把$\Delta x$约束在一个球内。$D$也可为非负数对角阵。

4. 由于是有约束优化，可以利用拉格朗日乘子将其转化为一个无约束优化问题：
   $$\underset{\Delta x_k}{\min }\frac{1}{2}{\Vert f(x_k+J(x_k\Delta x_k))\Vert }^2+\frac{\lambda }{2}{\Vert D\Delta x\Vert }^2$$

   类似高斯牛顿法展开，可得其增量的线性方程：
   $$(H+\lambda D^{T}D)\Delta X=g$$

   考虑它的简化形式，即$D=I$，那么相当于求解：
   $$(H+\lambda I)\Delta x=g$$

（1）当参数$\lambda$较小时，$H$占主导地位，说明二次近似模型在该范围内是比较好的，该方法接近于高斯牛顿法。
（2）当参数$\lambda$较大时，列文伯格-马夸尔特方法接近于一阶梯度下降法，说明二次近似不好。
（3）该方法可在一定程度上避免线性方程组的系数矩阵的非奇异和病态问题)，提供更稳定、更准确的增量$\Delta x$。

5. 非线性优化的框架分为Line Search和Trust Region两类。
   （1）Line Search：先固定搜索方向，后寻找步长，以最速梯度法和高斯牛顿法为代表。
   （2）Trust Region：先固定搜索区域，再考虑找该区域的最优点，以列文伯格-马夸尔特方法为代表

3.实践：Ceres

3.1 Ceres简介

Ceres库面向通用的最小二乘问题的求解，需要定义优化问题，设置一些选项，输入Ceres求解。一般形式如下（带边界核函数最小二乘）：

$$\begin{gathered} \underset{x}{\min }\frac{1}{2}\sum _i\left({\Vert (f_i(x_{i1},...x_{in}))\Vert }^2\right) \\ s.t{l}_j\le x_j\le {\mu }_j \end{gathered}$$
优化变量：$x_{i1},...x_{in}$ ，代价函数$f_i$ ,$\rho$为恒等函数时，得到一个无约束的最小二乘问题。

3.2 使用Ceres拟合曲线

曲线方程的形式为：
$$y=exp(a{x}^2+bx+c)+w$$
$a,b,c为参数，$w$为噪声，假设我们有N个关于$x,y$观测数据点，根据这些点求曲线的参数，可以求解下面的最小二乘来狙击曲线参数。
$$\underset{a,b,c}{\min }\frac{1}{2}\sum _{i=1}^N{\Vert y_i-exp(a{x}_i^2+b{x}_i+c)\Vert }^2$$

参考：。
Ceres官网：http://www.ceres-solver.org/
一文助你Ceres 入门——Ceres Solver新手向全攻略：https://blog.csdn.net/cqrtxwd/article/details/78956227
Ceres: Tutorial: Non-linear Least Squares：http://www.ceres-solver.org/nnls_tutorial.html
基于范围的for循环:https://blog.csdn.net/lixiaogang_theanswer/article/details/79969012

#include 
#include 
#include 
#include 

using namespace std;

// 代价函数的计算模型
//首先要定义求解问题的cost function
struct CURVE_FITTING_COST
{
    //在结构体中首先构造构造函
    CURVE_FITTING_COST ( double x, double y ) : _x ( x ), _y ( y ) {}////结构体的构造函数初始化列表
    //相当于CURVE_FITTING_COST ( double x,double y) {_x=x; _y=y}
    // 残差的计算
    //template的使用是为了简化不同类型的函数和类的重复定义，模版实例化时可以替换任意类型，不仅包括内置类型（int等），也包括自定义类型class,实例化之后才知道的数据的类型。
    template <typename T>
    bool operator() (
        const T* const abc,     // 模型参数，有3维
        T* residual ) const     // 残差
    {
        residual[0] = T ( _y ) - ceres::exp ( abc[0]*T ( _x ) *T ( _x ) + abc[1]*T ( _x ) + abc[2] ); // y-exp(ax^2+bx+c)
        return true;
    }
    const double _x, _y;    // x,y数据
};

int main ( int argc, char** argv )
{
    double a=1.0, b=2.0, c=1.0;         // 真实参数值
    int N=100;                          // 数据点
    double w_sigma=1.0;                 // 噪声Sigma值
    cv::RNG rng;                        // OpenCV随机数产生器
    double abc[3] = {0,0,0};            // abc参数的估计值

    vector<double> x_data, y_data;      // 数据

    cout<<"generating data: "<<endl;
    for ( int i=0; i<N; i++ )
    {
        double x = i/100.0;
        x_data.push_back ( x );
        y_data.push_back (
            exp ( a*x*x + b*x + c ) + rng.gaussian ( w_sigma )
        );
        cout<<x_data[i]<<" "<<y_data[i]<<endl;
    }

    // 构建最小二乘问题
    ceres::Problem problem;
    for ( int i=0; i<N; i++ )
    {
        problem.AddResidualBlock (     // 向问题中添加误差项
        // 使用自动求导，模板参数：误差类型，输出维度，输入维度，维数要与前面struct中一致
            new ceres::AutoDiffCostFunction<CURVE_FITTING_COST, 1, 3> ( 
                new CURVE_FITTING_COST ( x_data[i], y_data[i] )
            ),
            nullptr,            // 核函数，这里不使用，为空
            abc                 // 待估计参数
        );
    }

    // 配置求解器
    ceres::Solver::Options options;     // 这里有很多配置项可以填
    options.linear_solver_type = ceres::DENSE_QR;  // 增量方程如何求解
    options.minimizer_progress_to_stdout = true;   // 输出到cout

    ceres::Solver::Summary summary;                // 优化信息
    chrono::steady_clock::time_point t1 = chrono::steady_clock::now();
    ceres::Solve ( options, &problem, &summary );  // 开始优化
    chrono::steady_clock::time_point t2 = chrono::steady_clock::now();
    chrono::duration<double> time_used = chrono::duration_cast<chrono::duration<double>>( t2-t1 );
    cout<<"solve time cost = "<<time_used.count()<<" seconds. "<<endl;

    // 输出结果
    cout<<summary.BriefReport() <<endl;
    cout<<"estimated a,b,c = ";
    //表示的则是基于范围的for循环
    for ( auto a:abc ) cout<<a<<" ";
    cout<<endl;

    return 0;
}

代码中需要关注的有这么几点：

（1）定义代价函数的方式是自定义一个结构体。只需要在结构体中实现operator()方法，就算是给Ceres提供了一个调用接口，由Ceres内部在合适的时候调用该方法计算代价函数。
（2）构建最小二乘问题时，需要将所有误差项依次添加进去。而每个误差项又是由前面定义的结构体构成的。需要注意的是，每个误差项需要指定代价函数求导的方法，有三种方法可供选择：自动求导、数值求导和自行推导。一般采用自动求导，既方便，又节约运行时时间。
（3）以自动求导为例，ceres::AutoDiffCostFunction是个模板类，后两个模板参数为输出维度和输入维度，必须与前面定义的结构体中的residual和abc的维度一样。
（4）使用ceres::Solver::Options配置求解器。这个类有许多字段，每个字段都提供了一些枚举值供用户选择。所以需要时只要查一查文档就知道怎么设置了。

调用build/curve_fitting查看优化结果：

generating data: 
0 2.71828
0.01 2.93161
0.02 2.12942
0.03 2.46037
0.04 4.18814
0.05 2.73368
0.06 2.42751
……
0.94 44.285
0.95 42.8312
0.96 47.7941
0.97 48.5931
0.98 51.8487
0.99 51.0258
iter      cost      cost_change  |gradient|   |step|    tr_ratio  tr_radius  ls_iter  iter_time  total_time
   0  1.824887e+04    0.00e+00    1.38e+03   0.00e+00   0.00e+00  1.00e+04        0    3.88e-05    9.01e-05
   1  2.748700e+39   -2.75e+39    0.00e+00   7.67e+01  -1.52e+35  5.00e+03        1    6.85e-05    1.82e-04
   2  2.429783e+39   -2.43e+39    0.00e+00   7.62e+01  -1.35e+35  1.25e+03        1    2.18e-05    2.16e-04
   3  1.213227e+39   -1.21e+39    0.00e+00   7.30e+01  -6.73e+34  1.56e+02        1    1.87e-05    2.44e-04
   4  1.852387e+37   -1.85e+37    0.00e+00   5.56e+01  -1.03e+33  9.77e+00        1    1.80e-05    2.69e-04
   5  6.714689e+31   -6.71e+31    0.00e+00   2.96e+01  -3.85e+27  3.05e-01        1    1.82e-05    2.95e-04
   6  9.500531e+12   -9.50e+12    0.00e+00   9.50e+00  -8.39e+08  4.77e-03        1    1.78e-05    3.20e-04
   7  1.776982e+04    4.79e+02    1.83e+03   2.58e-01   1.18e+00  1.43e-02        1    4.06e-05    3.68e-04
   8  1.599969e+04    1.77e+03    3.45e+03   5.53e-01   1.46e+00  4.29e-02        1    3.86e-05    4.15e-04
   9  1.060557e+04    5.39e+03    7.62e+03   7.33e-01   1.68e+00  1.29e-01        1    3.92e-05    4.62e-04
  10  3.669783e+03    6.94e+03    9.60e+03   5.25e-01   1.39e+00  3.86e-01        1    3.76e-05    5.06e-04
  11  5.397541e+02    3.13e+03    5.00e+03   2.66e-01   1.12e+00  1.16e+00        1    3.74e-05    5.51e-04
  12  1.484444e+02    3.91e+02    1.22e+03   8.46e-02   1.02e+00  3.48e+00        1    3.74e-05    5.95e-04
  13  1.216815e+02    2.68e+01    3.76e+02   4.17e-02   1.01e+00  1.04e+01        1    3.73e-05    6.40e-04
  14  9.290109e+01    2.88e+01    2.42e+02   9.10e-02   1.01e+00  3.13e+01        1    3.86e-05    6.85e-04
  15  6.674330e+01    2.62e+01    1.09e+02   1.33e-01   1.00e+00  9.39e+01        1    3.73e-05    7.30e-04
  16  5.936574e+01    7.38e+00    2.14e+01   1.08e-01   9.94e-01  2.82e+02        1    3.72e-05    7.75e-04
  17  5.653118e+01    2.83e+00    1.36e+01   1.57e-01   9.98e-01  8.45e+02        1    4.25e-05    8.26e-04
  18  5.310764e+01    3.42e+00    8.50e+00   2.81e-01   9.89e-01  2.53e+03        1    3.74e-05    8.71e-04
  19  5.125939e+01    1.85e+00    2.84e+00   2.98e-01   9.90e-01  7.60e+03        1    3.77e-05    9.16e-04
  20  5.097693e+01    2.82e-01    4.34e-01   1.48e-01   9.95e-01  2.28e+04        1    3.72e-05    9.60e-04
  21  5.096854e+01    8.39e-03    3.24e-02   2.87e-02   9.96e-01  6.84e+04        1    3.69e-05    1.00e-03
solve time cost = 0.00104852 seconds. 
Ceres Solver Report: Iterations: 22, Initial cost: 1.824887e+04, Final cost: 5.096854e+01, Termination: CONVERGENCE
estimated a,b,c = 0.891943 2.17039 0.944142 

4.实践：g2o

4.1 图优化理论简介

图优化，是把优化问题表现成图的一种形式，顶点表示优化变量，边表示误差项，对于任意一个形式的非最小二乘问题，可以构建与之对应的一个图。

4.2 使用g2o拟合曲线

#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
using namespace std; 

// 曲线模型的顶点，模板参数：优化变量维度和数据类型
class CurveFittingVertex: public g2o::BaseVertex<3, Eigen::Vector3d>
{
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    virtual void setToOriginImpl() // 重置
    {
        _estimate << 0,0,0;
    }
    
    virtual void oplusImpl( const double* update ) // 更新
    {
        _estimate += Eigen::Vector3d(update);
    }
    // 存盘和读盘：留空
    virtual bool read( istream& in ) {}
    virtual bool write( ostream& out ) const {}
};

// 误差模型 模板参数：观测值维度，类型，连接顶点类型
class CurveFittingEdge: public g2o::BaseUnaryEdge<1,double,CurveFittingVertex>
{
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    CurveFittingEdge( double x ): BaseUnaryEdge(), _x(x) {}
    // 计算曲线模型误差
    void computeError()
    {
        const CurveFittingVertex* v = static_cast<const CurveFittingVertex*> (_vertices[0]);
        const Eigen::Vector3d abc = v->estimate();
        _error(0,0) = _measurement - std::exp( abc(0,0)*_x*_x + abc(1,0)*_x + abc(2,0) ) ;
    }
    virtual bool read( istream& in ) {}
    virtual bool write( ostream& out ) const {}
public:
    double _x;  // x 值， y 值为 _measurement
};

int main( int argc, char** argv )
{
    double a=1.0, b=2.0, c=1.0;         // 真实参数值
    int N=100;                          // 数据点
    double w_sigma=1.0;                 // 噪声Sigma值
    cv::RNG rng;                        // OpenCV随机数产生器
    double abc[3] = {0,0,0};            // abc参数的估计值

    vector<double> x_data, y_data;      // 数据
    
    cout<<"generating data: "<<endl;
    for ( int i=0; i<N; i++ )
    {
        double x = i/100.0;
        x_data.push_back ( x );
        y_data.push_back (
            exp ( a*x*x + b*x + c ) + rng.gaussian ( w_sigma )
        );
        cout<<x_data[i]<<" "<<y_data[i]<<endl;
    }
    
    // 构建图优化，先设定g2o
    typedef g2o::BlockSolver< g2o::BlockSolverTraits<3,1> > Block;  // 每个误差项优化变量维度为3，误差值维度为1
    Block::LinearSolverType* linearSolver = new g2o::LinearSolverDense<Block::PoseMatrixType>(); // 线性方程求解器
    Block* solver_ptr = new Block( linearSolver );      // 矩阵块求解器
    // 梯度下降方法，从GN, LM, DogLeg 中选
    g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg( solver_ptr );
    // g2o::OptimizationAlgorithmGaussNewton* solver = new g2o::OptimizationAlgorithmGaussNewton( solver_ptr );
    // g2o::OptimizationAlgorithmDogleg* solver = new g2o::OptimizationAlgorithmDogleg( solver_ptr );
    g2o::SparseOptimizer optimizer;     // 图模型
    optimizer.setAlgorithm( solver );   // 设置求解器
    optimizer.setVerbose( true );       // 打开调试输出
    
    // 往图中增加顶点
    CurveFittingVertex* v = new CurveFittingVertex();
    v->setEstimate( Eigen::Vector3d(0,0,0) );
    v->setId(0);
    optimizer.addVertex( v );
    
    // 往图中增加边
    for ( int i=0; i<N; i++ )
    {
        CurveFittingEdge* edge = new CurveFittingEdge( x_data[i] );
        edge->setId(i);
        edge->setVertex( 0, v );                // 设置连接的顶点
        edge->setMeasurement( y_data[i] );      // 观测数值
        edge->setInformation( Eigen::Matrix<double,1,1>::Identity()*1/(w_sigma*w_sigma) ); // 信息矩阵：协方差矩阵之逆
        optimizer.addEdge( edge );
    }
    
    // 执行优化
    cout<<"start optimization"<<endl;
    chrono::steady_clock::time_point t1 = chrono::steady_clock::now();
    optimizer.initializeOptimization();
    optimizer.optimize(100);
    chrono::steady_clock::time_point t2 = chrono::steady_clock::now();
    chrono::duration<double> time_used = chrono::duration_cast<chrono::duration<double>>( t2-t1 );
    cout<<"solve time cost = "<<time_used.count()<<" seconds. "<<endl;
    
    // 输出优化值
    Eigen::Vector3d abc_estimate = v->estimate();
    cout<<"estimated model: "<<abc_estimate.transpose()<<endl;
    
    return 0;
}

优化结果：

generating data: 
0 2.71828
0.01 2.93161
0.02 2.12942
0.03 2.46037
……
0.96 47.7941
0.97 48.5931
0.98 51.8487
0.99 51.0258
start optimization
iteration= 0	 chi2= 30373.727656	 time= 9.4326e-05	 cumTime= 9.4326e-05	 edges= 100	 schur= 0	 lambda= 699.050482	 levenbergIter= 7
iteration= 1	 chi2= 13336.948288	 time= 5.0567e-05	 cumTime= 0.000144893	 edges= 100	 schur= 0	 lambda= 1864.134619	 levenbergIter= 3
iteration= 2	 chi2= 6946.262996	 time= 4.1451e-05	 cumTime= 0.000186344	 edges= 100	 schur= 0	 lambda= 1242.756412	 levenbergIter= 1
iteration= 3	 chi2= 271.023166	 time= 4.0473e-05	 cumTime= 0.000226817	 edges= 100	 schur= 0	 lambda= 414.252137	 levenbergIter= 1
iteration= 4	 chi2= 118.903887	 time= 4.2708e-05	 cumTime= 0.000269525	 edges= 100	 schur= 0	 lambda= 138.084046	 levenbergIter= 1
iteration= 5	 chi2= 113.568660	 time= 4.0511e-05	 cumTime= 0.000310036	 edges= 100	 schur= 0	 lambda= 46.028015	 levenbergIter= 1
iteration= 6	 chi2= 107.476457	 time= 4.0557e-05	 cumTime= 0.000350593	 edges= 100	 schur= 0	 lambda= 15.342672	 levenbergIter= 1
iteration= 7	 chi2= 103.014522	 time= 4.0922e-05	 cumTime= 0.000391515	 edges= 100	 schur= 0	 lambda= 5.114224	 levenbergIter= 1
iteration= 8	 chi2= 101.988348	 time= 4.0378e-05	 cumTime= 0.000431893	 edges= 100	 schur= 0	 lambda= 1.704741	 levenbergIter= 1
iteration= 9	 chi2= 101.937388	 time= 4.0728e-05	 cumTime= 0.000472621	 edges= 100	 schur= 0	 lambda= 0.568247	 levenbergIter= 1
iteration= 10	 chi2= 101.937021	 time= 4.226e-05	 cumTime= 0.000514881	 edges= 100	 schur= 0	 lambda= 0.378831	 levenbergIter= 1
iteration= 11	 chi2= 101.937020	 time= 4.0703e-05	 cumTime= 0.000555584	 edges= 100	 schur= 0	 lambda= 0.252554	 levenbergIter= 1
iteration= 12	 chi2= 101.937020	 time= 4.9381e-05	 cumTime= 0.000604965	 edges= 100	 schur= 0	 lambda= 1.346956	 levenbergIter= 3
iteration= 13	 chi2= 101.937020	 time= 4.1188e-05	 cumTime= 0.000646153	 edges= 100	 schur= 0	 lambda= 0.897971	 levenbergIter= 1
iteration= 14	 chi2= 101.937020	 time= 5.331e-05	 cumTime= 0.000699463	 edges= 100	 schur= 0	 lambda= 38.313418	 levenbergIter= 4
iteration= 15	 chi2= 101.937020	 time= 6.0704e-05	 cumTime= 0.000760167	 edges= 100	 schur= 0	 lambda= 204.338228	 levenbergIter= 3
iteration= 16	 chi2= 101.937020	 time= 4.0342e-05	 cumTime= 0.000800509	 edges= 100	 schur= 0	 lambda= 136.225485	 levenbergIter= 1
iteration= 17	 chi2= 101.937020	 time= 7.0952e-05	 cumTime= 0.000871461	 edges= 100	 schur= 0	 lambda= 24378500205.440712	 levenbergIter= 8
iteration= 18	 chi2= 101.937020	 time= 4.9227e-05	 cumTime= 0.000920688	 edges= 100	 schur= 0	 lambda= 1560224013148.205566	 levenbergIter= 3
solve time cost = 0.00200526 seconds. 
estimated model: 0.890912   2.1719 0.943629