g2o中的边Edge

g2o中的边

不用管他是几元边，其实本质上都是误差项。
而几元边就是对几个变量也就是所谓顶点求导数、下降方向罢了。

g2o中包含三种形式边

1. 单元边BaseUnaryEdge
2. 二元边BaseBinaryEdge
3. 多元边 BaseMultiEdge,BaseVariableSizedEdge

说明一些含义啊

1. 假设XXX是任意变量类型,当然可以是自定义的结构体、类之类的都行，xxx是XXX类型的变量，当然不一定是同种or同一个。
2. 假设id是顶点编号，当然不一定是同种or同一个。
3. 假设VVV是顶点类型，当然不一定是同种or同一个。
4. 假设num误差项维度是一个具体的数值，1，2，3啥的，其实就是雅克比矩阵的行数

单元边BaseUnaryEdge

自定义单元边

class MyEdgeOne
    : public g2o::BaseUnaryEdge<num /*误差项维度*/,
                                XXX /*观测量数据类型,可以是自定义数据类型*/,
                                VVV0 /*i顶点类型*/>
{
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW//Eigen库的对齐宏啥的
    MyEdgeOne(): BaseBinaryEdge<num, XXX, VVV0>()
    {
    	//随便写点也行吧
    }

    virtual bool read(std::istream &) { return false; }//还没写过不会用
    virtual bool write(std::ostream &) const { return false; }//还没写过不会用

    void computeError()
    {
        _error = 自己算去;//计算误差
    }

    void linearizeOplus()
    {
         _jacobianOplusXi =自己算去 ; //对误差项求i顶点的导数
    }
		//按需要添加各种各样的数据类型与函数
		void setxxx(XXX xxx_)
		{
			xxx = xxx_;
		}
		XXX xxx;
		// 继承的类里面本身就有一些数据or函数主要是下面这几个
		// _error 误差
		// _measurement 观测
		// _jacobianOplusXi 雅克比矩阵
};

设置Edge的连接与观测等

MyEdgeOne *e = new MyEdgeOne();
e->setVertex(0, dynamic_cast<g2o::OptimizableGraph::Vertex *>(optimizer.vertex(id))); //设置i顶点
e->setMeasurement(xxx);//设置观测
e->setxxx(xxx);//自己定义的各种函数

设置完把边丢进去就行了

二元边BaseBinaryEdge

自定义二元边

class MyEdgeTwo
    : public g2o::BaseUnaryEdge<num /*误差项维度*/,
                                XXX /*观测量数据类型,可以是自定义数据类型*/,
                                VVV0 /*i顶点类型*/，
                                VVV1 /*j顶点类型*/>
{
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW//Eigen库的对齐宏啥的
    MyEdgeTwo(): BaseBinaryEdge<num, XXX, VVV0, VVV1>()
    {
    	//随便写点也行吧
    }

    virtual bool read(std::istream &) { return false; }//还没写过不会用
    virtual bool write(std::ostream &) const { return false; }//还没写过不会用

    void computeError()
    {
        _error = 自己算去;//计算误差
    }

    void linearizeOplus()
    {
         _jacobianOplusXi =自己算去 ; //对误差项求i顶点的导数
         _jacobianOplusXj =自己算去 ; //对误差项求j顶点的导数
    }
		//按需要添加各种各样的数据类型与函数
		void setxxx(XXX xxx_)
		{
			xxx = xxx_;
		}
		XXX xxx;
		// 继承的类里面本身就有一些数据or函数主要是下面这几个
		// _error 误差
		// _measurement 观测
		// _jacobianOplusXi 雅克比矩阵
		// _jacobianOplusXj 雅克比矩阵
};

设置Edge的连接与观测等

MyEdgeTwo *e = new MyEdgeTwo();
e->setVertex(0, dynamic_cast<g2o::OptimizableGraph::Vertex *>(optimizer.vertex(id))); //设置i顶点
e->setVertex(1, dynamic_cast<g2o::OptimizableGraph::Vertex *>(optimizer.vertex(id))); //设置j顶点
e->setId(id);//设置顶点id
e->setMeasurement(xxx);//设置观测
e->setxxx(xxx);//自己定义的各种函数

设置完把边丢进去就行了

多元边 BaseMultiEdge,BaseVariableSizedEdge

后面版本改名字了？！何必呢？？

template <int D, typename E>
using BaseMultiEdge = BaseVariableSizedEdge<D, E>;

自定义多元边

class MyEdgeHaoDuo
    : public g2o::BaseVariableSizedEdge<num /*误差项维度*/,
                                XXX /*观测量数据类型,可以是自定义数据类型*/>
{
 public:
  	EIGEN_MAKE_ALIGNED_OPERATOR_NEW//Eigen库的对齐宏啥的
  	MyEdgeHaoDuo()：g2o::BaseVariableSizedEdge<num /*误差项维度*/,
                                XXX /*观测量数据类型,可以是自定义数据类型*/>()
  	{
  		resize(EdgeNum/*顶点个数*/);
  	}

		void computeError()
    {
    		//取出顶点的用于计算呗
    		VVV0 *v0 = static_cast<VVV0 *>(_vertices[0]);
    		VVV1 *v1 = static_cast<VVV1 *>(_vertices[1]);
    		VVV2 *v2 = static_cast<VVV2 *>(_vertices[2]);
    		// ...好多个
    		//要加const也行
    		//const VVV1 *v1 = static_cast<VVV1 *>(_vertices[0]);
    		// ...
        _error = 自己算去;//计算误差
    }

    virtual bool read(std::istream &) { return false; }//还没写过不会用
    virtual bool write(std::ostream &) const { return false; }//还没写过不会用

    void linearizeOplus()
    {
    		_jacobianOplus[0]=自己算去;//计算对应VVV0的雅克比
    		_jacobianOplus[1]=自己算去;//计算对应VVV1的雅克比
    		_jacobianOplus[2]=自己算去;//计算对应VVV2的雅克比
    		// ...好多个
    }
    // 继承的类里面本身就有一些数据or函数主要是下面这几个
		// _error 误差
		// _vertices 存储雅克比矩阵的向量
};

设置Edge的连接与观测等

MyEdgeTwo *e = new MyEdgeTwo();
e->setVertex(0, dynamic_cast<g2o::OptimizableGraph::Vertex *>(optimizer.vertex(id))); //设置0顶点
e->setVertex(1, dynamic_cast<g2o::OptimizableGraph::Vertex *>(optimizer.vertex(id))); //设置1顶点
e->setVertex(2, dynamic_cast<g2o::OptimizableGraph::Vertex *>(optimizer.vertex(id))); //设置2顶点
//...好多顶点
e->setId(id);//设置顶点id
e->setMeasurement(xxx);//设置观测
e->setxxx(xxx);//自己定义的各种函数

将边Edge加入的优化器中

最后将边加入优化

 optimizer.addEdge(e);