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 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 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(_vertices[1]);
      const VertexSBAPointXYZ* v2 = static_cast(_vertices[0]);
      const CameraParameters * cam
        = static_cast(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 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 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 ( _vertices[0] );
const g2o::VertexSE3Expmap* pose = static_cast ( _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 ( _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)。


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

你可能感兴趣的:(SLAM学习笔记)