g2o:VertexSE3Expmap、VertexSBAPointXYZ、EdgeProjectXYZ2UV头文件代码阅读

正如高翔博士 视觉SLAM十四讲中所言，g2o中提供了许多关于BA的节点和边，我们无需自己再实现一遍。为了更好的了解和使用g2o，今天尝试着去阅读如标题所示的三个类的头文件代码。

• VertexSE3Expmap（李代数位姿）：

/**
 * \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;

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

  virtual void oplusImpl(const number_t* update_)  {
    Eigen::Map<const Vector6> update(update_);
    setEstimate(SE3Quat::exp(update)*estimate());
  }
};

首先看一看模板传入的类型参数：（1）6是指李代数的维数（3维旋转，3维平移，据书中所述，g2o是旋转在前，平移在后）；（2）SE3Quat：这个是g2o中定义的相机位姿的类，头文件在"g2o/types/slam3d/se3quat.h"。据书中所述，这个类中使用四元数和平移向量来存储位姿。

接着，看一看oplusImpl函数。这个函数功能是为优化变量进行更新，这里是对李代数进行求导，用到的公式是十四讲第一版的75页中的扰动模型。（这里另外提一下，对李导数的导数有两种定义，一种是李代数加法的求导方式，另一种是扰动模型，两种导数的结果会稍有不同，李代数加法求出的导数结果会带有左雅克比矩阵，较为复杂）

• VertexSBAPointXYZ空间点位置：

  /**
   * \brief Point vertex, XYZ
   */
   class G2O_TYPES_SBA_API VertexSBAPointXYZ : public BaseVertex<3, Vector3>
  {
    public:
      EIGEN_MAKE_ALIGNED_OPERATOR_NEW    
      VertexSBAPointXYZ();
      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> v(update);
        _estimate += v;
      }
  };
  

  传入参数类型就是常用的Eigen的Vector3d，对应空间中的x、y、z三个坐标;留意oplusImpl函数，这个更新函数直接粗暴地让估计值加上更新量（注意，这里传入的update是一个数组）

EdgeProjectXYZ2UV(投影方程边)

class G2O_TYPES_SBA_API EdgeProjectXYZ2UV : public  BaseBinaryEdge<2, Vector2, VertexSBAPointXYZ, VertexSE3Expmap>{
  public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW;

    EdgeProjectXYZ2UV();

    bool read(std::istream& is);

    bool write(std::ostream& os) const;

    void computeError()  {
      const VertexSE3Expmap* v1 = static_cast<const VertexSE3Expmap*>(_vertices[1]);
      const VertexSBAPointXYZ* v2 = static_cast<const VertexSBAPointXYZ*>(_vertices[0]);
      const CameraParameters * cam
        = static_cast<const CameraParameters *>(parameter(0));
      Vector2 obs(_measurement);
      _error = obs-cam->cam_map(v1->estimate().map(v2->estimate()));
    }

    virtual void linearizeOplus();

    CameraParameters * _cam;
};

先看传入的模板参数：（1）根据观测方程，e的单位应该是一个像素，是一个Eigen::Vector2d类型的变量，因此传入误差维度2以及误差类型Eigen::Vector2d；（2）传入的VertexSBAPointXYZ和VertexSE3Expmap就是上述的空间点位置和李代数位姿。

接着看误差计算函数computeError：该函数中的camera是要求用户设置的，在配置SparseOptimizer的时候传入相机参数，如下代码块所示：

    // parameter: camera intrinsics
    g2o::CameraParameters* camera = new g2o::CameraParameters (
        K.at<double> ( 0,0 ), Eigen::Vector2d ( K.at<double> ( 0,2 ), K.at<double> ( 1,2 ) ), 0
    );
    camera->setId ( 0 );
    optimizer.addParameter ( camera );

然后留意_error的计算公式：这条公式就是将待优化的三维空间点（v2->estimate()）投影到第二幅图像上（v1->estimate().map(v2->estimate())），然后用第一图像上的像素坐标减去第二幅图像上的像素坐标。