说明
g2o(General Graphic Optimization,G2O)是一个基于图优化的库。
图优化,是把优化问题表现成图(Graph)的一种方式。一个图由若干个顶点(Vertex),以及连接着这些顶点的边(Edge)组成。进而,用顶点表示优化变量,用边表示误差项。于是,对任意一个上述形式的非线性最小二乘问题,可以构建与之对应的一个图。可以简单地称它为图,也可以用概率图里的定义,称之为贝叶斯图或因子图。
g2o 是一个通用的图优化库。“通用”意味着你可以在 g2o 里求解任何能够表示为图优化的最小二乘问题,显然包括曲线拟合问题。
使用
为了使用 g2o,首先要将曲线拟合问题抽象成图优化。这个过程中,只要记住节点为优化变量,边为误差项即可。
步骤:
- 定义顶点和边的类型。
- 构建图。
- 选择优化算法。
- 调用 g2o 进行优化,返回结果。
源代码
#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>
#include <chrono>
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_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 {}
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<double> 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));
}
// 构建图优化,先设定g2o
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); // 打开调试输出
// 往图中增加顶点
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<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(10);
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;
}
注:
在这个程序中,我们从 g2o 派生出了用于曲线拟合的图优化顶点和边:CurveFittingVertex 和CurveFittingEdge,这实质上扩展了g2o的使用方式。这两个类分别派生自BaseVertex和BaseUnaryEdge类。在派生类中,重写了重要的虚函数:
- 顶点的更新函数:oplusImpl。
优化过程最重要的是增量 ∆x 的计算,而该函数处理的是 xk+1 = xk + ∆x 的过程。
为什么 g2o 不帮我们完成呢?在曲线拟合过程中,由于优化变量(曲线参数)本身位于向量空间中,这个更新计算确实就是简单的加法。但是,当优化变量不在向量空间中时,比如说 x 是相机位姿,它本身不一定有加法运算。这时,就需要重新定义增量如何加到现有的估计上的行为了。按照第4讲的解释,我们可能使用左乘更新或右乘更新,而不是直接的加法。 - 顶点的重置函数:setToOriginImpl。
把估计值置零即可。 - 边的误差计算函数:computeError。
该函数需要取出边所连接的顶点的当前估计值,根据曲线模型,与它的观测值进行比较。这和最小二乘问题中的误差模型是一致的。 - 边的雅可比计算函数:linearizeOplus。
这个函数里我们计算了每条边相对于顶点的雅可比。 - 存盘和读盘函数:read、write。
由于并不想进行读/写操作,所以留空。
定义了顶点和边之后,我们在 main 函数里声明了一个图模型,然后按照生成的噪声数据,往图模型中添加顶点和边,最后调用优化函数进行优化。