文章目录
g2o中的边
不用管他是几元边,其实本质上都是误差项。
而几元边就是对几个变量也就是所谓顶点求导数、下降方向罢了。
g2o中包含三种形式边
- 单元边
BaseUnaryEdge
- 二元边
BaseBinaryEdge
- 多元边
BaseMultiEdge
,BaseVariableSizedEdge
说明一些含义啊
- 假设
XXX
是任意变量类型,当然可以是自定义的结构体、类之类的都行,xxx
是XXX
类型的变量,当然不一定是同种or同一个。 - 假设
id
是顶点编号,当然不一定是同种or同一个。 - 假设
VVV
是顶点类型,当然不一定是同种or同一个。 - 假设
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);