g2o非线性优化

Posted 2022-12-11 chengwei0019

基于g2o的最小二乘方法。

g2o，即General Graph Optimization，他是一个基于图理论的优化库。图优化理论介绍，可以参考半闲居士的博客

有这么一个问题，给你一组二维数据，拟合其直线方程。

譬如下面的方程：

拟合y=Asin(Bx)+Ccos(Dx)y=Asin(Bx)+Ccos(Dx)，已知N组数据(xi,yi),i=0,1,⋯N−1(xi,yi),i=0,1,⋯N−1，待优化变量V=[A,B,C,D]V=[A,B,C,D]

即为优化如下问题：

整个图中，只有一个顶点，其待优化变量V（即参数A，B，C，D），连接只有一个顶点的边即一元边（Unary Edge），使用g2o优化步骤如下：

1. 定义顶点和边的类型（继承自g2o）
2. 选择优化算法
3. 构建图（添加顶点和边）
4. 优化，调用g2o优化，返回结果

顶点的定义：
    继承BaseVertex<定点byte数，顶点结构体>类
    实现setToRiginImpl()函数，里面给_estimate成员函数赋初值
    _estimate的类型就是模板里指定的顶点结构体
    实现：oplusImpl(const double * update_)。update_是指向跟新值得指针，使用前需要转换成真实的结    构类型。不一定是顶点类型。比如update_可以是李代数，但顶点类型是李群。
    可以使用esitimate()函数得到顶点的值，类型同上面的顶点结构体。
边的定义：
    继承BaseBinaryEdge<观察值byte数，观察值结构体，第一个顶点的类型，第二个顶点的类型>（两个顶点的边）
    或继承BaseUnaryEdge<观察值byte数，观察值结构体，顶点的类型>（一个顶点的边）
    实现函数computeError()，函数里面需要计算_error的值，其他顶点和测量值的成员变量参考其他例子。
    实现linearizeOplus()，单顶点的计算_jacobianOplusXi矩阵，双顶点的还要计算_jacobianOplusXj矩阵
SE3Quat
    使用map函数来变换一个3d点
SparseOptimizer
    这个应该是总的流程和数据管理器
    setAlgorithm设置具体计算更新值得算法
    addVertex加入顶点
    addEdge()
    addParameter()可以传入一些超参
    initializeOptimization()给顶点填入初始值
    optimize()启动优化流程
        调用所有边的linearizeOplus()函数，得到每个边的雅克比矩阵，然后把矩阵连接成一个大矩阵
        调用所有边的computeError()，得到误差矩阵
        基于雅克比和误差，使用优化器计算更新值deltaX（矩阵求逆，主要时间花费在这里）
        把deltaX拆分成各个顶点的值
        调用顶点的oplusImpl函数，更新顶点的_estimate变量
        回到第一步

---

1）定义顶点和边的类型

由于g2o没有本例中的顶点和边，故需要自己定义顶点和边的类型，其继承自BaseVertex和BaseEdge，可以参考相机位姿和3D点坐标常用的几种类型VertexSE3Expmap、VertexSBAPointXYZ、EdgeSE3ProjectXYZ、EdgeSE3ProjectXYZOnlyPose

定义方式1

class CurveFittingVertex :public g2o::BaseVertex<4, Vector4d>

public:
	EIGEN_MAKE_ALIGNED_OPERATOR_NEW

		CurveFittingVertex();
	virtual void setToOriginImpl()
	
		_estimate << 0, 0, 0, 0;
	
	virtual void oplusImpl(const double* update_);
	bool read(std::istream& is)  return 0; 
	bool write(std::ostream& os)const  return 0; 
;
CurveFittingVertex::CurveFittingVertex():BaseVertex<4,Eigen::Vector4d>()

void CurveFittingVertex::oplusImpl(const double* update_)

	Eigen::Map<const Vector4d> up(update_);
	_estimate += up;


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

public:
	EIGEN_MAKE_ALIGNED_OPERATOR_NEW

		CurveFittingEdge();
	void computeError();
	virtual void linearizeOplus();

	bool read(std::istream& is)  return 0; 
	bool write(std::ostream& os)const  return 0; 
public:
	double _x;
;
CurveFittingEdge::CurveFittingEdge():g2o::BaseUnaryEdge<1,double,CurveFittingVertex>()

void CurveFittingEdge::computeError()

	const CurveFittingVertex* v = static_cast<const CurveFittingVertex*>(_vertices[0]);
	const Vector4d abcd = v->estimate();
	_error(0, 0) = _measurement - (abcd[0] * sin(abcd[1] * _x) + abcd[2] * cos(abcd[3] * _x));

void CurveFittingEdge::linearizeOplus()

	CurveFittingVertex *vi = static_cast<CurveFittingVertex*>(_vertices[0]);
	Vector4d abcd = vi->estimate();
	_jacobianOplusXi(0, 0) = -sin(abcd[1] * _x);
	_jacobianOplusXi(0, 1) = -abcd[0] * _x*cos(abcd[1] * _x);
	_jacobianOplusXi(0, 2) = -cos(abcd[3] * _x);
	_jacobianOplusXi(0, 3) = abcd[2] * _x*sin(abcd[3] * _x);

定义方式2

class CurveFittingVertex :public g2o::BaseVertex<4, Vector4d>

public:
	EIGEN_MAKE_ALIGNED_OPERATOR_NEW
		CurveFittingVertex() 

	virtual void setToOriginImpl() 
	virtual bool read(std::istream& is)  return 0; 
	virtual bool write(std::ostream& os)const  return 0; 
	virtual void oplusImpl(const double* update)
	
		_estimate += Eigen::Vector4d::ConstMapType(update);
	
;

class CurveFittingEdge :public g2o::BaseUnaryEdge<2, Eigen::Vector2d, CurveFittingVertex>

public:
	EIGEN_MAKE_ALIGNED_OPERATOR_NEW

		CurveFittingEdge()

	bool read(std::istream& is)  return 0; 
	bool write(std::ostream& os)const  return 0; 

	void computeError() 
	
		const CurveFittingVertex* v = static_cast<const CurveFittingVertex*>(_vertices[0]);
		const Vector4d abcd = v->estimate();

		_error(0) = measurement()(1)- (abcd[0] * sin(abcd[1] * measurement()(0)) + abcd[2] * cos(abcd[3] * measurement()(0)));
	
	//virtual void linearizeOplus();
;

注意两种方式的区别，其中第二种方式中，我们没有定义计算雅克比的方法，这里会默认使用数值求导方法。如果我们能够推导出雅可比矩阵的解析形式并告诉优化库，就可以避免数值求导中的诸多问题

---

2）选择优化算法

	// 构建图优化，先设定g2o
	// 矩阵块：每个误差项优化变量维度为4 ，误差值维度为1
	typedef g2o::BlockSolver< g2o::BlockSolverTraits<4, 1> > Block;
	// 线性方程求解器：稠密的增量方程
	Block::LinearSolverType* linearSolver = new LinearSolverDense<Block::PoseMatrixType>();

	Block* solver_ptr = new Block(std::unique_ptr<Block::LinearSolverType>(linearSolver));    // 矩阵块求解器

	// 梯度下降方法，从GN, LM, DogLeg 中选
	g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg(std::unique_ptr<Block>(solver_ptr));
	// g2o::OptimizationAlgorithmGaussNewton* solver = new g2o::OptimizationAlgorithmGaussNewton( solver_ptr );
	// g2o::OptimizationAlgorithmDogleg* solver = new g2o::OptimizationAlgorithmDogleg( solver_ptr );

	g2o::SparseOptimizer optimizer;     // 图模型
	optimizer.setAlgorithm(solver);   // 设置求解器
	optimizer.setVerbose(true);     // 打开调试输出

---

3）构建图

	// 往图中增加顶点
	CurveFittingVertex *v = new CurveFittingVertex();
	// 设置优化初始估计值
	v->setEstimate(Eigen::Vector4d(1.6, 1.4, 6.2, 1.7));
	v->setId(0);
	v->setFixed(false);
	optimizer.addVertex(v);

	// 往图中增加边
	for (int i = 0; i < N; i++)
	
		CurveFittingEdge* edge = new CurveFittingEdge();
		edge->setId(i + 1);
		edge->setVertex(0, v);      // 设置连接的顶点
#ifdef TEST!
		edge->setMeasurement(y_data[i]);      // 观测数值
		edge->_x = x_data[i];
#else
		edge->setMeasurement(Eigen::Vector2d(x_data[i], y_data[i]));
#endif
		// 信息矩阵：协方差矩阵之逆
		edge->setInformation(Eigen::Matrix<double, 1, 1>::Identity() * 1 / (w_sigma* w_sigma));

		

		optimizer.addEdge(edge);
	

---

4）优化

optimizer.initializeOptimization();
	optimizer.optimize(100);

---

完整代码如下：

#include <iostream>
#include <g2o/core/base_vertex.h>// 顶点类型
#include <g2o/core/base_unary_edge.h>//一元边类型
#include <g2o/core/block_solver.h>//求解器的实现。主要来自choldmod, csparse。在使用g2o时要先选择其中一种。
#include <g2o/core/optimization_algorithm_levenberg.h>//莱文贝格－马夸特方法（Levenberg–Marquardt algorithm）能提供数非线性最小化（局部最小）的数值解。
#include <g2o/core/optimization_algorithm_gauss_newton.h>//高斯牛顿法
#include <g2o/core/optimization_algorithm_dogleg.h>//Dogleg（狗腿方法）
#include <g2o/solvers/dense/linear_solver_dense.h>
#include <Eigen/Core>//矩阵库
#include <opencv2/core/core.hpp>//opencv2
#include <cmath>//数学库
#include <Eigen/Core>
#include <Eigen/Dense>
#include <chrono>//时间库

using namespace std;
using namespace g2o;
using namespace Eigen;
#ifdef TEST!
class CurveFittingVertex :public g2o::BaseVertex<4, Vector4d>

public:
	EIGEN_MAKE_ALIGNED_OPERATOR_NEW

		CurveFittingVertex();
	virtual void setToOriginImpl()
	
		_estimate << 0, 0, 0, 0;
	
	virtual void oplusImpl(const double* update_);
	bool read(std::istream& is)  return 0; 
	bool write(std::ostream& os)const  return 0; 
;
CurveFittingVertex::CurveFittingVertex():BaseVertex<4,Eigen::Vector4d>()

void CurveFittingVertex::oplusImpl(const double* update_)

	Eigen::Map<const Vector4d> up(update_);
	_estimate += up;


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

public:
	EIGEN_MAKE_ALIGNED_OPERATOR_NEW

		CurveFittingEdge();
	void computeError();
	virtual void linearizeOplus();

	bool read(std::istream& is)  return 0; 
	bool write(std::ostream& os)const  return 0; 
public:
	double _x;
;
CurveFittingEdge::CurveFittingEdge():g2o::BaseUnaryEdge<1,double,CurveFittingVertex>()

void CurveFittingEdge::computeError()

	const CurveFittingVertex* v = static_cast<const CurveFittingVertex*>(_vertices[0]);
	const Vector4d abcd = v->estimate();
	_error(0, 0) = _measurement - (abcd[0] * sin(abcd[1] * _x) + abcd[2] * cos(abcd[3] * _x));

void CurveFittingEdge::linearizeOplus()

	CurveFittingVertex *vi = static_cast<CurveFittingVertex*>(_vertices[0]);
	Vector4d abcd = vi->estimate();
	_jacobianOplusXi(0, 0) = -sin(abcd[1] * _x);
	_jacobianOplusXi(0, 1) = -abcd[0] * _x*cos(abcd[1] * _x);
	_jacobianOplusXi(0, 2) = -cos(abcd[3] * _x);
	_jacobianOplusXi(0, 3) = abcd[2] * _x*sin(abcd[3] * _x);

#else
class CurveFittingVertex :public g2o::BaseVertex<4, Vector4d>

public:
	EIGEN_MAKE_ALIGNED_OPERATOR_NEW
		CurveFittingVertex() 

	virtual void setToOriginImpl() 
	virtual bool read(std::istream& is)  return 0; 
	virtual bool write(std::ostream& os)const  return 0; 
	virtual void oplusImpl(const double* update)
	
		_estimate += Eigen::Vector4d::ConstMapType(update);
	
;

class CurveFittingEdge :public g2o::BaseUnaryEdge<2, Eigen::Vector2d, CurveFittingVertex>

public:
	EIGEN_MAKE_ALIGNED_OPERATOR_NEW

		CurveFittingEdge()

	bool read(std::istream& is)  return 0; 
	bool write(std::ostream& os)const  return 0; 

	void computeError() 
	
		const CurveFittingVertex* v = static_cast<const CurveFittingVertex*>(_vertices[0]);
		const Vector4d abcd = v->estimate();

		_error(0) = measurement()(1)- (abcd[0] * sin(abcd[1] * measurement()(0)) + abcd[2] * cos(abcd[3] * measurement()(0)));
	
	//virtual void linearizeOplus();
;
#endif // TEST!
int main()

	double a = 5.0, b = 1.0, c = 10.0, d = 2.0; // 真实参数值
	int N = 100;
	double w_sigma = 2.0;   // 噪声值Sigma
	cv::RNG rng;    // 随机数产生器OpenCV
	double abcd[4] =  0, 0, 0, 0 ;  // 参数的估计值abc

	vector<double> x_data, y_data;

	cout << "generate random data" << endl;

	for (int i = 0; i < N; i++)
	
		//generate a random variable [-10 10]
		double x = rng.uniform(-10., 10.);
		double y = a * sin(b*x) + c * cos(d *x) + rng.gaussian(w_sigma);
		x_data.push_back(x);
		y_data.push_back(y);

		cout << x_data[i] << " , " << y_data[i] << endl;
	

	// 构建图优化，先设定g2o
	// 矩阵块：每个误差项优化变量维度为4 ，误差值维度为1
	typedef g2o::BlockSolver< g2o::BlockSolverTraits<4, 1> > Block;
	// 线性方程求解器：稠密的增量方程
	Block::LinearSolverType* linearSolver = new LinearSolverDense<Block::PoseMatrixType>();

	Block* solver_ptr = new Block(std::unique_ptr<Block::LinearSolverType>(linearSolver));    // 矩阵块求解器

	// 梯度下降方法，从GN, LM, DogLeg 中选
	g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg(std::unique_ptr<Block>(solver_ptr));
	// g2o::OptimizationAlgorithmGaussNewton* solver = new g2o::OptimizationAlgorithmGaussNewton( solver_ptr );
	// g2o::OptimizationAlgorithmDogleg* solver = new g2o::OptimizationAlgorithmDogleg( solver_ptr );

	g2o::SparseOptimizer optimizer;     // 图模型
	optimizer.setAlgorithm(solver);   // 设置求解器
	optimizer.setVerbose(true);     // 打开调试输出

	// 往图中增加顶点
	CurveFittingVertex *v = new CurveFittingVertex();
	// 设置优化初始估计值
	v->setEstimate(Eigen::Vector4d(1.6, 1.4, 6.2, 1.7));
	v->setId(0);
	v->setFixed(false);
	optimizer.addVertex(v);

	// 往图中增加边
	for (int i = 0; i < N; i++)
	
		CurveFittingEdge* edge = new CurveFittingEdge();
		edge->setId(i + 1);
		edge->setVertex(0, v);      // 设置连接的顶点
#ifdef TEST!
		edge->setMeasurement(y_data[i]);      // 观测数值
		edge->_x = x_data[i];
#else
		edge->setMeasurement(Eigen::Vector2d(x_data[i], y_data[i]));
#endif
		// 信息矩阵：协方差矩阵之逆
		edge->setInformation(Eigen::Matrix<double, 1, 1>::Identity() * 1 / (w_sigma* w_sigma));

		

		optimizer.addEdge(edge);
	

	// 执行优化
	cout << "strat optimization" << endl;

	chrono::steady_clock::time_point t1 = chrono::steady_clock::now();

	optimizer.initializeOptimization();
	optimizer.optimize(100);

	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::Vector4d abcd_estimate = v->estimate();
	cout << "estimated module: " << endl << abcd_estimate << endl;

	system("pause");
	return 0;

参考：

https://blog.csdn.net/stihy/article/details/55254756

https://github.com/tiger20/g2o_surface_fit

https://blog.csdn.net/ziliwangmoe/article/details/81460392