史上最简SLAM零基础解读(10.4) - g2o(图优化)→示例代码讲解(slam十四讲第二版为例)

本人讲解关于slam一系列文章汇总链接:史上最全slam从零开始   文末正下方中心提供了本人 联系方式， 点击本人照片即可显示 W X → 官方认证 {\color{blue}{文末正下方中心}提供了本人 \color{red} 联系方式，\color{blue}点击本人照片即可显示WX→官方认证} 文末正下方中心提供了本人联系方式，点击本人照片即可显示WX→官方认证  

一、前言

示例代码为 https://github.com/gaoxiang12/slambook2/blob/master/ch6/g2oCurveFitting.cpp

在g2o简介的博客中，讲解了运行该示例代码环境的搭建，同时对 g2o 进行了简单的讲解。然后又对 顶点 ( V e r t e x ) \color{red}顶点 (Vertex) 顶点(Vertex) 与 边 ( E d g e ) \color{red}边(Edge) 边(Edge) 进行详细分析，总的来说使用 g2o 进行优化,编程求解主要包含五个步骤，来看看代码编程的流程：

( 1 ) \color{blue} (1) (1) 顶点和边的类型定义；

( 2 ) \color{blue} (2) (2) 构建图优化实例，配置求解器；

( 3 ) \color{blue} (3) (3) 添加点和边，构建求解图；

( 4 ) \color{blue} (4) (4) 执行优化。

后续，会把源码拆解成这四个部分，分别进行讲解。在这之前，先说一下示例代码的目的。虽然前面的博客已经把代码运行了起来，但是并没有讲具体实现是的功能是什么。假设一条满足以下方程的曲线: y = exp ⁡ ( a x 2 + b x + c ) + w (01) \color{green} \tag{01} y=\exp \left(a x^{2}+b x+c\right)+w y=exp(ax2+bx+c)+w(01)其中 a ， b ， c a，b，c a，b，c 为曲线的参数， w w w 为高斯噪声，满足 w ∼ ( 0 , σ 2 ) w \sim\left(0, \sigma^{2}\right) w∼(0,σ2)。我们故意选择了这样一个非线性模型，使问题不至于太简单。现在，假设我们有 N N N 个关于 x , y x,y x,y 的观测数据点，想根据这些数据点求出曲线的参数。那么，可以求解下面的最小二乘问题以估计曲线参数: min ⁡ a , b , c 1 2 ∑ i = 1 N ∥ y i − exp ⁡ ( a x i 2 + b x i + c ) ∥ 2 (02) \color{green} \tag{02} \min _{a, b, c} \frac{1}{2} \sum_{i=1}^{N}\left\|y_{i}-\exp \left(a x_{i}^{2}+b x_{i}+c\right)\right\|^{2} a,b,cmin​21​i=1∑N​∥ ∥​yi​−exp(axi2​+bxi​+c)∥ ∥​2(02)请注意，在这个问题中，待估计的变量是 a ， b ， c a，b，c a，b，c；而不是 x x x。我们的程序里先根据模型生成 x ， y x，y x，y 的真值，然后在真值中添加高斯分布的噪声。使用 g2o 从带噪声的数据拟合参数模型。定义误差为： e i = y i − exp ⁡ ( a x i 2 + b x i + c ) (03) \color{green} \tag{03} e_{i}=y_{i}-\exp \left(a x_{i}^{2}+b x_{i}+c\right) ei​=yi​−exp(axi2​+bxi​+c)(03)那么，可以求出每个误差项对于状态变量的导数: ∂ e i ∂ a = − x i 2 exp ⁡ ( a x i 2 + b x i + c ) ∂ e i ∂ b = − x i exp ⁡ ( a x i 2 + b x i + c ) ∂ e i ∂ c = − exp ⁡ ( a x i 2 + b x i + c ) (04) \color{green} \tag{04} \begin{array}{l} \frac{\partial e_{i}}{\partial a}=-x_{i}^{2} \exp \left(a x_{i}^{2}+b x_{i}+c\right) \\ \frac{\partial e_{i}}{\partial b}=-x_{i} \exp \left(a x_{i}^{2}+b x_{i}+c\right) \\ \frac{\partial e_{i}}{\partial c}=-\exp \left(a x_{i}^{2}+b x_{i}+c\right) \end{array} ∂a∂ei​​=−xi2​exp(axi2​+bxi​+c)∂b∂ei​​=−xi​exp(axi2​+bxi​+c)∂c∂ei​​=−exp(axi2​+bxi​+c)​(04) 下面就从每个步骤来分析吧，这里就不在粘贴整体的源码了，前面链接已经给出。如果对Jacobian matrix(雅可比矩阵)不太熟悉的朋友，建议先阅读下面这篇博客。

推荐 \color{red}推荐 推荐：史上最简SLAM零基础解读(7) - Jacobian matrix(雅可比矩阵) → 理论分析与应用详解  

二、数据模拟

首先需要去模拟一些数据，假设(01)式中真实的 a b c _ r = { a = 1.0 ， b = 2.0 ， b = 1.0 } abc\_r=\{a=1.0，b=2.0，b=1.0\} abc_r={a=1.0，b=2.0，b=1.0}，需要优化的初始值设定为 a b c _ e = { a = 2.0 ， b = − 1.0 ， c = 5.0 } abc\_e=\{a = 2.0，b = -1.0，c= 5.0\} abc_e={a=2.0，b=−1.0，c=5.0}。 也就是说，通过优化之后，使得 a b c _ e abc\_e abc_e 接近于 a b c _ r abc\_r abc_r。现在利用 a b c _ r abc\_r abc_r 随机生成 N = 100 N=100 N=100 组数据(同时添加了噪声)。代码如下：

  double ar = 1.0, br = 2.0, cr = 1.0;         // 真实参数值
  double ae = 2.0, be = -1.0, ce = 5.0;        // 估计参数值
  int N = 100;                                 // 数据点
  double w_sigma = 1.0;                        // 噪声Sigma值
  double inv_sigma = 1.0 / w_sigma;
  cv::RNG rng;                                 // OpenCV随机数产生器

  vector x_data, y_data;      // 数据
  for (int i = 0; i setEstimate(Eigen::Vector3d(aest, best, cest));
    //设置图表中节点的id确保更改id后图表保持一致
    v->setId(0);
    //添加设置完成的顶点
    optimizer.addVertex(v);

    // 往图中增加边
    for (int i = 0; i setId(i);
        // 设置连接的顶点，１、顶点编号２、顶点实例化
        edge->setVertex(0, v);
        // 传入观测到的数值
        edge->setMeasurement(y_data[i]);
        // 设置信息矩阵：协方差矩阵之逆
        edge->setInformation(Eigen::Matrix::Identity() * 1 / (w_sigma * w_sigma));
        //将设置完成的边加入
        optimizer.addEdge(edge);
    }

( 1 ) \color{blue} (1) (1) 实例化一个点ｖ；传入优化变量的初始估计值；给顶点配置编号；将顶点添加至图中。 ( 2 ) \color{blue} (2) (2) 实例化边edge；给边配置编号；配置链接的顶点信息（顶点的编号，顶点的实例化）；传入观测值；配置信息矩阵；将边添加至图。

六、添加点和边

执行优化与输出：

  // 执行优化
  cout