SLAM14讲学习笔记（十一）g2o图优化中的要点与难点（前端VO中雅克比矩阵的定义）

使用g2o优化的前提是，需要对各种误差的理解足够充足。我将雅克比矩阵的详细解析，写在了这里：点击查看   在源码和自定义的类中，各种雅克比矩阵有的是写在源码里的，有的是需要自己修改和定义的。然而在实践中发现，雅克比矩阵的定义往往不一，容易使初学者摸不着头脑。因此，这篇文章我从代码角度出发，简单解析一下g2o定义时候的要点，和内置的雅克比矩阵的书写方式。  

在g2o中，内置了三大类： VertexSE3Expmap，这个表示李代数的位姿； VertexSBAPointXYZ，表示空间点的位置； EdgeProjectXYZ2UV，表示投影方程的边。 前两个是顶点类型，后一个是边类型。

顶点定义方式：

举VertexSE3Expmap来说：  

class G2O_TYPES_SBA_API VertexSE3Expmap : public BaseVertex<6, SE3Quat>
//顶点需要继承BaseVertex，这里顶点里面存储的是位姿，因此< >里面的6代表se3李代数的6个值，
//即维度为6，而SE3Quat作为数据类型，存储这个se3
{
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 double* update_)  {
    Eigen::Map<const Vector6d> update(update_);
    setEstimate(SE3Quat::exp(update)*estimate());
  }
};

  同样，对于VertexSBAPointXYZ来说：   class G2O_TYPES_SBA_API VertexSBAPointXYZ : public BaseVertex<3, Vector3D>   这里不管是世界坐标系还是相机坐标系下的点，都是xyz三个值，因此< >中，第一个是3,第二个是对应的数据格式，即Vector3D。   对于顶点的定义，照猫画虎即可，例如第六讲，高博的14讲书中124页，g2o拟合曲线那节，照样修改初始值和更新方式即可。    

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;
    }
};

边的定义方式：

在边的定义中，难度就陡然上来了。有难度主要是因为，缺乏必要的说明。例如高博书中不管是讲解源码中EdgeProjectXYZ2UV，还是讲解其他他自定义的边，都没有必要的注释。因此对于初学者来说，理解其内涵较为困难。在这里我针对其重点做一个讲解。   带估计的参数构成节点，而观测数据构成了边。   误差定义在边内，边是附着在顶点上的。误差关于顶点的偏导数定义在边里的雅克比矩阵上，而顶点的更新通过内置的oplusImpl函数实现自己的更新。   先看一个例子：  

//源码初看很吓人，但是不要被它吓到，看懂以后就会明白，其实它也”不过如此“
class G2O_TYPES_SBA_API EdgeProjectXYZ2UV : public  BaseBinaryEdge<2, Vector2D, 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));
      Vector2D obs(_measurement);
      _error = obs-cam->cam_map(v1->estimate().map(v2->estimate()));
    }
    virtual void linearizeOplus();
    CameraParameters * _cam;
};
//注释不在这里写，在本文下面将会详述

  这个内容比较关键，我们需要拆开，抓住重点看：   1.class G2O_TYPES_SBA_API EdgeProjectXYZ2UV : public  BaseBinaryEdge<2, Vector2D, VertexSBAPointXYZ, VertexSE3Expmap>   这是第一句，<2, Vector2D, VertexSBAPointXYZ, VertexSE3Expmap>这四个值中，2表示观测的值是2维的（说白了也就是像素坐标），因此类型为Vector2D；该边被绑定在两个类型的顶点上：VertexSBAPointXYZ, VertexSE3Expmap。（意思就是说，这条边构建的重投影误差，对两个点进行优化，一个是对世界坐标系下的点VertexSBAPointXYZ进行优化，一个是对位姿VertexSE3Expmap进行优化。）   注意：_vertices[1]和_vertices[0]，分别表示“ : public  BaseBinaryEdge<2, Vector2D, VertexSBAPointXYZ, VertexSE3Expmap>”中后面两个绑定点的类型。因此main函数中，这种边如果想绑定到节点上，即edge->setVertex(0,……)这里的“……”写的就是VertexSBAPointXYZ类型；如果这种边想绑定到VertexSE3Expmap类型的节点上，edge->setVertex( )中第一个参数就是填1.   2.computeError函数中，描述了error的计算过程，即观测的值，减去计算出的值。  

Vector2D obs(_measurement); 
_error = obs-cam->cam_map(v1->estimate().map(v2->estimate()));
//就拿这句话来说，obs是测量值，即某个点在像素坐标系下实实在在的值
//v2->estimate()，其实就是VertexSBAPointXYZ类型的一个3D点坐标
//v1->estimate() 就是从顶点中获取的位姿
//把“v2->estimate()”传入v1->estimate().map()中，其实就是世界坐标系下的点经过外参，得到相机坐标系下的点
//即（v1->estimate().map(v2->estimate())
//之后，经过cam->cam_map（）变换，得到像素坐标系下的点，即计算值
//测量值减去计算值，构建一个误差

  这个误差其实就是重投影误差。重投影误差，需要对涉及到的两个顶点求其偏导数。也就是说，重投影误差需要对世界坐标系下的坐标点和位姿分别求偏导数。这个之前我已经总结过，在点击查看中。   3.因此，如果绑定了两个顶点，在linearizeOplus()中则需要写清楚误差分别对于两个顶点的偏导数。第一个为_jacobianOplusXi,第二个为_jacobianOplusXj，需要显示的写出来。如果该边只绑定了一个顶点，则只写_jacobianOplusXi就可以了。   我们现在看看它的两个偏导数：（见书169页，linearizeOplus中，我只贴雅克比矩阵的部分）  

Matrix<double,2,3,Eigen::ColMajor> tmp;
tmp(0,0) = cam->focal_length;
tmp(0,1) = 0;
tmp(0,2) = -x/z*cam->focal_length;
tmp(1,0) = 0;
tmp(1,1) = cam->focal_length;
tmp(1,2) = -y/z*cam->focal_length;
//_jacobianOplusXi是误差关于世界坐标系下坐标点的偏导
//其中，-1./z * tmp是误差关于相机坐标系下坐标点的偏导，
//而T.rotation().toRotationMatrix()也就是R，是相机坐标系下坐标点关于世界坐标系下坐标点的偏导
//二者相乘，得到误差关于世界坐标系下坐标点的偏导
_jacobianOplusXi = -1./z * tmp * T.rotation().toRotationMatrix();
//_jacobianOplusXj是误差关于位姿扰动小量的偏导
//_jacobianOplusXj本质上是由两项相乘得到
//第一项，其实是误差关于相机坐标系下坐标点的偏导，也就是-1./z * tmp
//第二项，其实是相机坐标系下坐标点关于位姿扰动小量的偏导
//两项相乘，得到下面的jacobianOplusXj 
_jacobianOplusXj ( 0,0 ) =  x*y/z_2 *camera_->fx_;
_jacobianOplusXj ( 0,1 ) = - ( 1+ ( x*x/z_2 ) ) *camera_->fx_;
_jacobianOplusXj ( 0,2 ) = y/z * camera_->fx_;
_jacobianOplusXj ( 0,3 ) = -1./z * camera_->fx_;
_jacobianOplusXj ( 0,4 ) = 0;
_jacobianOplusXj ( 0,5 ) = x/z_2 * camera_->fx_;
_jacobianOplusXj ( 1,0 ) = ( 1+y*y/z_2 ) *camera_->fy_;
_jacobianOplusXj ( 1,1 ) = -x*y/z_2 *camera_->fy_;
_jacobianOplusXj ( 1,2 ) = -x/z *camera_->fy_;
_jacobianOplusXj ( 1,3 ) = 0;
_jacobianOplusXj ( 1,4 ) = -1./z *camera_->fy_;
_jacobianOplusXj ( 1,5 ) = y/z_2 *camera_->fy_;

  重投影误差关于世界坐标系下的坐标点P求偏导，即是误差e先对相机坐标系下的点P’求偏导，再由P’对于P求偏导。   （注意：由于重投影误差是观测值减去计算值，计算值在后，观测值在求导的时候看作是常量，因此“误差e先对相机坐标系下的点P’求偏导”说白了也就是-(u,v)对于P’求偏导。）   重投影误差关于位姿扰动小量求偏导，也就是误差e先对相机坐标系下的点P’求偏导，然后P’再对扰动小量求偏导。前者刚刚说过，实际上是-(u,v)对于P’求偏导，后者在李群李代数一节已经推导过，是[I,-P’^]。不过写在雅克比矩阵_jacobianOplusXj中，需要对调前三列和后三列（因为在公式推导中，是平移在前，旋转在后；而在g2o中是旋转在前，平移在后）  

到目前为止，有的人目前还没有问题。但是如果边是自己定义的，那该怎么办呢？雅克比矩阵怎么写？   再看一个例子（这个是第八章光流法和直接法的非线性优化内容）：  

class EdgeSE3ProjectDirect: public BaseUnaryEdge< 1, double, VertexSE3Expmap>

  这个是光度误差的内容。其中，< 1, double, VertexSE3Expmap>中，1表示观测值就是一个灰度值，是一维的，它的类型是double类型。绑定的节点是 VertexSE3Expmap类型，即绑定在位姿节点上。那么在具体的雅克比矩阵里面，就需要定义清楚，即光度误差，关于位姿扰动小量的偏导数。  

        //像素坐标对小量求偏导
        jacobian_uv_ksai ( 0,0 ) = - x*y*invz_2 *fx_;
        jacobian_uv_ksai ( 0,1 ) = ( 1+ ( x*x*invz_2 ) ) *fx_;
        jacobian_uv_ksai ( 0,2 ) = - y*invz *fx_;
        jacobian_uv_ksai ( 0,3 ) = invz *fx_;
        jacobian_uv_ksai ( 0,4 ) = 0;
        jacobian_uv_ksai ( 0,5 ) = -x*invz_2 *fx_; 
        jacobian_uv_ksai ( 1,0 ) = - ( 1+y*y*invz_2 ) *fy_;
        jacobian_uv_ksai ( 1,1 ) = x*y*invz_2 *fy_;
        jacobian_uv_ksai ( 1,2 ) = x*invz *fy_;
        jacobian_uv_ksai ( 1,3 ) = 0;
        jacobian_uv_ksai ( 1,4 ) = invz *fy_;
        jacobian_uv_ksai ( 1,5 ) = -y*invz_2 *fy_;
        Eigen::Matrix<double, 1, 2> jacobian_pixel_uv;
        //光度误差对于像素坐标求偏导
        jacobian_pixel_uv ( 0,0 ) = ( getPixelValue ( u+1,v )-getPixelValue ( u-1,v ) ) /2;
        jacobian_pixel_uv ( 0,1 ) = ( getPixelValue ( u,v+1 )-getPixelValue ( u,v-1 ) ) /2;
        //光度误差对于小量求偏导等于先对像素坐标求偏导，再由像素坐标对于小量求偏导
        _jacobianOplusXi = jacobian_pixel_uv*jacobian_uv_ksai;

  光度误差的内容同样之前也总结过：点击查看  
  光度误差是两项相乘，第一项，是灰度关于像素坐标的导数，即jacobian_pixel_uv的内容；第二项，是像素坐标关于扰动小量的导数。像素坐标关于扰动小量的导数又可以进一步拆分：像素坐标先对扰动小量的相机坐标系下的坐标q求偏导，再由q对扰动小量求偏导。（可以发现：像素坐标关于扰动小量求偏导，不就是重投影误差中，像素坐标关于扰动小量求偏导吗？）   所以雅克比矩阵像上述代码所示。

光上面这个例子，是绝对不够的！下面给出几个小测验：   我们看slam14讲第九章的代码，自定义了三种类型的边：   1.class EdgeProjectXYZRGBD : public g2o::BaseBinaryEdge<3, Eigen::Vector3d, g2o::VertexSBAPointXYZ, g2o::VertexSE3Expmap>   2.class EdgeProjectXYZRGBDPoseOnly: public g2o::BaseUnaryEdge<3, Eigen::Vector3d, g2o::VertexSE3Expmap >   3.class EdgeProjectXYZ2UVPoseOnly: public g2o::BaseUnaryEdge<2, Eigen::Vector2d, g2o::VertexSE3Expmap >   根据上面所述的内容： 1的形参是：观测值维度3，观测值类型Eigen::Vector3d（即观测值是相机坐标系下的点），连接顶点类型g2o::VertexSBAPointXYZ（世界坐标系下的点）, g2o::VertexSE3Expmap（位姿）   2的形参是：观测值维度3，观测值类型Eigen::Vector3d（即观测值是相机坐标系下的点），连接顶点类型g2o::VertexSE3Expmap（位姿）   3的形参是：观测值维度2，观测值类型Eigen::Vector2d，（即观测值是图像坐标系下的点），连接顶点类型g2o::VertexSE3Expmap（位姿）   那么问题来了，在这三个自定义的边的类中，linearizeOplus()中需要自己定义的雅克比矩阵该怎么写？ 这就需要看它们的误差是怎么定义的，一个一个来：  

对于第一个，EdgeProjectXYZRGBD，误差值是这么定义的：  

const g2o::VertexSBAPointXYZ* point = static_cast<const g2o::VertexSBAPointXYZ*> ( _vertices[0] );
const g2o::VertexSE3Expmap* pose = static_cast<const g2o::VertexSE3Expmap*> ( _vertices[1] );
_error = _measurement - pose->estimate().map ( point->estimate() );

  测量值是相机坐标系下的点，观测值是世界坐标系下的点，左乘外参得到的相机坐标系下的计算值。误差值是测量值减去观测值。   这条边绑定了两个节点，分别是世界坐标系下的点，和位姿；因此雅克比矩阵需要分别对二者求偏导。   对前者（对世界坐标系下的点求偏导），误差e=P测量  –  （R*（P世界）+t），P测量、t看成是常量，因此误差关于世界坐标系下的点的偏导，就是-R，因此，雅克比矩阵如下定义：  

_jacobianOplusXi = - T.rotation().toRotationMatrix();     

  对于后者（对位姿求偏导），误差e=P测量  – （R*（P世界）+t），P测量、t看成是常量，误差关于扰动小量位姿的偏导，就是 – （R*（P世界）+t）关于扰动小量位姿的偏导，这个在书的76页已经讲过了，答案是[I,-P’^]。（注意对调前三列和后三列）也就是：  

    _jacobianOplusXj ( 0,0 ) = 0;
    _jacobianOplusXj ( 0,1 ) = -z;   
    _jacobianOplusXj ( 0,2 ) = y;
    _jacobianOplusXj ( 0,3 ) = -1;
    _jacobianOplusXj ( 0,4 ) = 0;
    _jacobianOplusXj ( 0,5 ) = 0;
    _jacobianOplusXj ( 1,0 ) = z;
    _jacobianOplusXj ( 1,1 ) = 0;
    _jacobianOplusXj ( 1,2 ) = -x;
    _jacobianOplusXj ( 1,3 ) = 0;
    _jacobianOplusXj ( 1,4 ) = -1;
    _jacobianOplusXj ( 1,5 ) = 0;
    _jacobianOplusXj ( 2,0 ) = -y;
    _jacobianOplusXj ( 2,1 ) = x;
    _jacobianOplusXj ( 2,2 ) = 0;
    _jacobianOplusXj ( 2,3 ) = 0;
    _jacobianOplusXj ( 2,4 ) = 0;
    _jacobianOplusXj ( 2,5 ) = -1;

  对于第二个：EdgeProjectXYZRGBDPoseOnly，观测值是相机坐标系下的点的g2o边，仅仅附着于位姿节点。误差函数如下定义：  

void EdgeProjectXYZRGBDPoseOnly::computeError()
{
    const g2o::VertexSE3Expmap* pose = static_cast<const g2o::VertexSE3Expmap*> ( _vertices[0] );
    _error = _measurement - pose->estimate().map ( point_ );
}

  这也就是上面的EdgeProjectXYZRGBD，只绑定位姿节点。因此雅克比矩阵，也就只有其中的一项，也就是EdgeProjectXYZRGBD的_jacobianOplusXj：  

//写成_jacobianOplusXi而不是_jacobianOplusXj的原因是因为只绑定了一个节点
    _jacobianOplusXi ( 0,0 ) = 0;
    _jacobianOplusXi ( 0,1 ) = -z;
    _jacobianOplusXi ( 0,2 ) = y;
    _jacobianOplusXi ( 0,3 ) = -1;
    _jacobianOplusXi ( 0,4 ) = 0;
    _jacobianOplusXi ( 0,5 ) = 0;
    _jacobianOplusXi ( 1,0 ) = z;
    _jacobianOplusXi ( 1,1 ) = 0;
    _jacobianOplusXi ( 1,2 ) = -x;
    _jacobianOplusXi ( 1,3 ) = 0;
    _jacobianOplusXi ( 1,4 ) = -1;
    _jacobianOplusXi ( 1,5 ) = 0;
    _jacobianOplusXi ( 2,0 ) = -y;
    _jacobianOplusXi ( 2,1 ) = x;
    _jacobianOplusXi ( 2,2 ) = 0;
    _jacobianOplusXi ( 2,3 ) = 0;
    _jacobianOplusXi ( 2,4 ) = 0;
    _jacobianOplusXi ( 2,5 ) = -1;

  对于第三个：EdgeProjectXYZ2UVPoseOnly，这也就是源码中的EdgeProjectXYZ2UV只对位姿求偏导：  

    _jacobianOplusXi ( 0,0 ) =  x*y/z_2 *camera_->fx_;
    _jacobianOplusXi ( 0,1 ) = - ( 1+ ( x*x/z_2 ) ) *camera_->fx_;
    _jacobianOplusXi ( 0,2 ) = y/z * camera_->fx_;
    _jacobianOplusXi ( 0,3 ) = -1./z * camera_->fx_;
    _jacobianOplusXi ( 0,4 ) = 0;
    _jacobianOplusXi ( 0,5 ) = x/z_2 * camera_->fx_;
    _jacobianOplusXi ( 1,0 ) = ( 1+y*y/z_2 ) *camera_->fy_;
    _jacobianOplusXi ( 1,1 ) = -x*y/z_2 *camera_->fy_;
    _jacobianOplusXi ( 1,2 ) = -x/z *camera_->fy_;
    _jacobianOplusXi ( 1,3 ) = 0;
    _jacobianOplusXi ( 1,4 ) = -1./z *camera_->fy_;
    _jacobianOplusXi ( 1,5 ) = y/z_2 *camera_->fy_;

  在EdgeProjectXYZ2UV源码中，第一项绑定的是世界坐标系下的坐标节点，第二项是位姿节点，（源码不像这里只绑定位姿节点），所以，源码中，_jacobianOplusXj是上面的雅克比矩阵，而_jacobianOplusXi是误差关于世界坐标系下坐标的偏导，即-(u,v)先对P’求偏导，再由P’对P求偏导（也就是R）。  

这部分看懂代码的关键，就是要深刻理解误差求偏导对应的雅克比矩阵的含义。必须深刻理解以后，才能明白代码是在做什么。