g2o的顶点(Vertex) 从哪里来的?
vertex有关的第①个类: HyperGraph::Vertex,它在这个路径
g2o/core/hyper_graph.h
这个 HyperGraph::Vertex 是个abstract vertex,必须通过派生来使用。
第②个类:HyperGraph::Vertex 是通过类OptimizableGraph 来继承的, 而OptimizableGraph的定义在
g2o/core/optimizable_graph.h
我们找到vertex定义,发现果然,OptimizableGraph 继承自 HyperGraph
第③个类:**BaseVertex<D, T>**那么它在哪里呢? 在这个路径:
g2o/core/base_vertex.h
g2o的顶点(Vertex) 参数如何理解?
//模板参数 D 和 T
static const int Dimension = D; ///< dimension of the estimate (minimal) in the manifold space
typedef T EstimateType;
EstimateType _estimate;
D并非是顶点(更确切的说是状态变量)的维度,而是其在流形空间(manifold)的最小表示
T就是顶点(状态变量)的类型
如何自己定义顶点
因为在不同的应用场景(二维空间,三维空间),有不同的待优化变量(位姿,空间点),还涉及不同的优化类型(李代数位姿、李群位姿)
g2o本身内部定义了一些常用的顶点类型
VertexSE2 : public BaseVertex<3, SE2> //2D pose Vertex, (x,y,theta)
VertexSE3 : public BaseVertex<6, Isometry3> //6d vector (x,y,z,qx,qy,qz) (note that we leave out the w part of the quaternion)
VertexPointXY : public BaseVertex<2, Vector2>
VertexPointXYZ : public BaseVertex<3, Vector3>
VertexSBAPointXYZ : public BaseVertex<3, Vector3>
// SE3 Vertex parameterized internally with a transformation matrix and externally with its exponential map
VertexSE3Expmap : public BaseVertex<6, SE3Quat>
// SBACam Vertex, (x,y,z,qw,qx,qy,qz),(x,y,z,qx,qy,qz) (note that we leave out the w part of the quaternion.
// qw is assumed to be positive, otherwise there is an ambiguity in qx,qy,qz as a rotation
VertexCam : public BaseVertex<6, SBACam>
// Sim3 Vertex, (x,y,z,qw,qx,qy,qz),7d vector,(x,y,z,qx,qy,qz) (note that we leave out the w part of the quaternion.
VertexSim3Expmap : public BaseVertex<7, Sim3>
重新定义顶点一般需要考虑重写如下函数:
virtual bool read(std::istream& is);
virtual bool write(std::ostream& os) const;
virtual void oplusImpl(const number_t* update);
virtual void setToOriginImpl();
read,write:分别是读盘、存盘函数,一般情况下不需要进行读/写操作的话,仅仅声明一下就可以
setToOriginImpl:顶点重置函数,设定被优化变量的原始值。
oplusImpl:顶点更新函数。非常重要的一个函数,主要用于优化过程中增量△x 的计算。我们根据增量方程计算出增量之后,就是通过这个函数对估计值进行调整的,因此这个函数的内容一定要重视。
自己定义 顶点一般是下面的格式:
class myVertex: public g2::BaseVertex<Dim, Type>
{
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
myVertex(){}
virtual void read(std::istream& is) {}
virtual void write(std::ostream& os) const {}
virtual void setOriginImpl()
{
_estimate = Type(); //举例:_estimate << 0,0,0;
}
virtual void oplusImpl(const double* update) override
{
_estimate += /*update*/; //**
}
}
**:因为顶点类型是Eigen::Vector3d,属于向量,是可以通过加法来更新的。但是但是有些例子就不行,比如下面这个复杂点例子:李代数表示位姿VertexSE3Expmap
virtual void setToOriginImpl() {
_estimate = SE3Quat();
}
virtual void oplusImpl(const number_t* update_) {
Eigen::Map<const Vector6> update(update_);
setEstimate(SE3Quat::exp(update)*estimate()); //更新方式
}
如何向图中添加顶点
- 初始化一个顶点
- 给该顶点添加ID:setId()
- 给该顶点添加估计值:setEstimate()
- 设置该顶点是否被边缘化:setMarginalized ()
- 增加该顶点到优化器:optimizer.addVertex ()
int index = 1;
for ( const Point3f p:points_3d ) // landmarks
{
g2o::VertexSBAPointXYZ* point = new g2o::VertexSBAPointXYZ();
point->setId ( index++ );
point->setEstimate ( Eigen::Vector3d ( p.x, p.y, p.z ) );
point->setMarginalized ( true );
optimizer.addVertex ( point );
}