g2o 中定义好的常用顶点类型

前言

一般来说定义 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>