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

滴滴哒哒答答

已于 2024-10-06 19:21:31 修改

阅读量283

点赞数 2

文章标签：机器人

于 2024-09-28 10:25:45 首次发布

本文链接：https://blog.csdn.net/m0_49384824/article/details/142611919

版权

前言

新版 g2o 的源码相对于旧版有所改动。总体来说，差别在 BlockSolver 的接口由裸指针变成了 unique_ptr、VertexSBAPointXYZ 类被删除等。具体可以看：SLAM十四讲ch7代码调整（undefined reference to symbol）。

//***g2o源码 g2o/g2o/core/base_vertex.h ***//
/**
 * \brief 模板化 BaseVertex
 *
 * D  : int 类型，表示vertex的最小维度，例如3D空间中旋转是3维的，则 D = 3
 * T  : 待估计 vertex 的数据类型，例如用四元数表达三维旋转时，T 为 Quaternion 类型
 */
template <int D, typename T>
class BaseVertex : public OptimizableGraph::Vertex {
    // 类的具体实现...
};

一般来说定义 Vertex 需要重写这几个函数

//*** g2o源码 g2o/g2o/core/optimizable_graph.h ***// 

// 读盘、存盘函数，不需要进行读/写操作的话，仅仅声明一下就可以
// 纯虚函数，从流中读取顶点，即顶点的内部状态
virtual bool read(std::istream& is) = 0;
// 纯虚函数，将顶点写入到流
virtual bool write(std::ostream& os) const = 0;

// 设置被优化顶点的初始值
virtual void setToOriginImpl() = 0;

// 顶点更新函数。计算出增量 △X 后，通过该函数对顶点(估计值)进行更新
// update 即 △X，其数据类型可以理解为是一个指向长度为 D（vertex的最小维度） 的数组的指针
virtual void oplusImpl(const number_t* update) = 0;

g2o 顶点模版

class myVertex: public g2o::BaseVertex<Dim, Type>{
  public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW

    // myVertex 类的构造函数，调用了基类 BaseVertex<Dim, Type> 的构造函数，初始化该顶点。
    myVertex() : BaseVertex<Dim, Type>() {}

    bool read(std::istream& is) override {
      // 从输入流读取顶点数据的实现
    }

    bool write(std::ostream& os) const override {
      // 将顶点数据写入输出流的实现
    }

    // _estimate 的数据类型为 EstimateType，即 BaseVertex 中的模版参数 T
    void setOriginImpl() override { 
        _estimate = Type();  // 将估计值重置为原点
    }

    void oplusImpl(const number_t* update) override {
        _estimate += /* 更新逻辑 */;
    }
  }

Eigen::Map<T> 是 Eigen 库中的一个类模板，它提供了一种将现有的原始数据（如指针或数组）映射为 Eigen 对象（如向量或矩阵）的方式，而不需要复制数据。这意味着可以对原始数据进行矩阵运算，同时保持与原始数据的关联。

// @param T: 想要映射得到的 Eigen 类型（如 Vector2d、MatrixXd 等）
// @param ptr：指向原始数据的指针
// @param mapped：经过映射后得到的 Eigen 类型数据
   Eigen::Map<const T> mapped(ptr);

g2o 中定义好的常用顶点类型汇总如下

// 2D二维点 Vertex，(x、y)
VertexPointXY : public BaseVertex<2, Vector2>
// 3D空间点 Vertex，(x、y、z)
VertexPointXYZ : public BaseVertex<3, Vector3>

// 2D位姿 Vertex，(x、y、theta)
VertexSE2 : public BaseVertex<3, SE2>
// 3D位姿 Vertex，6 维增量表示为(x、y、z、qx、qy、qz)，以四元数表示旋转（省略了四元数的 w 部分），qw 通过模长为 1 约束
// 位姿表示为 Isometry3，即欧式变换矩阵(4x4)
VertexSE3 : public BaseVertex<6, Isometry3>
// 3D位姿 Vertex，6 维增量表示为(dqx, dqy, dqz, dx, dy, dz)，是平移和旋转的李代数表示
// 位姿表示为 SE3Quat，即四元数和平移向量
// g2o 中 SE3Quat 的定义方式是旋转在前，平移在后
VertexSE3Expmap : public BaseVertex<6, SE3Quat>

// Sim3 Vertex, 7 维增量表示为(x、y、z、qx、qy、qz、s)，以四元数表示旋转（省略了四元数的 w 部分）
// 相似变换 Sim，比欧式变换多了一个自由度 s，允许物体进行均匀缩放
VertexSim3Expmap : public BaseVertex<7, Sim3>
// SBACam Vertex，6 维增量表示为(x、y、z、qx、qy、qz)，以四元数表示旋转（省略了四元数的 w 部分），qw 通过模长为1约束
// SBACam 类表示了一个带有相机内参和坐标变换的相机模型。它继承了 SE3Quat 类，并增加了处理相机内参、立体基线、以及从世界坐标系到图像坐标系的变换所需的额外成员。
VertexCam : public BaseVertex<6, SBACam>

一、VertexPointXY : public BaseVertex<2, Vector2>

//*** g2o源码 g2o/g2o/types/slam2d/vertex_point_xy.h ***//
// 2D二维点 Vertex

#ifndef G2O_VERTEX_POINT_XY_H
#define G2O_VERTEX_POINT_XY_H

#include "g2o_types_slam2d_api.h"
#include "g2o/config.h"
#include "g2o/core/base_vertex.h"
#include "g2o/core/hyper_graph_action.h"

#include <Eigen/Core>

namespace g2o {

  class G2O_TYPES_SLAM2D_API VertexPointXY : public BaseVertex<2, Vector2>
  {
    public:
      EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
      VertexPointXY();

      virtual void setToOriginImpl() {
        _estimate.setZero();
      }

      /* 类中其他函数 */

      virtual void oplusImpl(const number_t* update)
      {
        _estimate[0] += update[0];
        _estimate[1] += update[1];
      }

      virtual bool read(std::istream& is);
      virtual bool write(std::ostream& os) const;

  };

//*** g2o源码 g2o/g2o/types/slam2d/vertex_point_xy.cpp ***//
#include "vertex_point_xy.h"

#ifdef G2O_HAVE_OPENGL
#include "g2o/stuff/opengl_primitives.h"
#include "g2o/stuff/opengl_wrapper.h"
#endif

#include <typeinfo>

#include "g2o/stuff/macros.h"

namespace g2o {

VertexPointXY::VertexPointXY() : BaseVertex<2, Vector2>() {
  _estimate.setZero();
}

bool VertexPointXY::read(std::istream& is) {
  return internal::readVector(is, _estimate);
}

bool VertexPointXY::write(std::ostream& os) const {
  return internal::writeVector(os, estimate());
}

}  // namespace g2o

二、VertexPointXYZ : public BaseVertex<3, Vector3>

//*** g2o源码 g2o/g2o/types/slam3d/vertex_pointxyz.h ***//
// 3D空间点 Vertex

#ifndef G2O_VERTEX_TRACKXYZ_H_
#define G2O_VERTEX_TRACKXYZ_H_

#include "g2o/core/base_vertex.h"
#include "g2o/core/hyper_graph_action.h"
#include "g2o_types_slam3d_api.h"

namespace g2o {
/**
 * \brief Vertex for a tracked point in space
 */
class G2O_TYPES_SLAM3D_API VertexPointXYZ : public BaseVertex<3, Vector3> {
 public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
  VertexPointXYZ() {}
  virtual bool read(std::istream& is);
  virtual bool write(std::ostream& os) const;

  virtual void setToOriginImpl() { _estimate.fill(0.); }

  virtual void oplusImpl(const number_t* update_) {
    Eigen::Map<const Vector3> update(update_);
    _estimate += update;
  }

  /* 类中其他函数 */

};

}  // namespace g2o

//*** g2o源码 g2o/g2o/types/slam3d/vertex_pointxyz.cpp ***//
#include "vertex_pointxyz.h"

#include <stdio.h>

#ifdef G2O_HAVE_OPENGL
#include "g2o/stuff/opengl_primitives.h"
#include "g2o/stuff/opengl_wrapper.h"
#endif

#include <typeinfo>

namespace g2o {

bool VertexPointXYZ::read(std::istream& is) {
  return internal::readVector(is, _estimate);
}

bool VertexPointXYZ::write(std::ostream& os) const {
  return internal::writeVector(os, estimate());
}

}  // namespace g2o

三、VertexSE2 : public BaseVertex<3, SE2>

//*** g2o源码 g2o/g2o/types/slam2d/vertex_se2.h ***//
// 2D 位姿顶点, (x,y,theta)

#ifndef G2O_VERTEX_SE2_H
#define G2O_VERTEX_SE2_H

#include "g2o/config.h"
#include "g2o/core/base_vertex.h"
#include "g2o/core/hyper_graph_action.h"
#include "g2o_types_slam2d_api.h"
#include "se2.h"

namespace g2o {

/**
 * \brief 2D pose Vertex, (x,y,theta)
 */
class G2O_TYPES_SLAM2D_API VertexSE2 : public BaseVertex<3, SE2> {
 public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
  VertexSE2();

  virtual void setToOriginImpl() { _estimate = SE2(); }

  virtual void oplusImpl(const number_t* update) {
    Vector2 t = _estimate.translation(); //提取平移部分 t
    t += Eigen::Map<const Vector2>(update);
    number_t angle = normalize_theta(_estimate.rotation().angle() + update[2]);
    _estimate.setTranslation(t);
    _estimate.setRotation(Rotation2D(angle));
  }

  /* 类中其他函数 */

  virtual bool read(std::istream& is);
  virtual bool write(std::ostream& os) const;
}

//*** g2o源码 g2o/g2o/types/slam2d/vertex_se2.cpp ***//
#include "vertex_se2.h"

#include <typeinfo>

#ifdef G2O_HAVE_OPENGL
#include "g2o/stuff/opengl_primitives.h"
#include "g2o/stuff/opengl_wrapper.h"

namespace g2o {

VertexSE2::VertexSE2() : BaseVertex<3, SE2>() {}

bool VertexSE2::read(std::istream& is) {
  Vector3 p;
  bool state = internal::readVector(is, p);
  setEstimate(p);
  return state;
}

bool VertexSE2::write(std::ostream& os) const {
  return internal::writeVector(os, estimate().toVector());
}

}  // namespace g2o

四、VertexSE3 : public BaseVertex<6, Isometry3>

//*** g2o源码 g2o/g2o/types/slam3d/vertex_se3.h ***//
// 3D位姿 Vertex，6 维增量表示为(x、y、z、qx、qy、qz)，以四元数表示旋转，qw 通过模长为1约束
// 位姿表示为 Isometry3，即欧式变换矩阵(4x4)

#ifndef G2O_VERTEX_SE3_
#define G2O_VERTEX_SE3_

#include "g2o/config.h"
#include "g2o/core/base_vertex.h"
#include "g2o/core/hyper_graph_action.h"
#include "isometry3d_mappings.h"
#include "g2o_types_slam3d_api.h"

namespace g2o {

/**
 * \brief 3D pose Vertex, represented as an Isometry3
 *
 * 3D 位姿顶点，表示为 Isometry3，即一种仿射变换（实际为欧式变换），仅通过连接旋转矩阵和平移矩阵构成。
 * 因此，不包含缩放或投影。为了避免 Isometry3 的旋转部分在数值上变得不稳定，
 * 我们在多次调用 oplus 方法后对旋转矩阵进行正交化处理。
 *
 * 构建的增量 △X 为一个 6 维向量(x,y,z,qx,qy,qz)（注意，我们省略了四元数的 w 部分）
*/
  class G2O_TYPES_SLAM3D_API VertexSE3 : public BaseVertex<6, Isometry3>
  {
    public:
      EIGEN_MAKE_ALIGNED_OPERATOR_NEW;

      static const int orthogonalizeAfter = 1000; //< orthogonalize the rotation matrix after N updates

      VertexSE3();

      virtual void setToOriginImpl() {
        _estimate = Isometry3::Identity();
      }

      virtual bool read(std::istream& is);
      virtual bool write(std::ostream& os) const;

      /* 类中其他函数 */

      /**
       * 更新该顶点的位置。update，即 △X 以 (x,y,z,qx,qy,qz) 的形式给出，
       * 其中 (x,y,z) 表示平移增量，而 (qx,qy,qz) 对应四元数的相关元素。
       * 四元数缺失的元素 qw 由以下公式恢复：（ ||(qw,qx,qy,qz)|| == 1  的约束）
       * || (qw,qx,qy,qz) || == 1 => qw = sqrt(1 - || (qx,qy,qz) ||
       */
      virtual void oplusImpl(const number_t* update)
      {
        Eigen::Map<const Vector6> v(update);
        Isometry3 increment = internal::fromVectorMQT(v);
        _estimate = _estimate * increment;
        // _numOplusCalls：调用 oplusImpl() 函数的次数。
        // 当更新次数超过 orthogonalizeAfter时，对旋转矩阵进行正交化，以避免数值误差导致旋转部分不再保持正交性。
        if (++_numOplusCalls > orthogonalizeAfter) {
          _numOplusCalls = 0;
          // 对 Isometry3 矩阵的前三列（即旋转部分）进行正交化处理。
          internal::approximateNearestOrthogonalMatrix(_estimate.matrix().topLeftCorner<3,3>());
        }
      }

    protected:
      int _numOplusCalls;     ///< store how often opluse was called to trigger orthogonaliation of the rotation matrix
  };

} // end namespace

//*** g2o源码 g2o/g2o/types/slam3d/vertex_se3.cpp ***//
#include "vertex_se3.h"

#include "g2o/core/factory.h"
#ifdef G2O_HAVE_OPENGL
#include "g2o/stuff/opengl_primitives.h"
#include "g2o/stuff/opengl_wrapper.h"
#endif

#include <iostream>

#include "g2o/core/cache.h"

using namespace Eigen;

namespace g2o {

VertexSE3::VertexSE3() : BaseVertex<6, Isometry3>(), _numOplusCalls(0) {
  setToOriginImpl();
  updateCache();
}

bool VertexSE3::read(std::istream& is) {
  Vector7 est;
  bool state = internal::readVector(is, est);
  setEstimate(internal::fromVectorQT(est));
  return state;
}

bool VertexSE3::write(std::ostream& os) const {
  return internal::writeVector(os, internal::toVectorQT(estimate()));
}

}  // namespace g2o

顶点更新方式：右乘增量（数据类型为 Isometry3）

_estimate = _estimate * △X

      // 顶点更新函数
      virtual void oplusImpl(const number_t* update)
      {
        Eigen::Map<const Vector6> v(update);
        Isometry3 increment = internal::fromVectorMQT(v);
        _estimate = _estimate * increment;
        if (++_numOplusCalls > orthogonalizeAfter) {
          _numOplusCalls = 0;
          internal::approximateNearestOrthogonalMatrix(_estimate.matrix().topLeftCorner<3,3>());
        }
      }

五、VertexSE3Expmap : public BaseVertex<6, SE3Quat>

//*** g2o源码 g2o/g2o/types/sba/vertex_se3_expmap.h ***//
// SE(3)顶点,内部用变换矩阵参数化，外部用指数映射参数化
// 3D位姿 Vertex，6 维增量表示为(dqx, dqy, dqz, dx, dy, dz)，是平移和旋转的李代数表示
// 位姿表示为 SE3Quat，即四元数和平移向量

#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/vertex_se3_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

顶点更新方式：左乘增量（数据类型为 SE3Quat）。通过李代数到李群的指数映射（exp），将增量转换为 SE3Quat 类型。

_estimate = SE3Quat::exp(△X) * _estimate

// 顶点更新函数
void VertexSE3Expmap::oplusImpl(const number_t* update_) {
  Eigen::Map<const Vector6> update(update_);
  setEstimate(SE3Quat::exp(update) * estimate());
}

六、VertexSim3Expmap : public BaseVertex<7, Sim3>

//*** g2o源码 g2o/g2o/types/sim3/types_seven_dof_expmap.h ***//
// Sim3 Vertex, 7 维增量表示为(x、y、z、qx、qy、qz、s)，以四元数表示旋转（省略了四元数的 w 部分）
// 相似变换 Sim，比欧式变换多了一个自由度 s，允许物体进行均匀缩放

#ifndef G2O_SEVEN_DOF_EXPMAP_TYPES
#define G2O_SEVEN_DOF_EXPMAP_TYPES
#include "g2o/config.h"
#include "g2o/core/base_vertex.h"
#include "g2o/core/base_binary_edge.h"
#include "g2o/types/sba/types_six_dof_expmap.h"
#include "sim3.h"

/**
 * \brief Sim3 顶点，(x, y, z, qw, qx, qy, qz)
 * 构建的增量 △X 为一个 7 维向量 (x, y, z, qx, qy, qz, s)（注意，我们省略了四元数的 w 部分）
 *
 * 将表示两个 cameras 之间的相对位姿变换。
*/
class G2O_TYPES_SIM3_API VertexSim3Expmap : public BaseVertex<7, Sim3>
{
 public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
  VertexSim3Expmap();
  virtual bool read(std::istream& is);
  virtual bool write(std::ostream& os) const;

  virtual void setToOriginImpl() {
    _estimate = Sim3();
  }

  /* 类中其他函数 */

  virtual void oplusImpl(const number_t* update_)
  {
    Eigen::Map<Vector7> update(const_cast<number_t*>(update_));

    if (_fix_scale)
      update[6] = 0;

    Sim3 s(update);
    setEstimate(s*estimate());
  }

  bool _fix_scale;

};

} // end namespace

      //*** g2o源码 g2o/g2o/types/sim3/sim3.h ***//
      Sim3(const Vector7 & update)
      {

        Vector3 omega;
        for (int i=0; i<3; i++)
          omega[i]=update[i];

        Vector3 upsilon;
        for (int i=0; i<3; i++)
          upsilon[i]=update[i+3];

        number_t sigma = update[6];
        number_t theta = omega.norm();
        Matrix3 Omega = skew(omega);
        s = std::exp(sigma); //通过指数函数计算尺度因子 s
        Matrix3 Omega2 = Omega*Omega;
        Matrix3 I;
        I.setIdentity();
        Matrix3 R;

        number_t eps = cst(0.00001);
        number_t A,B,C;
        // (qx、qy、qz)的范数（theta）很小时，R 矩阵（旋转矩阵）可以通过泰勒展开来近似（R = I + Omega + Omega²/2）
        if (fabs(sigma)<eps)
        {
          C = 1;
          if (theta<eps)
          {
            A = cst(1./2.);
            B = cst(1./6.);
            R = (I + Omega + Omega*Omega/2);//R=I+(1-cos(theta))*a^a^+sin(theta)*a^~=(omit O(theta^3))=I+theta^2/2*a^a^+theta*a^
          }
          // 当 theta 较大时，使用 Rodrigues 公式进行旋转矩阵的计算
          else
          {
            number_t theta2= theta*theta;
            A = (1-std::cos(theta))/(theta2);
            B = (theta-std::sin(theta))/(theta2*theta);
            R = I + std::sin(theta)/theta *Omega + (1-std::cos(theta))/(theta*theta)*Omega2;
          }
        }
        else
        {
          C=(s-1)/sigma;
          if (theta<eps)
          {
            number_t sigma2= sigma*sigma;
            A = ((sigma-1)*s+1)/sigma2;
            B= ((cst(0.5)*sigma2-sigma+1)*s-1)/(sigma2*sigma);//B=[C-((s*cos(theta)-1)*sigma+s*sin(theta)*theta)/(sigma^2+theta^2)]/theta^2~=(omit O(theta^2))=
	    //(1/2*s*sigma-s)/(sigma^2)+[C-(s-1)*sigma/(sigma^2+theta^2)]/theta^2~=(0.5*sigma^2*s-s*sigma)/sigma^3+[s-1]/sigma^3=[s*(0.5*sigma^2-sigma+1)-1]/sigma^3  
            R = (I + Omega + Omega2/2);//R=I+(1-cos(theta))*a^a^+sin(theta)*a^~=I+theta^2/2*a^a^+theta*a^
          }
          else
          {
            R = I + std::sin(theta)/theta *Omega + (1-std::cos(theta))/(theta*theta)*Omega2;
            number_t a=s*std::sin(theta);
            number_t b=s*std::cos(theta);
            number_t theta2= theta*theta;
            number_t sigma2= sigma*sigma;
            number_t c=theta2+sigma2;
            A = (a*sigma+ (1-b)*theta)/(theta*c);
            B = (C-((b-1)*sigma+a*theta)/(c))*1/(theta2);

          }
        }
        r = Quaternion(R);



        Matrix3 W = A*Omega + B*Omega2 + C*I;
        t = W*upsilon;
      }

//*** g2o源码 g2o/g2o/types/sim3/types_seven_dof_expmap.h ***//
#include "types_seven_dof_expmap.h"

#include "g2o/core/factory.h"
#include "g2o/stuff/macros.h"

namespace g2o {

G2O_REGISTER_TYPE(VERTEX_SIM3 : EXPMAP, VertexSim3Expmap);

VertexSim3Expmap::VertexSim3Expmap() : BaseVertex<7, Sim3>() {
  _marginalized = false;
  _fix_scale = false;

  _principle_point1[0] = 0;
  _principle_point1[1] = 0;
  _focal_length1[0] = 1;
  _focal_length1[1] = 1;

  _principle_point2[0] = 0;
  _principle_point2[1] = 0;
  _focal_length2[0] = 1;
  _focal_length2[1] = 1;
}

bool VertexSim3Expmap::read(std::istream &is) {
  Vector7 cam2world;
  bool state = true;
  state &= internal::readVector(is, cam2world);
  state &= internal::readVector(is, _focal_length1);
  state &= internal::readVector(is, _principle_point1);
  setEstimate(Sim3(cam2world).inverse());
  return state;
}

bool VertexSim3Expmap::write(std::ostream &os) const {
  Sim3 cam2world(estimate().inverse());
  Vector7 lv = cam2world.log();
  internal::writeVector(os, lv);
  internal::writeVector(os, _focal_length1);
  internal::writeVector(os, _principle_point1);
  return os.good();
}

} // end namespace

顶点更新方式：左乘增量（数据类型为 Sim3）。

_estimate = Sim3(△X) * _estimate

  // 顶点更新函数
  virtual void oplusImpl(const number_t* update_)
  {
    Eigen::Map<Vector7> update(const_cast<number_t*>(update_));

    if (_fix_scale)
      update[6] = 0;

    Sim3 s(update);
    setEstimate(s*estimate());
  }

七、VertexCam : public BaseVertex<6, SBACam>

//*** g2o源码 g2o/g2o/g2o/types/sba/vertex_cam.h ***//
// SBACam Vertex，6 维增量表示为(x、y、z、qx、qy、qz)，以四元数表示旋转（省略了四元数的 w 部分），qw 通过模长为1约束
// SBACam 类表示了一个带有相机内参和坐标变换的相机模型。它继承了 SE3Quat 类，并增加了处理相机内参、立体基线、以及从世界坐标系到图像坐标系的变换所需的额外成员。

#ifndef G2O_SBA_VERTEX_CAM_H
#define G2O_SBA_VERTEX_CAM_H

#include "g2o/core/base_vertex.h"
#include "g2o_types_sba_api.h"
#include "sbacam.h"

namespace g2o {

/**
 * \brief SBACam 顶点, (x,y,z,qw,qx,qy,qz)
 * 构建的增量 △X 为一个 6 维向量(x,y,z,qx,qy,qz)（注意，我们省略了四元数的 w 部分）
 * 假设 qw 为正值，否则 qx、qy、qz 作为旋转会有二义性。
 */
class G2O_TYPES_SBA_API VertexCam : public BaseVertex<6, SBACam> {
 public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
  VertexCam();

  virtual bool read(std::istream& is);
  virtual bool write(std::ostream& os) const;

  virtual void setToOriginImpl() { _estimate = SBACam(); }

  /* 类中其他函数 */

  virtual void oplusImpl(const number_t* update) {
    Eigen::Map<const Vector6> v(update);
    _estimate.update(v);
    _estimate.setTransform();
    _estimate.setProjection();
    _estimate.setDr();
  }

};
}  // namespace g2o

//*** g2o源码 g2o/g2o/types/sba/sbacam.cpp ***//
// update from the linear solution
// defined in se3quat
void SBACam::update(const Vector6& update) {
  // position update
  _t += update.head(3);
  // small quaternion update
  Quaternion qr;
  qr.vec() = update.segment<3>(3);
  // 利用单位四元数的约束：qw^2 + qx^2 + qy^2 + qz^2 = 1 来恢复 qw
  // 此时假设 qw 为正，以确保旋转方向没有二义性。
  qr.w() =
      sqrt(cst(1.0) - qr.vec().squaredNorm());  // should always be positive
  _r *= qr;                                     // post-multiply
  _r.normalize();
  setTransform();
  setProjection();
  setDr();
}

//*** g2o源码 g2o/g2o/g2o/types/sba/vertex_cam.cpp ***//
#include "vertex_cam.h"

namespace g2o {

// constructor
VertexCam::VertexCam() {}

bool VertexCam::read(std::istream& is) {
  // first the position and orientation (vector3 and quaternion)
  Vector3 t;
  internal::readVector(is, t);
  Quaternion r;
  internal::readVector(is, r.coeffs());
  r.normalize();  // recover nummeric precision

  // form the camera object
  SBACam cam(r, t);

  // now fx, fy, cx, cy, baseline
  number_t fx, fy, cx, cy, tx;

  // try to read one value
  is >> fx;
  if (is.good()) {
    is >> fy >> cx >> cy >> tx;
    cam.setKcam(fx, fy, cx, cy, tx);
  } else {
    is.clear();
    std::cerr << "cam not defined, using defaults" << std::endl;
    cam.setKcam(300, 300, 320, 320, cst(0.1));
  }

  setEstimate(cam);
  return true;
}

bool VertexCam::write(std::ostream& os) const {
  const SBACam& cam = estimate();

  // first the position and orientation (vector3 and quaternion)
  internal::writeVector(os, cam.translation());
  internal::writeVector(os, cam.rotation().coeffs());

  // now fx, fy, cx, cy, baseline
  os << cam.Kcam(0, 0) << " ";
  os << cam.Kcam(1, 1) << " ";
  os << cam.Kcam(0, 2) << " ";
  os << cam.Kcam(1, 2) << " ";
  os << cam.baseline << " ";
  return os.good();
}

}  // namespace g2o

顶点更新方式：平移部分直接相加；旋转部分右乘增量（数据类型为 Quaternion）。

_t = _t + Vector3(△X.head(3))

_r = _r * Quaternion(△X.segment<3>(3))