G2O定义边时用到的linearizeOplus函数的注意事项

问题描述

本文依据问题仍采用利用G2O优化PNP的位姿问题，链接在这里：上篇文章利用G2O优化PNP的位姿估计。
但注意到在G2O边的定义中，我没有使用linearizeOplus这个函数，但是老师给出的很多实例都定义了这个函数。那么linearizeOplus函数究竟有什么作用呢，我们下面来讨论一下！

G2O边的定义

G2O定义边时最重要的四个函数分别是读盘、存盘、误差计算、雅可比函数计算函数（也就是linearizeOplus函数），定义方式如下：

class myEdge: public g2o::BaseBinaryEdge<errorDim, errorType, Vertex1Type, Vertex2Type>
  {
      public:
      EIGEN_MAKE_ALIGNED_OPERATOR_NEW      
      myEdge(){}     
      virtual bool read(istream& in) {}
      virtual bool write(ostream& out) const {}      
      virtual void computeError() override
      {
          // ...
          _error = _measurement - Something;
      }      
      virtual void linearizeOplus() override
      {
          _jacobianOplusXi(pos, pos) = something;
          // ...         
          /*
          _jocobianOplusXj(pos, pos) = something;
          ...
          */
      }      
      private:
      // data
  }

• 但既然linearizeOplus是这么重要的一个函数，为什么在上篇我们没有定义它仍旧能正常运行并能得出不错的结果？下面我们再来分析一下！

linearizeOplus留空的情况

linearizeOplus函数是边（误差）对于各个顶点（优化变量）的偏导数（雅可比矩阵）的解析求导函数，若此函数不被定义，则采用G2O内置的数值求导，但是数值求导很慢，不如解析求导迅速，效果不佳，优点是不需要计算雅可比矩阵，比较方便，下面具体比较一下：

class EdgeProjection : public g2o::BaseUnaryEdge<2, Eigen::Vector2d, VertexPose> {
public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW;

  EdgeProjection(const Eigen::Vector3d &pos, const Eigen::Matrix3d &K) : _pos3d(pos), _K(K) {}

  virtual void computeError() override {
    const VertexPose *v = static_cast<VertexPose *> (_vertices[0]);
    Sophus::SE3 T = v->estimate();
    Eigen::Vector3d pos_pixel = _K * (T * _pos3d);
    pos_pixel /= pos_pixel[2];
    _error = _measurement - pos_pixel.head<2>();
  }
  virtual bool read(istream &in) override {}

  virtual bool write(ostream &out) const override {}

private:
  Eigen::Vector3d _pos3d;
  Eigen::Matrix3d _K;
};

输出结果：

calling bundle adjustment by g2o
iteration= 0	 chi2= 410.670189	 time= 0.000278232	 cumTime= 0.000278232	 edges= 75	 schur= 0	 lambda= 77.732901	 levenbergIter= 1
iteration= 1	 chi2= 299.764658	 time= 0.000171042	 cumTime= 0.000449274	 edges= 75	 schur= 0	 lambda= 25.910967	 levenbergIter= 1
iteration= 2	 chi2= 299.763574	 time= 0.000151284	 cumTime= 0.000600558	 edges= 75	 schur= 0	 lambda= 17.273978	 levenbergIter= 1
iteration= 3	 chi2= 299.763574	 time= 0.000154272	 cumTime= 0.00075483	 edges= 75	 schur= 0	 lambda= 11.515985	 levenbergIter= 1
iteration= 4	 chi2= 299.763574	 time= 0.000189568	 cumTime= 0.000944398	 edges= 75	 schur= 0	 lambda= 7.677324	 levenbergIter= 1
iteration= 5	 chi2= 299.763574	 time= 0.000204764	 cumTime= 0.00114916	 edges= 75	 schur= 0	 lambda= 276604732907436576.000000	 levenbergIter= 10
optimization costs time: 0.001712 seconds.
pose estimated by g2o =
  0.997906 -0.0509194  0.0398875  -0.126782
 0.0498187   0.998362  0.0281209 -0.0084395
 -0.041254 -0.0260749   0.998808  0.0603494
         0          0          0          1
solve pnp by g2o cost time: 0.001712 seconds.

运行时间为0.001712 s，6次迭代后收敛。

linearizeOplus不留空的情况

单顶点：

_jacobianOplusXi

双顶点：

_jacobianOplusXi
_jacobianOplusXj

//实现linearizeOplus()，单顶点的计算_jacobianOplusXi矩阵。
//双顶点的还要计算_jacobianOplusXj矩阵。
//_jacobianOplusXi = d(error)/d(vi), _jacobianOplusXj = d(error)/d(vj)
//矩阵维度 = 观测值维度 x 顶点维度
//所以这个实例是2*6维

virtual void linearizeOplus() override {
    const VertexPose *v = static_cast<VertexPose *> (_vertices[0]);
    Sophus::SE3 T = v->estimate();
    Eigen::Vector3d pos_cam = T * _pos3d;
    double fx = _K(0, 0);
    double fy = _K(1, 1);
    double cx = _K(0, 2);
    double cy = _K(1, 2);
    double X = pos_cam[0];
    double Y = pos_cam[1];
    double Z = pos_cam[2];
    double Z2 = Z * Z;
    _jacobianOplusXi
      << -fx / Z, 0, fx * X / Z2, fx * X * Y / Z2, -fx - fx * X * X / Z2, fx * Y / Z,
      0, -fy / Z, fy * Y / (Z * Z), fy + fy * Y * Y / Z2, -fy * X * Y / Z2, -fy * X / Z;
  }

输出结果：

calling bundle adjustment by g2o
iteration= 0	 chi2= 410.670199	 time= 4.8726e-05	 cumTime= 4.8726e-05	 edges= 75	 schur= 0	 lambda= 77.732900	 levenbergIter= 1
iteration= 1	 chi2= 299.764658	 time= 1.6665e-05	 cumTime= 6.5391e-05	 edges= 75	 schur= 0	 lambda= 25.910967	 levenbergIter= 1
iteration= 2	 chi2= 299.763574	 time= 1.4825e-05	 cumTime= 8.0216e-05	 edges= 75	 schur= 0	 lambda= 17.273978	 levenbergIter= 1
iteration= 3	 chi2= 299.763574	 time= 1.4432e-05	 cumTime= 9.4648e-05	 edges= 75	 schur= 0	 lambda= 11.515985	 levenbergIter= 1
iteration= 4	 chi2= 299.763574	 time= 1.4483e-05	 cumTime= 0.000109131	 edges= 75	 schur= 0	 lambda= 7.677323	 levenbergIter= 1
iteration= 5	 chi2= 299.763574	 time= 4.7861e-05	 cumTime= 0.000156992	 edges= 75	 schur= 0	 lambda= 5.118216	 levenbergIter= 1
iteration= 6	 chi2= 299.763574	 time= 7.7733e-05	 cumTime= 0.000234725	 edges= 75	 schur= 0	 lambda= 351721100425.998779	 levenbergIter= 8
optimization costs time: 0.000581615 seconds.
pose estimated by g2o =
  0.997906 -0.0509194  0.0398875  -0.126782
 0.0498187   0.998362  0.0281209 -0.0084395
 -0.041254 -0.0260749   0.998808  0.0603494
         0          0          0          1
solve pnp by g2o cost time: 0.000710443 seconds.

经过7次迭代收敛，运行时间为0.0007s，所以当我们定义了解析导数时，运行速率得到很大提升。所以，如果可以得话，我们务必手动定义一下雅可比矩阵，尽量不使用自动求导，因为运行时间对于SLAM真的很重要！

G2O_MAKE_AUTO_AD_FUNCTIONS

有些版本的G2O中，在G2O安装包的example文件夹中的曲线拟合部分有这个自动求导指令，但是我看高翔老师提供的两个版本的安装包都没有这个指令，所以我也没搞明白？
只能附上一篇官方源代码链接：
g2o/g2o/examples/data_fitting/curve_fit.cpp
其中它的边是这么定义的：

class EdgePointOnCurve
    : public g2o::BaseUnaryEdge<1, Eigen::Vector2d, VertexParams> {
 public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
  EdgePointOnCurve() {}
  virtual bool read(std::istream& /*is*/) {
    cerr << __PRETTY_FUNCTION__ << " not implemented yet" << endl;
    return false;
  }
  virtual bool write(std::ostream& /*os*/) const {
    cerr << __PRETTY_FUNCTION__ << " not implemented yet" << endl;
    return false;
  }

  template <typename T>
  bool operator()(const T* params, T* error) const {
    const T& a = params[0];
    const T& b = params[1];
    const T& lambda = params[2];
    T fval = a * exp(-lambda * T(measurement()(0))) + b;
    error[0] = fval - measurement()(1);
    return true;
  }

  G2O_MAKE_AUTO_AD_FUNCTIONS  // use autodiff
};