１、背景知识介绍

优化的目的是为了通过当前已知的系统理想化的模型和实际测量的数据获取最近接真实值的系统结果。这样的定义让人很容易联想起来各种滤波方法的目的，的确滤波方法和图优化方案解决的问题都是对不可靠的测量值进行处理以获取尽可能接近真实值的结果，例如以卡尔曼滤波器为例，在进行操作之前我们需要有一个相对靠谱的预测模型用来获取先验（预测）信息，以及实时的测量数据用来矫正预测信息。g2o图优化则是将优化问题和图论相结合，最典型的作用就是将待优化问题通过测量的数据建立最小二乘并将该最小二乘问题通过图论中的边的顶点表示出来，之后调用g2o库通过求解对应的图来实现对最小二乘问题的求解以达到优化的目的。其中图优化中的点表示待优化变量，如（ＸＹＺ）；边表示误差，边依赖于点的存在而存在，边可能和一个点、俩个点、多个点相连，每条边都表示与相连的点之间的误差。在slam问题中以相机位姿和观测到的路标做点，点与点之间存在的误差（重投影误差、相机位姿估计误差）做边，如下图所示。

２、代码详解

下面主要对g2o优化库的使用步骤做简要介绍，g2o的使用步骤主要分成以下四部分：

1. 点和边的类型定义；
2. 构建图优化实例，配置求解器；
3. 添加点和边，构建求解图；
4. 执行优化。

通过以下代码展示通过建立最小二乘求解模型exp^ax2+bx+c的参数ａｂｃ：

//Ｇ２Ｏ求解最小二乘
#include <iostream>
#include <g2o/core/g2o_core_api.h>
#include <g2o/core/base_vertex.h>
#include <g2o/core/base_unary_edge.h>
#include <g2o/core/block_solver.h>
#include <g2o/core/optimization_algorithm_levenberg.h>
#include <g2o/core/optimization_algorithm_gauss_newton.h>
#include <g2o/core/optimization_algorithm_dogleg.h>
#include <g2o/solvers/dense/linear_solver_dense.h>
#include <Eigen/Core>
#include <opencv2/core/core.hpp>
#include <cmath>using namespace std;
//第一部分；定义顶点和边的类型,先定义顶点后定义边，边是依赖于顶点存在的
// 曲线模型的顶点，模板参数：优化变量维度和数据类型
class CurveFittingVertex : public g2o::BaseVertex<3, Eigen::Vector3d> {
public://类成员变量如果是固定大小对象需要加上 EIGEN_MAKE_ALIGNED_OPERATOR_NEWEIGEN_MAKE_ALIGNED_OPERATOR_NEW// 重置；override 重载　virtual void setToOriginImpl() override{_estimate << 0, 0, 0;}// 更新virtual void oplusImpl(const double *update) override{_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_NEWEIGEN_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]);_jacobianOplusXi[0] = -_x * _x * y;_jacobianOplusXi[1] = -_x * y;_jacobianOplusXi[2] = -y;}//读写，暂时不用，留空virtual bool read(istream &in) {}virtual bool write(ostream &out) const {}
private:double _x;  // x 值， y 值为 _measurement
};int main(int argc, char **argv)
{//进行数据准备double areal = 1.0, breal = 2.0, creal = 1.0;         // 真实参数值double aest = 2.0, best = -1.0, cest = 5.0;        // 估计参数值int PointNum = 100;                                 // 数据点double w_sigma = 1.0;                        // 噪声Sigma值double inv_sigma = 1.0 / w_sigma;cv::RNG rng;                                 // OpenCV随机数产生器// 迭代数据存储vector<double> x_data, y_data;for (int i = 0; i < PointNum; i++) {double x = i / 100.0;x_data.push_back(x);y_data.push_back(exp(areal * x * x + breal * x + creal) + rng.gaussian(w_sigma * w_sigma));}// 第二部分；配置优化器，构建图优化// 定义重载求解变量块，每个误差项优化变量维度为3，误差值维度为1typedef g2o::BlockSolver<g2o::BlockSolverTraits<3, 1>> BlockSolverType;// 定义线性求解器类型typedef g2o::LinearSolverDense<BlockSolverType::PoseMatrixType> LinearSolverType;//创建高斯牛顿求解器auto solver = new g2o::OptimizationAlgorithmGaussNewton(g2o::make_unique<BlockSolverType>(g2o::make_unique<LinearSolverType>()));// 图模型//创建稀疏优化器g2o::SparseOptimizer optimizer;// 设置求解器optimizer.setAlgorithm(solver);// 打开调试输出optimizer.setVerbose(true);//　第三部分；添加点和边// 实例化顶点，往图中增加顶点CurveFittingVertex *v = new CurveFittingVertex();//配置初始估计值v->setEstimate(Eigen::Vector3d(aest, best, cest));//设置图表中节点的id确保更改id后图表保持一致v->setId(0);//添加设置完成的顶点optimizer.addVertex(v);// 往图中增加边for (int i = 0; i < PointNum; i++){//实例化边，并传入标点值CurveFittingEdge *edge = new CurveFittingEdge(x_data[i]);//设置图表中边的id确保更改id后图表保持一致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;//开始优化optimizer.initializeOptimization();//优化次数optimizer.optimize(10);// 输出优化值Eigen::Vector3d abc_estimate = v->estimate();cout << "真实值为: " << areal << " " << breal << " " << creal << endl;cout << "初始预测值为: " << aest << " " << best << " " << cest << endl;cout << "结果为: " << abc_estimate[0] << " " << abc_estimate[1] << " " << abc_estimate[2] << endl;return 0;
}

运行结果：
start optimization
iteration= 0 chi2= 376785.128234 time= 3.2139e-05 cumTime= 3.2139e-05 edges= 100 schur= 0
iteration= 1 chi2= 35673.566018 time= 1.8115e-05 cumTime= 5.0254e-05 edges= 100 schur= 0
iteration= 2 chi2= 2195.012304 time= 1.5005e-05 cumTime= 6.5259e-05 edges= 100 schur= 0
iteration= 3 chi2= 174.853126 time= 2.1757e-05 cumTime= 8.7016e-05 edges= 100 schur= 0
iteration= 4 chi2= 102.779695 time= 2.4055e-05 cumTime= 0.000111071 edges= 100 schur= 0
iteration= 5 chi2= 101.937194 time= 2.4749e-05 cumTime= 0.00013582 edges= 100 schur= 0
iteration= 6 chi2= 101.937020 time= 2.3813e-05 cumTime= 0.000159633 edges= 100 schur= 0
iteration= 7 chi2= 101.937020 time= 2.3953e-05 cumTime= 0.000183586 edges= 100 schur= 0
iteration= 8 chi2= 101.937020 time= 2.4606e-05 cumTime= 0.000208192 edges= 100 schur= 0
iteration= 9 chi2= 101.937020 time= 1.683e-05 cumTime= 0.000225022 edges= 100 schur= 0
真实值为: 1 2 1
初始预测值为: 2 -1 5
结果为: 0.890912 2.1719 0.943629

接下来我们将上述代码分为四部分进行详细介绍:

一、点和边的类型定义

点类型的定义

// 曲线模型的顶点，模板参数：优化变量维度和数据类型
class CurveFittingVertex : public g2o::BaseVertex<3, Eigen::Vector3d> {
public://类成员变量如果是固定大小对象需要加上 EIGEN_MAKE_ALIGNED_OPERATOR_NEWEIGEN_MAKE_ALIGNED_OPERATOR_NEW// 重置；override 重载　virtual void setToOriginImpl() override{_estimate << 0, 0, 0;}// 更新virtual void oplusImpl(const double *update) override{_estimate += Eigen::Vector3d(update);}// 存盘和读盘：留空，暂时不用virtual bool read(istream &in) {}virtual bool write(ostream &out) const {}
};

顶点类成员继承继承自BaseVertex，模板成员参数分别是１、待优化变量维度（３）；２、优化变量数据类型（Vector3d）。
顶点类包含四个重要的成员函数需要重载:

1. setToOriginImp()用于重置优化变量值。(改)
2. oplusImpl()用于对优化变量进行更新调整，是优化变量向真实值靠近。（改）
3. read和write分别是读盘、存盘函数，一般情况下不需要进行读/写操作的话，仅仅声明一下就可以.

边类型的定义

// 误差模型 模板参数：观测值维度，类型，连接顶点类型
class CurveFittingEdge : public g2o::BaseUnaryEdge<1, double, CurveFittingVertex> {
public://类成员变量如果是固定大小对象需要加上 EIGEN_MAKE_ALIGNED_OPERATOR_NEWEIGEN_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]);_jacobianOplusXi[0] = -_x * _x * y;_jacobianOplusXi[1] = -_x * y;_jacobianOplusXi[2] = -y;}//读写，暂时不用，留空virtual bool read(istream &in) {}virtual bool write(ostream &out) const {}
private:double _x;  // x 值， y 值为 _measurement
};

边类成员继承继承自BaseUnaryedge（不同类型的边有不同的基类，这里是一元边基类），模板成员参数分别是１、观测变量维度（１）；２、观测变量数据类型（Vector3d）；３、上述定义的顶点类型。
边类包含四个重要的成员函数需要重载:

1. virtual void　computeError();计算惨差，通过成员变量_vertices[0]和顶点编号０获取０号顶点变量；之后通过estimate()成员函数获取最近的优化变量；最后通过 _measurement中存储的观测变量与优化变量做参差计算并传给_error(0, 0)。
2. virtual void linearizeOplus();同样获取顶点并获取待优化的变量，最后通过数值求导获取雅克比梯度，注意这里需要添加负号，利用负梯度方向迭代。
3. read 、write留空。

二、构建图优化实例，配置求解器

 // 第二部分；配置优化器，构建图优化// 定义重载求解变量块，每个误差项优化变量维度为3，误差值维度为1typedef g2o::BlockSolver<g2o::BlockSolverTraits<3, 1>> BlockSolverType;// 定义线性求解器类型typedef g2o::LinearSolverDense<BlockSolverType::PoseMatrixType> LinearSolverType;//创建高斯牛顿求解器auto solver = new g2o::OptimizationAlgorithmGaussNewton(g2o::make_unique<BlockSolverType>(g2o::make_unique<LinearSolverType>()));// 图模型//创建稀疏优化器g2o::SparseOptimizer optimizer;// 设置求解器optimizer.setAlgorithm(solver);// 打开调试输出optimizer.setVerbose(true);

1. 定义变量块BlockSolverType，模板参数分别表示１、优化变量维度（３）２、参差变量维度（１）．
2. 定义线性求解器LinearSolverType。
3. 选取求解方法。
4. 创建并配置稀疏优化器。

三、添加点和边

//　第三部分；添加点和边// 实例化顶点，往图中增加顶点CurveFittingVertex *v = new CurveFittingVertex();//配置初始估计值v->setEstimate(Eigen::Vector3d(aest, best, cest));//设置图表中节点的id确保更改id后图表保持一致v->setId(0);//添加设置完成的顶点optimizer.addVertex(v);// 往图中增加边for (int i = 0; i < PointNum; i++){//实例化边，并传入标点值CurveFittingEdge *edge = new CurveFittingEdge(x_data[i]);//设置图表中边的id确保更改id后图表保持一致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. 实例化一个点ｖ；传入优化变量的初始估计值；给顶点配置编号；将顶点添加至图中。
2. 实例化边edge；给边配置编号；配置链接的顶点信息（顶点的编号，顶点的实例化）；传入观测值；配置信息矩阵；将边添加至图。

四、执行优化

// 第四部分；执行优化cout << "start optimization" << endl;//开始优化optimizer.initializeOptimization();//优化次数optimizer.optimize(10);// 输出优化值Eigen::Vector3d abc_estimate = v->estimate();

优化器初始化；配置优化次数（１０）；获取优化变量。

３、ax^2+bx+c实现

为了加深理解实现函数ax²+bx+c函数参数ａｂｃ的优化估计：

一、程序：

//Ｇ２Ｏ求解最小二乘
#include <iostream>
#include <g2o/core/g2o_core_api.h>
#include <g2o/core/base_vertex.h>
#include <g2o/core/base_unary_edge.h>
#include <g2o/core/block_solver.h>
#include <g2o/core/optimization_algorithm_levenberg.h>
#include <g2o/core/optimization_algorithm_gauss_newton.h>
#include <g2o/core/optimization_algorithm_dogleg.h>
#include <g2o/solvers/dense/linear_solver_dense.h>
#include <Eigen/Core>
#include <opencv2/core/core.hpp>
#include <cmath>using namespace std;
//第一部分；定义顶点和边的类型,先定义顶点后定义边，边是依赖于顶点存在的
// 曲线模型的顶点，模板参数：优化变量维度和数据类型
class CurveFittingVertex : public g2o::BaseVertex<3, Eigen::Vector3d> {
public://类成员变量如果是固定大小对象需要加上 EIGEN_MAKE_ALIGNED_OPERATOR_NEWEIGEN_MAKE_ALIGNED_OPERATOR_NEW// 重置；override 重载　virtual void setToOriginImpl() override{_estimate << 0, 0, 0;}// 更新virtual void oplusImpl(const double *update) override{_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_NEWEIGEN_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 - (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 = (abc[0] * _x * _x + abc[1] * _x + abc[2]);_jacobianOplusXi[0] = -_x * _x;_jacobianOplusXi[1] = -_x;_jacobianOplusXi[2] = -1;}//读写，暂时不用，留空virtual bool read(istream &in) {}virtual bool write(ostream &out) const {}private:double _x;  // x 值， y 值为 _measurement
};int main(int argc, char **argv)
{//进行数据准备double areal = 1.0, breal = 2.0, creal = 1.0;         // 真实参数值double aest = 2.0, best = -1.0, cest = 5.0;        // 估计参数值int PointNum = 100;                                 // 数据点double w_sigma = 1.0;                        // 噪声Sigma值double inv_sigma = 1.0 / w_sigma;cv::RNG rng;                                 // OpenCV随机数产生器// 迭代数据存储vector<double> x_data, y_data;for (int i = 0; i < PointNum; i++) {double x = i / 100.0;x_data.push_back(x);y_data.push_back((areal * x * x + breal * x + creal) + rng.gaussian(w_sigma * w_sigma));}// 第二部分: 构建图优化，配置求解器// 定义重载求解变量块，每个误差项优化变量维度为3，误差值维度为1typedef g2o::BlockSolver<g2o::BlockSolverTraits<3, 1>> BlockSolverType;// 定义线性求解器类型typedef g2o::LinearSolverDense<BlockSolverType::PoseMatrixType> LinearSolverType;//创建高斯牛顿求解器auto solver = new g2o::OptimizationAlgorithmGaussNewton(g2o::make_unique<BlockSolverType>(g2o::make_unique<LinearSolverType>()));// 图模型g2o::SparseOptimizer optimizer;// 设置求解器optimizer.setAlgorithm(solver);// 打开调试输出optimizer.setVerbose(true);//　第三部分: 添加点和边，构架求解图// 实例化顶点，往图中增加顶点CurveFittingVertex *v = new CurveFittingVertex();//配置初始估计值v->setEstimate(Eigen::Vector3d(aest, best, cest));//设置图表中节点的id确保更改id后图表保持一致v->setId(0);//添加设置完成的顶点optimizer.addVertex(v);// 往图中增加边for (int i = 0; i < PointNum; i++){//实例化边，并传入标点值CurveFittingEdge *edge = new CurveFittingEdge(x_data[i]);//设置图表中边的id确保更改id后图表保持一致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;//开始优化optimizer.initializeOptimization();//优化次数optimizer.optimize(10);// 输出优化值Eigen::Vector3d abc_estimate = v->estimate();cout << "真实值为: " << areal << " " << breal << " " << creal << endl;cout << "初始预测值为: " << aest << " " << best << " " << cest << endl;cout << "结果为: " << abc_estimate[0] << " " << abc_estimate[1] << " " << abc_estimate[2] << endl;return 0;
}

二、运行结果

start optimization
真实值为: 1 2 1
初始预测值为: 2 -1 5
结果为: 1.43205 2.18296 0.838242
iteration= 0 chi2= 102.023665 time= 2.8165e-05 cumTime= 2.8165e-05 edges= 100 schur= 0
iteration= 1 chi2= 102.023665 time= 2.1604e-05 cumTime= 4.9769e-05 edges= 100 schur= 0
iteration= 2 chi2= 102.023665 time= 1.0514e-05 cumTime= 6.0283e-05 edges= 100 schur= 0
iteration= 3 chi2= 102.023665 time= 1.8689e-05 cumTime= 7.8972e-05 edges= 100 schur= 0
iteration= 4 chi2= 102.023665 time= 1.6087e-05 cumTime= 9.5059e-05 edges= 100 schur= 0
iteration= 5 chi2= 102.023665 time= 1.5163e-05 cumTime= 0.000110222 edges= 100 schur= 0
iteration= 6 chi2= 102.023665 time= 6.993e-06 cumTime= 0.000117215 edges= 100 schur= 0
iteration= 7 chi2= 102.023665 time= 6.601e-06 cumTime= 0.000123816 edges= 100 schur= 0
iteration= 8 chi2= 102.023665 time= 6.617e-06 cumTime= 0.000130433 edges= 100 schur= 0
iteration= 9 chi2= 102.023665 time= 6.228e-06 cumTime= 0.000136661 edges= 100 schur= 0

Process finished with exit code 0