1. Introduction

图优化，是把优化问题表现成图的一种方式。这里的图是图论意义上的图。一个图由若干个顶点（Vertex），以及连接着这些顶点的边（Edge）组成。进而，用顶点表示优化变量，用边表示误差项。

在这里插入图片描述上图中给出了一个简单的图优化的例子：

图优化的顶点
三角形----相机的位姿节点；
圆形----路标点；
图优化的边
实线----相机的运动模型
虚线----相机的观测模型

为了使用 g2o，首先要将拟合问题抽象成图优化。节点为优化变量，边为误差项。在 g2o 中，将边称做超边(Hyper Edge)，而整个图被叫做超图(Hyper Graph)。

g2o 主要包含以下步骤：

定义顶点和边的类型
构建图
选择优化算法
调用 g2o 进行优化，返回结果

2. 示例：优化 exp(ax^2+bx+c)

现想要拟合曲线 $e^{ax^2+bx+c}$ ，将曲线拟合问题抽象成图优化。在该曲线拟合问题中，整个问题只有一个顶点（有且仅需优化 $a, b, c$ ）:曲线模型的参数是 $a, b, c$ ；而各个带噪声的数据点，构成了一个个误差项，也就是图优化的边。由图片可知，这里是一个一元边(Unary Edge)----整个图中只有一个顶点(因为在每一组数据中，数据本身并没有发生变化，仅仅是被施加了不同噪声的相同数据集，因为都是同一个顶点)。有多少组观测数据就有多少图优化的边？
在这里插入图片描述

Step 1: 构建图优化，并选择优化算法

typedef g2o::BlockSolver<g2o::BlockSolverTraits<3, 1>> BlockSolverType;  // 定义每个误差项优化变量维度为3，误差值维度为1
typedef g2o::LinearSolverDense<BlockSolverType::PoseMatrixType> LinearSolverType; // 线性求解器类型

// 梯度下降方法，可以从GN, LM, DogLeg 中选
auto solver = new g2o::OptimizationAlgorithmGaussNewton(
 g2o::make_unique<BlockSolverType>(g2o::make_unique<LinearSolverType>()));
g2o::SparseOptimizer optimizer;     // 图模型
optimizer.setAlgorithm(solver);   // 设置求解器
optimizer.setVerbose(true);       // 打开调试输出

这里首先创建了一个线性求解器。BlockSolver 在 Hessian 块上执行求解器。由这里的 LinearSolverDense 可知，使用的是 Dense Cholesky 分解法。

这里的 OptimizationAlgorithmGaussNewton 表示求解器使用的是高斯牛顿来做非线性优化。SparseOptimizer 表示创建一个稀疏优化器。

Step 2: 派生用于曲线拟合的图优化顶点

// 曲线模型的顶点，模板参数：优化变量维度和数据类型
class CurveFittingVertex : public g2o::BaseVertex<3, Eigen::Vector3d> {
    
    
public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW // 在new一个对象时会总是返回一个对齐的指针

  // 重置
  virtual void setToOriginImpl() override {
    
    
    _estimate << 0, 0, 0;
  }

  // 更新
  virtual void oplusImpl(const double *update) override {
    
    
    _estimate += Eigen::Vector3d(update);
  }
};

1.这里 BaseVertex 的模板为 template <int D, typename T>，其中 D 表示顶点最小的维度；T 表示待估计 Vertex 类型；
2.setToOriginImpl为顶点的重置函数，用来设定待优化变量的初始值；
3.oplusImpl为顶点更新函数，主要用于在计算出增量 $\Delta x$ 后，用来更新 $x_{k+1}=x_k+\Delta x$ ；这里的顶点更新看起来只是很简单的加法而已，为什么 g2o 不帮我们完成呢？在曲线拟合过程中，由于优化变量(曲线参数)本身位于向量空间中，这个更新计算就只是简单的加法。但是当优化变量不在向量空间时，如 $x$ 是相机位姿（李代数表示位姿），它本身不一定有加法运算。这时，就需要重新定义增量如何加到现有的估计上的行为了。比如会使用左乘更新或右乘更新，而不是直接的加法。

Step 3: 派生用于曲线拟合的图优化边

// 误差模型 模板参数：观测值维度，类型，连接顶点类型
class CurveFittingEdge : public g2o::BaseUnaryEdge<1, double, CurveFittingVertex> {
    
    
public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW

  CurveFittingEdge(double x) : BaseUnaryEdge(), _x(x) {
    
    }

  // 计算曲线模型误差
  virtual void computeError() override {
    
    
    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 void linearizeOplus() override {
    
    
    const CurveFittingVertex *v = static_cast<const CurveFittingVertex *> (_vertices[0]);
    const Eigen::Vector3d abc = v->estimate();
    double y = exp(abc[0] * _x * _x + abc[1] * _x + abc[2]); // exp(ax^2+bx+c)
    _jacobianOplusXi[0] = -_x * _x * y;
    _jacobianOplusXi[1] = -_x * y;
    _jacobianOplusXi[2] = -y;
  }
  
public:
  double _x;  // x 值， y 值为 _measurement
};

1.computeError为边的误差计算函数。该函数需要取出边所连接的顶点的当前估计值(即更新后的 $\Delta x$ )，根据曲线模型，与它的观测值进行比较。这和最小二乘问题中的误差模型是一致的；

_error(0, 0) = _measurement - std::exp(abc(0, 0) * _x * _x + abc(1, 0) * _x + abc(2, 0));
// $error = y- e^{ax^2 - bx - c}$

2.linearizeOplus为边的雅克比计算函数。这个函数计算了每条边相对于顶点的雅克比。其中：

Step 4: 往图中添加顶点

CurveFittingVertex *v = new CurveFittingVertex();
v->setEstimate(Eigen::Vector3d(ae, be, ce));
v->setId(0);
optimizer.addVertex(v);

1.setEstimate用来设定初始值，需要先给定估计参数值，在该情境下估计的是 $a_e,b_e,c_e$ ;
2.setId定义了节点的编号；
3.定义好的顶点，需要将它添加到 SparseOptimizer 中，即:optimizer.addVertex(v)。

Step 5: 往图中添加边

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);
 }

1. $N$ 为观测数据点个数，这里设定的观测数据点为 100，则 $N = 100$ ;
2.每条边都需要设定一个 id；
3.void setVertex(size_t i, Vertex* v)将超边关联到第 $i$ 个顶点。

Step 6: 设置优化参数，执行优化

optimizer.initializeOptimization();
optimizer.optimize(10);

1.int optimize(int iterations, bool online = false)中iterations为迭代次数；

~~Step 7: 输出优化值~~

Eigen::Vector3d abc_estimate = v->estimate(); // 估计值读取方式
cout << "estimated model: " << abc_estimate.transpose() << endl;

g2o 曲线拟合

g2o 曲线拟合

1. Introduction

2. 示例：优化 exp(ax^2+bx+c)

猜你喜欢