SLAM-G2O分析

简介：由图中的关系，可以知道，g2o问题的食物链顶层是SparseOptimizer,一个SparseOptimizer包含优化算法，g2o里面有三种优化算法，分别为GN , LM , Dogleg,而每一种算法都需要执行Ax=b的求解，而求解这些矩阵就需要求解器Solver,用的最多的就是线性求解器，g2o里面有几种线性求解器分别为cholmod，　csparse，　dense，　eigen，　pcg。这些求解器针对什么问题后面会详细介绍。下面可以说图优化的流程了：

１．构建SparseOptimizer　，　选一种优化算法，一共有三种分别为　OptimizationAlgorithmLevenberg，OptimizationAlgorithmDogleg，OptimizationAlgorithmGaussNewton，然后在给选定的算法一个求解器。这个过程的代码如下，我们是以ＢＡ为例：

　　

    g2o::SparseOptimizer optimizer;
    g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg(
        new g2o::BlockSolverX(
            new g2o::LinearSolverEigen<g2o::BlockSolverX::PoseMatrixType>()));
　　　optimizer.setAlgorithm(solver);

２：构建属于本问题的节点，我们首先要明白，我们的节点一共有几种，所谓的节点就是我们需要优化的变量，对于ＢＡ问题我们要优化相机的位姿和地图点的三维坐标，先说相机的位姿：

　　2.1　构建位姿节点

　　　　（１）继承ＢaseVertex,写自己的节点，这个时候要清楚，自己的优化变量多少维度，类型是什么，比如g2o里面提供的位姿节点VertexSE3Expmap，就是６维的，然后类型是SE3Quat；　　

　　　　（２）设置节点的输入输出流

　　　　（３）设定默认值，这个时候操纵变量_estimate

　　　　（４）最重要的一步oplusImpl()函数，在这个函数里面参数是传入的是节点的更新量，在这个函数里面要完成之前的估计值_estimate和这个增量作用之后得到的估计值，将这个估计值再设置给_estimate,完成节点的更新。

　　　　　

class  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 double* update_)  {
    Eigen::Map<const Vector6d> update(update_);
    setEstimate(SE3Quat::exp(update)*estimate());//////setEstimate就是用来设置节点值的
  }
};

 　　2.2 构建地图点节点

 class VertexSBAPointXYZ : public BaseVertex<3, Vector3d>
{
  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 double* update)
    {
      Eigen::Map<const Vector3d> v(update);
      _estimate += v;
    }
};

 ３．构建边，每一个边就是误差函数，既然我们要估计两种变量，那就是两元边，这个边链接了相机的位姿和地图点。两元边的设置需要设置，观测维度，观测类型，关联节点类型两个种。自定义二元边需要继承BaseBinaryEdge.这里我们以EdgeSE3ProjectXYZ为例说一下继承之后需要重写哪些函数，，首先computeError是一个比较重要的函数，如何根据节点的值计算误差，那有可能人有人问，既然两个类型节点，我怎么知道计算的时候用哪一个，是这样的，_vertices[0]代表VertexSBAPointXYZ，_vertices[１]代表VertexSE3Expmap，；其实这里我们要保证一个约定就是，我们在给边添加节点的时候一般用ｓｅｔＶｅｒｔｅｘ函数，里面有指定序号的，这里的序号指定和我们public BaseBinaryEdge<2, Vector2d, VertexSBAPointXYZ, VertexSE3Expmap>中的节点顺序，以及我们利用_vertices取值的时候的索引先后关系要一一对应好。如下面代码：

　　

    

class  EdgeSE3ProjectXYZ: public  BaseBinaryEdge<2, Vector2d, VertexSBAPointXYZ, VertexSE3Expmap>{　/// 2代表观测维度为２，也就是我们的像素x y ; Vector2d是观测量的类型，VertexSBAPointXYZ是一个节点的类型对应_vertices[0]，　VertexSE3Expmap是另一个节点的类型
public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW

  EdgeSE3ProjectXYZ();

  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]);
    Vector2d obs(_measurement);
    _error = obs-cam_project(v1->estimate().map(v2->estimate()));
  }

  bool isDepthPositive() {
    const VertexSE3Expmap* v1 = static_cast<const VertexSE3Expmap*>(_vertices[1]);
    const VertexSBAPointXYZ* v2 = static_cast<const VertexSBAPointXYZ*>(_vertices[0]);
    return (v1->estimate().map(v2->estimate()))(2)>0.0;
  }
    

  virtual void linearizeOplus();

  Vector2d cam_project(const Vector3d & trans_xyz) const;

  double fx, fy, cx, cy;
};            

３.1 linearizeOplus()也是一个很重要的函数，对应雅克比矩阵，对于二元边，我们要得到误差函数相对于每一个状态变量的雅克比，那二元边就有两种雅克比，那这两种雅克比我们都要设定，如下面的代码：_jacobianOplusXi　对应第０个状态变量的雅克比矩阵，和_vertices[0]对应，_jacobianOplusXj是第二个状态变量的雅克比矩阵，这是二元边的专属。对于一元边linearizeOplus()中只需要设置_jacobianOplusXi就可以了。

void EdgeSE3ProjectXYZ::linearizeOplus() {
  VertexSE3Expmap * vj = static_cast<VertexSE3Expmap *>(_vertices[1]);
  SE3Quat T(vj->estimate());
  VertexSBAPointXYZ* vi = static_cast<VertexSBAPointXYZ*>(_vertices[0]);
  Vector3d xyz = vi->estimate();
  Vector3d xyz_trans = T.map(xyz);

  double x = xyz_trans[0];
  double y = xyz_trans[1];
  double z = xyz_trans[2];
  double z_2 = z*z;

  Matrix<double,2,3> tmp;
  tmp(0,0) = fx;
  tmp(0,1) = 0;
  tmp(0,2) = -x/z*fx;

  tmp(1,0) = 0;
  tmp(1,1) = fy;
  tmp(1,2) = -y/z*fy;

  _jacobianOplusXi =  -1./z * tmp * T.rotation().toRotationMatrix();

  _jacobianOplusXj(0,0) =  x*y/z_2 *fx;
  _jacobianOplusXj(0,1) = -(1+(x*x/z_2)) *fx;
  _jacobianOplusXj(0,2) = y/z *fx;
  _jacobianOplusXj(0,3) = -1./z *fx;
  _jacobianOplusXj(0,4) = 0;
  _jacobianOplusXj(0,5) = x/z_2 *fx;

  _jacobianOplusXj(1,0) = (1+y*y/z_2) *fy;
  _jacobianOplusXj(1,1) = -x*y/z_2 *fy;
  _jacobianOplusXj(1,2) = -x/z *fy;
  _jacobianOplusXj(1,3) = 0;
  _jacobianOplusXj(1,4) = -1./z *fy;
  _jacobianOplusXj(1,5) = y/z_2 *fy;
}

 4. 这一步就要给我们的优化器添加节点了：我们这里要注意节点的id是连续增长的。

　　４.1 先添加位姿节点

　　　　

        g2o::VertexSE3Expmap * vSE3 = new g2o::VertexSE3Expmap();
        vSE3->setEstimate(Converter::toSE3Quat(pKF->GetPose()));////设置估计值
        vSE3->setId(pKF->mnId);////设置节点id
        vSE3->setFixed(pKF->mnId==0);/////节点是否固定
        optimizer.addVertex(vSE3);//添加进优化器

 　　4.2 添加地图点节点

　　

        g2o::VertexSBAPointXYZ* vPoint = new g2o::VertexSBAPointXYZ();
        vPoint->setEstimate(Converter::toVector3d(pMP->GetWorldPos()));
        const int id = pMP->mnId+maxKFid+1;
        vPoint->setId(id);
        vPoint->setMarginalized(true);
        optimizer.addVertex(vPoint);

 5.　下面我们要设置边，每一个关键帧的一个位姿节点和很多地图点之间可以构建边，构建部分包括　设置边的节点，设置观测，设置信息矩阵，设置核函数：

   g2o::EdgeSE3ProjectXYZ* e = new g2o::EdgeSE3ProjectXYZ();

                e->setVertex(0, dynamic_cast<g2o::OptimizableGraph::Vertex*>(optimizer.vertex(id)));
                e->setVertex(1, dynamic_cast<g2o::OptimizableGraph::Vertex*>(optimizer.vertex(pKF->mnId)));
                //e->setVertex(2, dynamic_cast<g2o::OptimizableGraph::Vertex*>(vCam));
                e->setMeasurement(obs);
                const float &invSigma2 = pKF->mvInvLevelSigma2[kpUn.octave];
                e->setInformation(Eigen::Matrix2d::Identity()*invSigma2);

                if(bRobust)
                {
                    g2o::RobustKernelHuber* rk = new g2o::RobustKernelHuber;
                    e->setRobustKernel(rk);
                    rk->setDelta(thHuber2D);
                }

                e->fx = pKF->fx;
                e->fy = pKF->fy;
                e->cx = pKF->cx;
                e->cy = pKF->cy;

                optimizer.addEdge(e);

 ６．　开始优化

    optimizer.initializeOptimization();
    optimizer.optimize(nIterations);

 ７：优化完成之后我们可以通过优化器中取出对应序号的节点，里面estimate()函数得到估计值，如下：

        g2o::VertexSE3Expmap* vSE3 = static_cast<g2o::VertexSE3Expmap*>(optimizer.vertex(pKF->mnId));
        g2o::SE3Quat SE3quat = vSE3->estimate();

 ８：优化完成后，对每一条边都进行检查，剔除误差较大的边（认为是错误的边），并设置setLevel为0，即下次不再对该边进行优化，其中vpEdgesMono里面保存着边的指针。另外，用chi2函数来获得（加权）最小二乘误差等，这些可以用来判断某条边的误差是否过大。最后，边允许获得/设置level，将边设置为不同的level，在优化时就可以分开优化不同level的边（orb-slam2的优化就采用了这种multi-level optimization）。其实chi()就是误差和信息矩阵的加权乘积，如下：

      virtual double chi2() const 
      {
        return _error.dot(information()*_error);
      }
　

// 优化完成后，进行Edge的检查
for(size_t i=0, iend=vpEdgesMono.size(); i<iend;i++)
{
    g2o::EdgeSE3ProjectXYZ* e = vpEdgesMono[i];
    MapPoint* pMP = vpMapPointEdgeMono[i];

    if(pMP->isBad())
        continue;

    // 基于卡方检验计算出的阈值（假设测量有一个像素的偏差）
    // 第二个判断条件，用于检查构成该边的MapPoint在该相机坐标系下的深度是否为正？
    if(e->chi2()>5.991 || !e->isDepthPositive())
    {
        e->setLevel(1);// 不优化
    }

    // 因为剔除了错误的边，所以下次优化不再使用核函数
    e->setRobustKernel(0);
}

 ９：其实有时候我们每进行一次迭代，之后都可以检查误差，然后设置边的级别，然后再优化这样子。

杂项解析：

每种edge都需要用户来实现computeError函数。

１：　由于只链接一个节点，因此BaseUnaryEdge只多了一个模版参数：节点类型。并且多了一个雅可比矩阵的成员变量：_jacobianOplusXi。此外，由于HyperGraph::Edge类中用了vector<Vertex *>来存储一条边所连接的节点，因此，BaseUnaryEdge类在初始化的时候会将这个数组resize为1（对应地，BaseBinaryEdge类初始化时会将之resize为2）。

２：　其最主要的函数是计算雅可比矩阵的linearizeOplus函数。可以由用户提供雅可比矩阵的解析解，而默认是g2o自己进行数值求导：g2o使用中值求导方法，其步长delta为常数值。函数对节点的_estimate加上和减去对应步长，再计算对应的误差值，再除以2*delta来计算误差函数对于特定误差项的导数。此时，为了保存节点原来的值，就会使用push/pop函数往_backup变量中保持修改前的节点估计值。（数值求导部分我们后面再细说。）

３：　另一个比较主要的函数是constructQuadraticForm函数。该函数用来计算该误差项的系数矩阵H和对应的b向量。以BaseUnaryEdge为例，函数先调用chi2函数获得误差 ( )，如果没有使用鲁棒核函数，直接使用 来计算系数矩阵H，用 来计算b向量；否则，调用对应函数得到鲁棒核函数的值，及其一阶导、二阶导（rho，见后面的鲁棒核函数）。b向量可以直接通过 来计算，但系数矩阵H则调用robustInformation函数来重新计算加权信息矩阵 (weightedOmega)。

如下面：

      InformationType robustInformation(const Eigen::Vector3d& rho)
      {
        InformationType result = rho[1] * _information;
        //ErrorVector weightedErrror = _information * _error;
        //result.noalias() += 2 * rho[2] * (weightedErrror * weightedErrror.transpose());
        return result;
      }

rho[0] = rho
rho[1] = rho'
rho[2]=rho''
新的权重计算公式为：　rho'  +2rho'' * _error * _error^t.
然后最后的信息矩阵Ｗ＝　（　rho' +2rho'' * _error * _error^t　）* _information(原来程序设置的信息矩阵).
这样原来的 Ｈ＝Ｊ^t W J.
原来的 b = -rho'Ｊ^t_information_error.

４：BaseBinaryEdge链接两个节点，因此比BaseEdge多两个模版参数。对应的，自然也会有两个雅可比矩阵：_jacobianOplusXi和_jacobianOplusXj。其余本质上和BaseUnaryEdge并没有差别。需要注意的是，BaseBinaryEdge类也定义了一个_hessian变量，但这个海塞矩阵并不是残差对于观测（节点）进行求导得到的雅可比J得到的 ，而是通过 得到的，显然，这是_Hpl矩阵，即位姿和路标点的协方差矩阵（或位姿和位姿间的协方差矩阵）。

BaseMultiEdge由于链接的节点数目是不固定的，因此其直接继承BaseEdge类而不在需要额外的模版参数。相关的雅可比矩阵也变成了一个vector变量：_jacobianOplus。链接多个节点并没有带来多大的不同。一些变量如_hessian和_jacobianOplus定义成了vector。而在计算雅可比矩阵（linearizeOplus）时，BaseUnaryEdge只计算一个雅可比矩阵，BaseBinaryEdge计算两个，而BaseMulitEdge利用循环计算了多个雅可比矩阵，分别存储_jacobianOplus[I]中；constructQuadraticForm函数也是同理。

关于方程求解器：

总结：
LinearSolverCholmod ：使用sparse cholesky分解法。继承自LinearSolverCCS
LinearSolverCSparse：使用CSparse法。继承自LinearSolverCCS
LinearSolverDense ：使用dense cholesky分解法。继承自LinearSolver
LinearSolverEigen： 依赖项只有eigen，使用eigen中sparse Cholesky 求解，因此编译好后可以方便的在其他地方使用，性能和CSparse差不多。继承自LinearSolver
LinearSolverPCG ：使用preconditioned conjugate gradient 法，继承自LinearSolver