前言
一般来说定义 Vertex 需要重写这几个函数
// 读盘、存盘函数,不需要进行读/写操作的话,仅仅声明一下就可以
virtual bool read(std::istream& is);
virtual bool write(std::ostream& os) const;
// 设置被优化顶点的初始值
virtual void setToOriginImpl();
// 顶点更新函数,用于优化过程中增量 的计算
virtual void oplusImpl(const number_t* update);
一、VertexSE2 : public BaseVertex<3, SE2>
//***g2o源码 g2o/g2o/types/slam2d/vertex_se2.h ***//
// 2D 位姿顶点, (x,y,theta)
class G2O_TYPES_SLAM2D_API VertexSE2 : public BaseVertex<3, SE2>
二、VertexSE3 : public BaseVertex<6, Isometry3>
//***g2o源码 g2o/g2o/types/slam3d/vertex_se3.h ***//
// 欧式变换矩阵(4x4),状态由 6d 向量表示(x,y,z,qx,qy,qz),省略了四元数的w部分。
class G2O_TYPES_SLAM3D_API VertexSE3 : public BaseVertex<6, Isometry3>
三、VertexPointXY : public BaseVertex<2, Vector2>
//***g2o源码 g2o/g2o/types/slam2d/vertex_point_xy.h ***//
// 二维点
class G2O_TYPES_SLAM2D_API VertexPointXY : public BaseVertex<2, Vector2>
四、VertexPointXYZ : public BaseVertex<3, Vector3>
//*** g2o源码 g2o/g2o/types/slam3d/vertex_pointxyz.h ***//
// 三维空间中被跟踪的点
class G2O_TYPES_SLAM3D_API VertexPointXYZ : public BaseVertex<3, Vector3>
五、VertexSBAPointXYZ : public BaseVertex<3, Vector3>
//*** g2o源码 g2o/g2o/types/sba/types_sba.h ***//
// 三维空间中的点,主要用于 Bundle Adjustment,BA
class G2O_TYPES_SBA_API VertexSBAPointXYZ : public BaseVertex<3, Vector3>
六、VertexSE3Expmap : public BaseVertex<6, SE3Quat>
//*** g2o源码 g2o/g2o/types/sba/types_six_dof_expmap.h ***//
// SE(3)顶点,内部用变换矩阵参数化,外部用指数映射参数化
#ifndef G2O_SBA_VERTEXSE3EXPMAP_H
#define G2O_SBA_VERTEXSE3EXPMAP_H
#include "g2o/core/base_vertex.h"
#include "g2o/types/slam3d/se3quat.h"
#include "g2o_types_sba_api.h"
namespace g2o {
/**
* \brief SE3 Vertex parameterized internally with a transformation matrix
* and externally with its exponential map
*/
class G2O_TYPES_SBA_API VertexSE3Expmap : public BaseVertex<6, SE3Quat> {
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
VertexSE3Expmap();
bool read(std::istream& is);
bool write(std::ostream& os) const;
void setToOriginImpl();
void oplusImpl(const number_t* update_);
};
} // namespace g2o
#endif
//*** g2o源码 g2o/g2o/types/sba/types_six_dof_expmap.cpp ***//
#include "vertex_se3_expmap.h"
#include "g2o/stuff/misc.h"
namespace g2o {
VertexSE3Expmap::VertexSE3Expmap() : BaseVertex<6, SE3Quat>() {}
bool VertexSE3Expmap::read(std::istream& is) {
Vector7 est;
internal::readVector(is, est);
setEstimate(SE3Quat(est).inverse());
return true;
}
bool VertexSE3Expmap::write(std::ostream& os) const {
return internal::writeVector(os, estimate().inverse().toVector());
}
void VertexSE3Expmap::setToOriginImpl() { _estimate = SE3Quat(); }
void VertexSE3Expmap::oplusImpl(const number_t* update_) {
Eigen::Map<const Vector6> update(update_);
setEstimate(SE3Quat::exp(update) * estimate());
}
} // namespace g2o
七、VertexCam : public BaseVertex<6, SBACam>
//*** g2o源码 g2o/g2o/g2o/types/sba/vertex_cam.h ***//
// SBACam 顶点,(x,y,z,qw,qx,qy,qz)
// 状态由 6d 向量表示(x,y,z,qx,qy,qz),省略了四元数的w部分。qw 假设为正值,以避免在表示旋转时的歧义。
class G2O_TYPES_SBA_API VertexCam : public BaseVertex<6, SBACam>
八、VertexSim3Expmap : public BaseVertex<7, Sim3>
//*** g2o源码 g2o/g2o/types/sim3/types_seven_dof_expmap.h ***//
// Sim(3)顶点,(x,y,z,qw,qx,qy,qz)
// 状态由 7d 向量表示(x,y,z,qx,qy,qz),省略了四元数的w部分。qw 假设为正值,以避免在表示旋转时的歧义
// 表示两个相机之间的相对变换
class G2O_TYPES_SIM3_API VertexSim3Expmap : public BaseVertex<7, Sim3>