简介

g2o是一个通用的图优化库，可以应用到任何能够表示成图优化的最小二乘问题。例如BA，icp，曲线拟合等。

但它不像Ceres一样有丰富详细的学习资料。

图优化，是把优化问题表现成图（Graph）（指图论中的图）的一种方式，一组优化变量和变量之间的误差项，使用图来图来描述，使它们之间的关系更加直观化；

• 三角形和圆圈，都是顶点，待优化的变量；
• 红色与蓝色的线，都是边，描述相连顶点优化变量关系，也就是误差项；

g2o使用步骤如下：

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

         先看上半部分。SparseOptimizer 是我们最终要维护的东东。它是一个Optimizable Graph，从而也是一个Hyper Graph。一个 SparseOptimizer 含有很多个顶点 （都继承自 Base Vertex）和很多个边（继承自 BaseUnaryEdge, BaseBinaryEdge或BaseMultiEdge）。这些 Base Vertex 和 Base Edge 都是抽象的基类，而实际用的顶点和边，都是它们的派生类。我们用 SparseOptimizer.addVertex 和 SparseOptimizer.addEdge 向一个图中添加顶点和边，最后调用 SparseOptimizer.optimize 完成优化。

　　在优化之前，需要指定我们用的求解器和迭代算法。从图中下半部分可以看到，一个 SparseOptimizer 拥有一个 Optimization Algorithm，继承自Gauss-Newton, Levernberg-Marquardt, Powell's dogleg 三者之一（我们常用的是GN或LM）。同时，这个 Optimization Algorithm 拥有一个Solver，它含有两个部分。一个是 SparseBlockMatrix ，用于计算稀疏的雅可比和海塞； 一个是用于计算迭代过程中最关键的一步

曲线拟合问题

1、问题描述

其中a,b,c为待估计的参数，w为噪声。在程序里利用模型生成x，y的数据，在给数据添加服从高斯分布的噪声。之后用ceres优化求解参数a,b,c。

2、g2o基本类

2.1、基本模板点与边 

Base Vertex  点

BaseUnaryEdge 边

BaseBinaryEdge 边

BaseMultiEdge 边

2.2、 g2o已经定义好的边和点

在types中已经提供了一些边和节点的定义，例如

相机位姿结点：g2o::VertexSE3Expmap，来自；

特征点空间坐标结点：g2o::VertexSBAPointXYZ，来自；

 重投影误差：g2o::EdgeProjectXYZ2UV，来自；

3、代码实现讲解

3.1、自定义顶点

// 曲线模型的顶点，模板参数：优化变量维度和数据类型
class CurveFittingVertex : public g2o::BaseVertex<3, Eigen::Vector3d> {
public:EIGEN_MAKE_ALIGNED_OPERATOR_NEW// 重置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为g2o的基本节点类，需要继承从重写他内部的函数从而形成我们想要的新的顶点。

1、class CurveFittingVertex : public g2o::BaseVertex<3, Eigen::Vector3d>创建自己的节点

后边两个参数分别代表优化参数的维度为3，优化参数的类型为Eigen::Vector3d

2、virtual void setToOriginImpl()设置初始值

给待估计的节点设置初始值，本例中将节点设置为(0,0,0)

3、virtual void oplusImpl(const double *update)设置更新方式

采用相加的方式更新

3.2、定义边

// 误差模型 模板参数：观测值维度，类型，连接顶点类型
class CurveFittingEdge : public g2o::BaseUnaryEdge<1, double, CurveFittingVertex> {
public:EIGEN_MAKE_ALIGNED_OPERATOR_NEWCurveFittingEdge(double x) : BaseUnaryEdge(), _x(x) {}// 计算曲线模型误差virtual void computeError() override {const CurveFittingVertex *v = static_cast (_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 (_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 {}public:double _x;  // x 值， y 值为 _measurement
};

 首先明确边代表误差项

1、定义自己的边

class CurveFittingEdge : public g2o::BaseUnaryEdge<1, double, CurveFittingVertex>

 因为只有一个节点所以继承一元边。边代表的误差如下：

所以维度为1，类型设为double，节点是 CurveFittingVertex

2、设置误差

virtual void computeError() override {
//定义节点const CurveFittingVertex *v = static_cast (_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));}

 _vertices[0] 对应 “class CurveFittingEdge : public g2o::BaseUnaryEdge<1, double, CurveFittingVertex>”绑定的第一个顶点 CurveFittingVertex

变量abc即为优化参数

3、计算雅可比

virtual void linearizeOplus() override {const CurveFittingVertex *v = static_cast (_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;}

3.3、构建图

 auto solver = new g2o::OptimizationAlgorithmGaussNewton(g2o::make_unique(g2o::make_unique()));g2o::SparseOptimizer optimizer;     // 图模型optimizer.setAlgorithm(solver);   // 设置求解器optimizer.setVerbose(true);       // 打开调试输出// 往图中增加顶点CurveFittingVertex *v = new CurveFittingVertex();v->setEstimate(Eigen::Vector3d(ae, be, ce));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::Identity() * 1 / (w_sigma * w_sigma)); // 信息矩阵：协方差矩阵之逆optimizer.addEdge(edge);  // 添加}

1、求解器

typedef g2o::BlockSolver> BlockSolverType;
// 每个误差项优化变量维度为3，误差值维度为1；

typedef g2o::LinearSolverDense LinearSolverType;
// 线性求解器类型

g2o::OptimizationAlgorithmGaussNewton
// GN
2、增加节点

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

3、增加边

 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::Identity() * 1 / (w_sigma * w_sigma)); // 信息矩阵：协方差矩阵之逆optimizer.addEdge(edge);  // 添加}

3.4 、进行优化

		// 执行优化cout << "start optimization" << endl;chrono::steady_clock::time_point t1 = chrono::steady_clock::now();optimizer.initializeOptimization();optimizer.optimize(10);chrono::steady_clock::time_point t2 = chrono::steady_clock::now();chrono::duration time_used = chrono::duration_cast>(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;
}

3.5、完整代码

#include 
#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() 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_NEWCurveFittingEdge(double x) : BaseUnaryEdge(), _x(x) {}// 计算曲线模型误差virtual void computeError() override {const CurveFittingVertex *v = static_cast (_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 (_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 {}public:double _x;  // x 值， y 值为 _measurement
};int main(int argc, char **argv) {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 < N; i++) {double x = i / 100.0;x_data.push_back(x);y_data.push_back(exp(ar * x * x + br * x + cr) + rng.gaussian(w_sigma * w_sigma));}// 构建图优化，先设定g2otypedef g2o::BlockSolver> BlockSolverType;  // 每个误差项优化变量维度为3，误差值维度为1typedef g2o::LinearSolverDense LinearSolverType; // 线性求解器类型// 梯度下降方法，可以从GN, LM, DogLeg 中选auto solver = new g2o::OptimizationAlgorithmGaussNewton(g2o::make_unique(g2o::make_unique()));g2o::SparseOptimizer optimizer;     // 图模型optimizer.setAlgorithm(solver);   // 设置求解器optimizer.setVerbose(true);       // 打开调试输出// 往图中增加顶点CurveFittingVertex *v = new CurveFittingVertex();v->setEstimate(Eigen::Vector3d(ae, be, ce));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::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(10);chrono::steady_clock::time_point t2 = chrono::steady_clock::now();chrono::duration time_used = chrono::duration_cast>(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;
}

参考

slam 基本算法 --- 分别使用 【高斯牛顿，g2o】进行曲线拟合 （理论+实践）_CGJustDoIT的博客-CSDN博客

深入理解图优化与g2o：g2o篇 - 半闲居士 - 博客园

slam十四讲

本文来自互联网用户投稿，文章观点仅代表作者本人，不代表本站立场，不承担相关法律责任。如若转载，请注明出处。 如若内容造成侵权/违法违规/事实不符，请点击【内容举报】进行投诉反馈！