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