g2o入门详解

本文为原创博客,转载请注明出处---https://blog.csdn.net/q_z_r_s

机器感知 一个专注于SLAM、三维重建、机器视觉等相关技术文章分享的公众号

---

开源地址：点击该链接

---

g2o用法总结

入门操作

• error = y - (x+1)^2 ,给出观测值y的情况下,求解出x的值,一个Vertex,一个Edge(BaseUnaryEdge)

class hello_vertex: public BaseVertex<1, Eigen::Matrix<double,1,1>> {
    //数据类型要以Eigen::Matrix给出,因为需要转置操作
    /**
     * update the position of the node from the parameters in v.
     * Implement in your class!
     */
	//optimizable_graph.h  
    virtual void oplusImpl(const double* v) {
		cout << "v: " << *v << endl;
		_estimate(0,0) += *v;				//_estimate的类型即为继承模板时传入的类型
		cout << "_estimate: " << _estimate(0,0) << endl;
	}
	
    //! sets the node to the origin (used in the multilevel stuff)
    //optimizable_graph.h
    virtual void setToOriginImpl() {
		cout << "setToOriginImpl " << _estimate(0,0)  << endl;
		_estimate(0,0) = 0;
	}
	
    //! read the vertex from a stream, i.e., the internal state of the vertex
    virtual bool read(std::istream& is) {}
    //! write the vertex to a stream
    virtual bool write(std::ostream& os) const {}
};

class hello_edge: public BaseUnaryEdge<1, double, hello_vertex> {
    // computes the error of the edge and stores it in an internal structure
    //optimizable_graph.h
    virtual void computeError() {
		const hello_vertex* v = static_cast<const hello_vertex*> (_vertices[0]);
		const Eigen::Matrix<double,1,1> abc = v->estimate();
		//typedef Eigen::Matrix<double, D, 1, Eigen::ColMajor> ErrorVector;
		//_error = abc - (Eigen::Matrix<double, 1, 1> )(10);
		cout << "computeError: " ;
		//_error(0,0) = _measurement - abc(0,0);
		//此处误差的定义影响后续解析求导时Jacob矩阵前边是否添加负号
		_error(0,0) = -_measurement + (abc(0,0) + 1)*(abc(0,0) + 1);
		cout << _error(0,0) << endl;
	}
	
	virtual void linearizeOplus() {
		const hello_vertex* v = static_cast<const hello_vertex*> (_vertices[0]);
		const Eigen::Matrix<double,1,1> abc = v->estimate();
		//需要提供负梯度,与Ceres相同
		//之所以是负梯度,因为err = _measurement - f(_estimate)
		//因此为负的
		_jacobianOplusXi(0,0) = 2 * (abc(0,0) + 1);
		cout << "_jacobianOplusXi: " << _jacobianOplusXi << endl;
	}
	
	//! read the vertex from a stream, i.e., the internal state of the vertex
        virtual bool read(std::istream& is) {}
        //! write the vertex to a stream
        virtual bool write(std::ostream& os) const {}
};

int main(int argc, char** argv)
{
    //建立矩阵块求解器BlockSolver;优化变量维度:1，误差维度为1
	typedef g2o::BlockSolver< g2o::BlockSolverTraits<1,1> > Block;  
    //线性方程求解器
	Block::LinearSolverType*linearSolver = new LinearSolverDense<Block::PoseMatrixType>(); 
	Block* solver_ptr = new Block(linearSolver);
	//选择梯度下降方法，GN, LM, DogLeg三选一
	g2o::OptimizationAlgorithmLevenberg* solver = 
        new g2o::OptimizationAlgorithmLevenberg(solver_ptr);
	//建立优化器
    SparseOptimizer optimizer;
	optimizer.setAlgorithm(solver);
    // 打开调试输出
	optimizer.setVerbose(true);  
	//加顶点
	hello_vertex* v = new hello_vertex();
    //设置估计值的初值,实际中发现,此值被setToOriginImpl()所覆盖了
	v->setEstimate((Eigen::Matrix<double,1,1>)100);
	v->setId(0);//Vertex独一无二的ID值,edge通过这个ID来与其建立联系
	optimizer.addVertex(v);
	//加边
	hello_edge* edge = new hello_edge;
	edge->setId(0);
	//第一个参数指定上一步添加Vertex时的索引,这样edge就可以通过这个索引值和Vertex联系起来了
	edge->setVertex(0, v);           // 设置连接的顶点
	edge->setMeasurement(66.6);      // 观测数值
    // 信息矩阵：协方差矩阵之逆
	edge->setInformation(Eigen::Matrix<double,1,1>::Identity()*0.25); 
	optimizer.addEdge(edge);
	// 执行优化前的初始化
	optimizer.initializeOptimization();
    //开始执行优化,并设置迭代次数
	optimizer.optimize(100);
	// 输出优化值
	Eigen::Matrix<double,1,1> est_estimate = v->estimate();
	cout<<"estimated value: "<<est_estimate.transpose()<<endl;		
	return 0;
}

Powell’s Function

//第一个参数:Vertex维度n;第二个参数:维度为n的Vertex存储的数据类型
class powell_vertex: public BaseVertex<4, Eigen::Matrix<double,1,4>> {
	virtual void oplusImpl(const double* v) {
        //base_vertex.h: EstimateType _estimate;
        //typedef T EstimateType,T即为传入的模板参数Eigen::Matrix<double,1,4>
        //定义顶点类型,即定义_estimate的类型
        cout << "oplus: "  << endl;
        cout << Eigen::Matrix<double,1,4>(v) << endl;
        _estimate += Eigen::Matrix<double,1,4>(v);
        cout << "_estimate: " << _estimate << endl;
	}
    virtual void setToOriginImpl() {
        cout << "initial _estimate" << endl;
        _estimate << 0,0,0,0;
	}
	
	virtual bool read(std::istream& is) {}
    virtual bool write(std::ostream& os) const {}	//const 不能忽略
};
//e(xi)的维度、以及此维度的边存储的数据类型; vertex的类型
class powell_edge: public BaseUnaryEdge<4, Eigen::Matrix<double,1,4>, powell_vertex> {
//错误的模板参数写法
//class powell_edge: public BaseUnaryEdge<4, double, powell_vertex> {
	virtual void computeError() {
		//vertex* vtx = new vertex;
		const powell_vertex* v = static_cast<const powell_vertex*> (_vertices[0]);
		Eigen::Matrix<double,1,4> est =  v->estimate();
		//cout << "est: " << est(0,0)  << "---" << est(0,1)  <<"---" 
        //<< est(0,2) <<  "---"<< est(0,3) << endl;
        //以下为错误写法,错误1:模板参数2应为Matrix<double,1,4>,而不是double
        //错误2:_error是列向量,怪不得来回检查程序总是找不到哪里写错了,难关编译正常
        //运行时出现:段错误(段错误很多原因是非法访问内存,尤其是数组类的数据超出边界访问)
        //追踪代码:typedef Eigen::Matrix<double, D, 1, Eigen::ColMajor> ErrorVector
        //ErrorVector _error;可见'_error'维度为:'D×1'维的列向量
        // 		_error(0,0) = (&_measurement)[0] - (est(0,0) + 10*est(0,1));
        // 		_error(0,1) = (&_measurement)[1] - sqrt(5)*(est(0,2) - est(0,3));
        // 		_error(0,2) = (&_measurement)[2] - (est(0,1) - 2*est(0,2))*(est(0,1) - 
        //						2*est(0,2));
        // 		_error(0,3) = (&_measurement)[3] - sqrt(10)*(est(0,0) - est(0,2))*
        //						(est(0,0) - est(0,2));
		_error(0,0) = _measurement(0,0)- (est(0,0) + 10*est(0,1));
		_error(1,0) = _measurement(0,1) - sqrt(5)*(est(0,2) - est(0,3));
		_error(2,0) = _measurement(0,2) - (est(0,1) - 2*est(0,2))*(est(0,1) - 2*est(0,2));
		_error(3,0) = _measurement(0,3) - sqrt(10)*(est(0,0) - est(0,2))*(est(0,0) - est(0,2));
		cout << "_error: " << _error.transpose() << endl;
	}
	
	virtual bool read(std::istream& is) {}
    virtual bool write(std::ostream& os) const {}	//const 不能忽略
};
//次边为:一条边多个维度,还有另一种做法就是一条边一个维度,即把D个误差项分开来写,结果相同,推荐此种写法

int main(int argc, char **argv) {
	//注意维度对应--边--顶点---int _PoseDim(vertex), int _LandmarkDim(edge)    
	typedef g2o::BlockSolver< g2o::BlockSolverTraits<4,4> > Block;  
	Block::LinearSolverType* linearSolver = 
        new g2o::LinearSolverDense<Block::PoseMatrixType>(); // 线性方程求解器
	Block* solver_ptr = new Block( linearSolver );// 矩阵块求解器
	// 梯度下降方法，从GN, LM, DogLeg 中选
	cout << "config blocksolver" << endl;
	g2o::OptimizationAlgorithmLevenberg* solver = 
        new g2o::OptimizationAlgorithmLevenberg( solver_ptr );
	g2o::SparseOptimizer optimizer;     // 图模型
	optimizer.setAlgorithm( solver );   // 设置求解器
	optimizer.setVerbose( true );       // 打开调试输出
	cout << "add Vertex" << endl;
	// 往图中增加顶点
	powell_vertex* v = new powell_vertex;
	Eigen::Matrix<double,1,4> estimate_initial;
	estimate_initial <<0.5,1.5,2.5,3.5;
	v->setEstimate(estimate_initial);
	v->setId(0);
	optimizer.addVertex( v );
	cout << "add Edge" << endl;
	//加边
	powell_edge* eg = new powell_edge;
	//double measurement_initial[4] = {21,-sqrt(5),16,9*sqrt(10)};
    //昨天没有找到真正错误时,试错的做法
	//for (int i=0;i<4;i++) {昨天没有找到真正错误时,试错的做法
    eg->setId(0);
    eg->setVertex( 0, v );                // 设置连接的顶点
    Eigen::Matrix<double,1,4> measurement_initial;
    measurement_initial << 21,-sqrt(5),16,9*sqrt(10);
    eg->setMeasurement(measurement_initial);      // 观测数值
    //这个'4'对应于powell_edge第一个参数
    eg->setInformation(Eigen::Matrix<double,4,4>::Identity()*1/0.25); 
    optimizer.addEdge( eg );
	//}昨天没有找到真正错误时,试错的做法
	// 执行优化
	cout<<"start optimization"<<endl;
	chrono::steady_clock::time_point t1 = chrono::steady_clock::now();
	cout<<"initial optimization"<<endl;
	optimizer.initializeOptimization();
	cout<<"start"<<endl;
	optimizer.optimize(200);
	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::Matrix<double,1,4> abc_estimate = v->estimate();
	cout<<"estimated value: "<<abc_estimate.transpose()<<endl;
    
    return 0;
}

总结

1. 定义顶点
2. 定义边
3. 配置BlockSolver(LinerSolverType)
4. 配置OptimizationAlgorithm
5. 配置Optimizer
6. 添加顶点
7. 添加边
8. 启动优化