G2o的顶点与边属于 HyperGraph 抽象类的继承OptimizableGraph 的继承。
BaseVertex<D,T> D是预测点的维度(在流形空间的最小表示)T是待估计vertex的数据类型,比如用四元数表达三维旋转的话,T就是Quaternion 类型
// 顶点的定义
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); // 输入有数据返回True
virtual bool write(std::ostream& os) const;
virtual void oplusImpl(const number_t* update);
virtual void setToOriginImpl();
-
read,write:分别是读盘、存盘函数,一般情况下不需要进行读/写操作的话,仅仅声明一下就可以
-
setToOriginImpl:顶点重置函数,设定被优化变量的原始值。
-
oplusImpl:顶点更新函数。非常重要的一个函数,主要用于优化过程中增量△x 的计算。我们根据增量方程计算出增量之后,就是通过这个函数对估计值进行调整的,因此这个函数的内容一定要重视。
// 顶点类型是Eigen::Vector3d,属于向量,是可以通过加法来更新
class CurveFittingVertex: public g2o::BaseVertex<3, Eigen::Vector3d>
{
public:EIGEN_MAKE_ALIGNED_OPERATOR_NEWvirtual void setToOriginImpl() // 重置{_estimate << 0,0,0;}virtual void oplusImpl( const double* update ) // 更新{_estimate += Eigen::Vector3d(update);}// 存盘和读盘:留空virtual bool read( istream& in ) {}virtual bool write( ostream& out ) const {}
};
/**\* \brief SE3 Vertex parameterized internally with a transformation matrixand externally with its exponential map*/class G2O_TYPES_SBA_API VertexSE3Expmap : public BaseVertex<6, SE3Quat>{
public:EIGEN_MAKE_ALIGNED_OPERATOR_NEWVertexSE3Expmap();bool read(std::istream& is);bool write(std::ostream& os) const;virtual void setToOriginImpl() {_estimate = SE3Quat();}virtual void oplusImpl(const number_t* update_) {Eigen::Map<const Vector6> update(update_);setEstimate(SE3Quat::exp(update)*estimate()); //更新方式}
};
SE3Quat是相机位姿类型,它内部使用了四元数表达旋转,然后加上位移来存储位姿,同时支持李代数上的运算,比如对数映射(log函数)、李代数上增量(update函数)等操作,相机位姿顶点类VertexSE3Expmap使用了李代数表示相机位姿,这是因为旋转矩阵是有约束的矩阵,它必须是正交矩阵且行列式为1。使用它作为优化变量就会引入额外的约束条件,从而增大优化的复杂度。而将旋转矩阵通过李群-李代数之间的转换关系转换为李代数表示(使用罗德里格斯公式),就可以把位姿估计变成无约束的优化问题,求解难度降低。
添加顶点
CurveFittingVertex* v = new CurveFittingVertex();
v->setEstimate( Eigen::Vector3d(0,0,0) );
v->setId(0);
optimizer.addVertex( v );
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 );}