g2o学习——顶点和边之外的solver

写在前面

最近学习g2o的程序，跟着例程做了几个程序，其实其中大多数要注意的就是顶点和边的一些东西，本次博客旨在记录那些不被看到的过程，也就是g2o帮助我们做了哪些东西，主要参考的就是如下网站：
http://docs.ros.org/fuerte/api/re_vision/html/namespaceg2o.html
这个网站里面有较为全面的g2o的类以及函数的讲解，很方便。
那么这里也就是一个比较浅显的总结，并没有很深入，大致上就是记录g2o在解决问题的内部过程。

---

块求解器（BlockSolver_P_L）

这个地方我在之前的博客中提过一次，这里再提一下，首先祭出他的源码：

template<int p, int l>
using BlockSolverPL = BlockSolver< BlockSolverTraits >;

接着往上走，我们会看到比较详细的BlockSoverTraits类

template <int _PoseDim, int _LandmarkDim>
struct BlockSolverTraits
{
  static const int PoseDim = _PoseDim;
  static const int LandmarkDim = _LandmarkDim;
  typedef Eigen::Matrix PoseMatrixType;
  typedef Eigen::Matrix LandmarkMatrixType;
  typedef Eigen::Matrix PoseLandmarkMatrixType;
  typedef Eigen::Matrix1, Eigen::ColMajor> PoseVectorType;
  typedef Eigen::Matrix1, Eigen::ColMajor> LandmarkVectorType;

  typedef SparseBlockMatrix PoseHessianType;
  typedef SparseBlockMatrix LandmarkHessianType;
  typedef SparseBlockMatrix PoseLandmarkHessianType;
  typedef LinearSolver LinearSolverType;
};

/**
 * \brief traits to summarize the properties of the dynamic size optimization problem
 */
template <>
struct BlockSolverTraits
{
  static const int PoseDim = Eigen::Dynamic;
  static const int LandmarkDim = Eigen::Dynamic;
  typedef MatrixX PoseMatrixType;
  typedef MatrixX LandmarkMatrixType;
  typedef MatrixX PoseLandmarkMatrixType;
  typedef VectorX PoseVectorType;
  typedef VectorX LandmarkVectorType;

  typedef SparseBlockMatrix PoseHessianType;
  typedef SparseBlockMatrix LandmarkHessianType;
  typedef SparseBlockMatrix PoseLandmarkHessianType;
  typedef LinearSolver LinearSolverType;
};

接着是比较详细的BlockSolver类

template <typename Traits>
class BlockSolver: public BlockSolverBase
{
  public:
    static const int PoseDim = Traits::PoseDim;
    static const int LandmarkDim = Traits::LandmarkDim;
    typedef typename Traits::PoseMatrixType PoseMatrixType;
    typedef typename Traits::LandmarkMatrixType LandmarkMatrixType; 
    typedef typename Traits::PoseLandmarkMatrixType PoseLandmarkMatrixType;
    typedef typename Traits::PoseVectorType PoseVectorType;
    typedef typename Traits::LandmarkVectorType LandmarkVectorType;

    typedef typename Traits::PoseHessianType PoseHessianType;
    typedef typename Traits::LandmarkHessianType LandmarkHessianType;
    typedef typename Traits::PoseLandmarkHessianType PoseLandmarkHessianType;
    typedef typename Traits::LinearSolverType LinearSolverType;

  public:

    /**
     * allocate a block solver ontop of the underlying linear solver.
     * NOTE: The BlockSolver assumes exclusive access to the linear solver and will therefore free the pointer
     * in its destructor.
     */
    BlockSolver(std::unique_ptr linearSolver);
    ~BlockSolver();

    virtual bool init(SparseOptimizer* optmizer, bool online = false);
    virtual bool buildStructure(bool zeroBlocks = false);
    virtual bool updateStructure(const std::vector& vset, const HyperGraph::EdgeSet& edges);
    virtual bool buildSystem();
    virtual bool solve();
    virtual bool computeMarginals(SparseBlockMatrix& spinv, const std::vector<std::pair<int, int> >& blockIndices);
    virtual bool setLambda(number_t lambda, bool backup = false);
    virtual void restoreDiagonal();
    virtual bool supportsSchur() {return true;}
    virtual bool schur() { return _doSchur;}
    virtual void setSchur(bool s) { _doSchur = s;}

    LinearSolver& linearSolver() const { return *_linearSolver;}

    virtual void setWriteDebug(bool writeDebug);
    virtual bool writeDebug() const {return _linearSolver->writeDebug();}

    virtual bool saveHessian(const std::string& fileName) const;

    virtual void multiplyHessian(number_t* dest, const number_t* src) const { _Hpp->multiplySymmetricUpperTriangle(dest, src);}

  protected:
    void resize(int* blockPoseIndices, int numPoseBlocks, 
        int* blockLandmarkIndices, int numLandmarkBlocks, int totalDim);

    void deallocate();

    std::unique_ptr> _Hpp;
    std::unique_ptr> _Hll;
    std::unique_ptr> _Hpl;

    std::unique_ptr> _Hschur;
    std::unique_ptr> _DInvSchur;

    std::unique_ptr> _HplCCS;
    std::unique_ptr> _HschurTransposedCCS;

    std::unique_ptr _linearSolver;

    std::vector > _diagonalBackupPose;
    std::vector > _diagonalBackupLandmark;

#    ifdef G2O_OPENMP
    std::vector _coefficientsMutex;
#    endif

    bool _doSchur;

    std::unique_ptr> _coefficients;
    std::unique_ptr> _bschur;

    int _numPoses, _numLandmarks;
    int _sizePoses, _sizeLandmarks;
};

那么有了以上的信息，整个BlockSolver_P_L的作用也就比较清楚了，还是那句老话：P表示的是Pose的维度（注意一定是流形manifold下的最小表示），L表示Landmark的维度（这里就不涉及流行什么事儿了）。这里思考一个问题，假如说在某个应用下，我们的P和L在程序开始并不能确定（例如程序中既有映射关系的边，同时还有位姿图之间的边，这时候的 Hpp H p p 矩阵将不是那么的稀疏），那么此时这个块状求解器如何定义呢？其实也比较简单，g2o已经帮我们定义了一个不定的BlockSolverTraits，所有的参数都在中间过程中被确定，那么为什么g2o还帮助我们定义了一些常用的类型呢？一种可能是出于节约初始化时间的角度出发的，但是个人估计不是很靠谱，毕竟C++申请出一块内存应该还是很快的，没有必要为了这点儿时间纠结；那么另一种也许就是作者想把常用的类型定义出来而已吧～

---

优化器（optimizer）最初做的事（initializeOptimization）

想必这句话大家应该也经常用到

optimizer.initializeOptimization();

别看他若不经风的，其实在整个程序中的用处还真的很大（这不是废话嘛，哪个函数的用处都很大好吗），下面就仔细的说一下这个函数里面都干了什么：
1. 把所有的顶点（Vertex）插入到vset的集合中（妈妈在也不用担心我插入了相同的定点）
2. 遍历vset集合，取出每个顶点的边（这里每个边都有一个level的概念，默认情况下，g2o只处理level=0的边，在orbslam中，如果确定某个边的重投影误差过大，则把level设置为1，也就是舍弃这个边对于整个优化的影响），并判断边所连接的顶点是否都是有效的（在vset中），如果是，则认为这是一个有效的边和顶点，并分别加入到_activeEdges和_activeVertices中（妈妈在也不用担心边少顶点或者图中没有边的顶点了）
3. 对上述的_activeEdges和_activeVertices按照ID号进行排序，其中Vertex的ID号是自己设置的，而Edge的ID号是g2o内部有个变量进行赋值的
4. 对于上述的_activeVertices，剔除掉固定点（fixed）之后，把所有的顶点按照**不被**margin在前，被margin在后的顺序排成vector类型，变量为_ivMap，这个变量很重要，基本上后面的所有程序都是用这个变量进行遍历的

以上就是初始化过程的全部，具体的代码如下：

bool SparseOptimizer::initializeOptimization(HyperGraph::VertexSet& vset, int level){
  if (edges().size() == 0) {
    cerr << __PRETTY_FUNCTION__ << ": Attempt to initialize an empty graph" << endl;
    return false;
  }
  preIteration(-1);
  bool workspaceAllocated = _jacobianWorkspace.allocate(); (void) workspaceAllocated;
  assert(workspaceAllocated && "Error while allocating memory for the Jacobians");
  clearIndexMapping();
  _activeVertices.clear();
  _activeVertices.reserve(vset.size());
  _activeEdges.clear();
  set auxEdgeSet; // temporary structure to avoid duplicates
  for (HyperGraph::VertexSet::iterator it=vset.begin(); it!=vset.end(); ++it){
    OptimizableGraph::Vertex* v= (OptimizableGraph::Vertex*) *it;
    const OptimizableGraph::EdgeSet& vEdges=v->edges();
    // count if there are edges in that level. If not remove from the pool
    int levelEdges=0;
    for (OptimizableGraph::EdgeSet::const_iterator it=vEdges.begin(); it!=vEdges.end(); ++it){
      OptimizableGraph::Edge* e=reinterpret_cast(*it);
      if (level < 0 || e->level() == level) {

        bool allVerticesOK = true;
        for (vector::const_iterator vit = e->vertices().begin(); vit != e->vertices().end(); ++vit) {
          if (vset.find(*vit) == vset.end()) {
            allVerticesOK = false;
            break;
          }
        }
        if (allVerticesOK && !e->allVerticesFixed()) {
          auxEdgeSet.insert(e);
          levelEdges++;
        }

      }
    }
    if (levelEdges){
      _activeVertices.push_back(v);

      // test for NANs in the current estimate if we are debugging
#      ifndef NDEBUG
      int estimateDim = v->estimateDimension();
      if (estimateDim > 0) {
        VectorX estimateData(estimateDim);
        if (v->getEstimateData(estimateData.data()) == true) {
          int k;
          bool hasNan = arrayHasNaN(estimateData.data(), estimateDim, &k);
          if (hasNan)
            cerr << __PRETTY_FUNCTION__ << ": Vertex " << v->id() << " contains a nan entry at index " << k << endl;
        }
      }
#      endif

    }
  }

  _activeEdges.reserve(auxEdgeSet.size());
  for (set::iterator it = auxEdgeSet.begin(); it != auxEdgeSet.end(); ++it)
    _activeEdges.push_back(*it);

  sortVectorContainers();
  bool indexMappingStatus = buildIndexMapping(_activeVertices);
  postIteration(-1);
  return indexMappingStatus;
}

---

优化器（Optimizer）的优化（optimize）

过了初始化之后，我们基本上就可以调用optimize函数进行图优化了，如果只看optimize函数的代码，十分简单，首先判断_ivMap是否为空，之后直接调用_algorithm的solve函数，最后打印一些信息，之后循环一定的次数，此时就大功告成。所以下面的内容主要是围绕着_algorithm的solve函数进行。

算法（algorithm）的solve函数

1. 在第一次进行迭代的时候，g2o要对整个矩阵块进行初始化，主要依靠块求解器的buildStructure()函数进行，那么想必大家也知道了，既然在块求解器中，那么必然就是对要用的矩阵块进行初始化，确实如此，我们稍微对这个函数进行展开：
   （1） 对_ivMap进行遍历，如果顶点的margin标志为false，则认为是Pose，否则，认为是Landmark，所以在使用g2o的时候千万要注意，不要想当然的认为g2o会帮助我们区分Pose和Landmark
   （2） 构建期待已久的 Hpp，Hll H p p ， H l l 等矩阵，每个矩阵的类型都是SparseBlockMatrix类型的
   （3） 开始对应环节，这个过程首先是遍历_ivMap，将 Hpp和Hll H p p 和 H l l 中对应于该顶点的矩阵块映射到顶点内部的_hessian矩阵中，这步的作用在我的上个博客中已经讲过了，感兴趣的可以看g2o学习——再看顶点和边，之后开始遍历所有的边，取出边的两个顶点，如果两个顶点都不margin的话，则在把 Hpp H p p 中的两个顶点相交的地方的内存映射给边的内部_hessian矩阵；如果两个中一个margin一个不margin的话，则把 Hpl H p l 中相交的地方的内存映射给边的_hessian矩阵；如果两个都margin的话，则把 Hll H l l 中相交的地方的内存映射给边的_hessian矩阵
   （4） 映射完成之后，如果我们使用schur消元的话，还要构建一个schur消元的中间矩阵，_Hschur，这个地方比较复杂，这里不做展开，对照公式看源码就可以
   这部分的源码给出：

template Traits>
bool BlockSolver<Traits>::buildStructure(bool zeroBlocks)
{
  assert(_optimizer);

  size_t sparseDim = 0;
  _numPoses=0;
  _numLandmarks=0;
  _sizePoses=0;
  _sizeLandmarks=0;
  int* blockPoseIndices = new int[_optimizer->indexMapping().size()];
  int* blockLandmarkIndices = new int[_optimizer->indexMapping().size()];

  for (size_t i = 0; i < _optimizer->indexMapping().size(); ++i) {
    OptimizableGraph::Vertex* v = _optimizer->indexMapping()[i];
    int dim = v->dimension();
    if (! v->marginalized()){
      v->setColInHessian(_sizePoses);
      _sizePoses+=dim;
      blockPoseIndices[_numPoses]=_sizePoses;
      ++_numPoses;
    } else {
      v->setColInHessian(_sizeLandmarks);
      _sizeLandmarks+=dim;
      blockLandmarkIndices[_numLandmarks]=_sizeLandmarks;
      ++_numLandmarks;
    }
    sparseDim += dim;
  }
  resize(blockPoseIndices, _numPoses, blockLandmarkIndices, _numLandmarks, sparseDim);
  delete[] blockLandmarkIndices;
  delete[] blockPoseIndices;

  // allocate the diagonal on Hpp and Hll
  int poseIdx = 0;
  int landmarkIdx = 0;
  for (size_t i = 0; i < _optimizer->indexMapping().size(); ++i) {
    OptimizableGraph::Vertex* v = _optimizer->indexMapping()[i];
    if (! v->marginalized()){
      //assert(poseIdx == v->hessianIndex());
      PoseMatrixType* m = _Hpp->block(poseIdx, poseIdx, true);
      if (zeroBlocks)
        m->setZero();
      v->mapHessianMemory(m->data());
      ++poseIdx;
    } else {
      LandmarkMatrixType* m = _Hll->block(landmarkIdx, landmarkIdx, true);
      if (zeroBlocks)
        m->setZero();
      v->mapHessianMemory(m->data());
      ++landmarkIdx;
    }
  }
  assert(poseIdx == _numPoses && landmarkIdx == _numLandmarks);

  // temporary structures for building the pattern of the Schur complement
  SparseBlockMatrixHashMap<PoseMatrixType>* schurMatrixLookup = 0;
  if (_doSchur) {
    schurMatrixLookup = new SparseBlockMatrixHashMap<PoseMatrixType>(_Hschur->rowBlockIndices(), _Hschur->colBlockIndices());
    schurMatrixLookup->blockCols().resize(_Hschur->blockCols().size());
  }

  // here we assume that the landmark indices start after the pose ones
  // create the structure in Hpp, Hll and in Hpl
  for (SparseOptimizer::EdgeContainer::const_iterator it=_optimizer->activeEdges().begin(); it!=_optimizer->activeEdges().end(); ++it){
    OptimizableGraph::Edge* e = *it;

    for (size_t viIdx = 0; viIdx < e->vertices().size(); ++viIdx) {
      OptimizableGraph::Vertex* v1 = (OptimizableGraph::Vertex*) e->vertex(viIdx);
      int ind1 = v1->hessianIndex();
      if (ind1 == -1)
        continue;
      int indexV1Bak = ind1;
      for (size_t vjIdx = viIdx + 1; vjIdx < e->vertices().size(); ++vjIdx) {
        OptimizableGraph::Vertex* v2 = (OptimizableGraph::Vertex*) e->vertex(vjIdx);
        int ind2 = v2->hessianIndex();
        if (ind2 == -1)
          continue;
        ind1 = indexV1Bak;
        bool transposedBlock = ind1 > ind2;
        if (transposedBlock){ // make sure, we allocate the upper triangle block
          std::swap(ind1, ind2);
        }
        if (! v1->marginalized() && !v2->marginalized()){
          PoseMatrixType* m = _Hpp->block(ind1, ind2, true);
          if (zeroBlocks)
            m->setZero();
          e->mapHessianMemory(m->data(), viIdx, vjIdx, transposedBlock);
          if (_Hschur) {// assume this is only needed in case we solve with the schur complement
            schurMatrixLookup->addBlock(ind1, ind2);
          }
        } else if (v1->marginalized() && v2->marginalized()){
          // RAINER hmm.... should we ever reach this here????
          LandmarkMatrixType* m = _Hll->block(ind1-_numPoses, ind2-_numPoses, true);
          if (zeroBlocks)
            m->setZero();
          e->mapHessianMemory(m->data(), viIdx, vjIdx, false);
        } else { 
          if (v1->marginalized()){ 
            PoseLandmarkMatrixType* m = _Hpl->block(v2->hessianIndex(),v1->hessianIndex()-_numPoses, true);
            if (zeroBlocks)
              m->setZero();
            e->mapHessianMemory(m->data(), viIdx, vjIdx, true); // transpose the block before writing to it
          } else {
            PoseLandmarkMatrixType* m = _Hpl->block(v1->hessianIndex(),v2->hessianIndex()-_numPoses, true);
            if (zeroBlocks)
              m->setZero();
            e->mapHessianMemory(m->data(), viIdx, vjIdx, false); // directly the block
          }
        }
      }
    }
  }

  if (! _doSchur) {
    delete schurMatrixLookup;
    return true;
  }

  _DInvSchur->diagonal().resize(landmarkIdx);
  _Hpl->fillSparseBlockMatrixCCS(*_HplCCS);

  for (OptimizableGraph::Vertex* v : _optimizer->indexMapping()) {
    if (v->marginalized()){
      const HyperGraph::EdgeSet& vedges=v->edges();
      for (HyperGraph::EdgeSet::const_iterator it1=vedges.begin(); it1!=vedges.end(); ++it1){
        for (size_t i=0; i<(*it1)->vertices().size(); ++i)
        {
          OptimizableGraph::Vertex* v1= (OptimizableGraph::Vertex*) (*it1)->vertex(i);
          if (v1->hessianIndex()==-1 || v1==v)
            continue;
          for  (HyperGraph::EdgeSet::const_iterator it2=vedges.begin(); it2!=vedges.end(); ++it2){
            for (size_t j=0; j<(*it2)->vertices().size(); ++j)
            {
              OptimizableGraph::Vertex* v2= (OptimizableGraph::Vertex*) (*it2)->vertex(j);
              if (v2->hessianIndex()==-1 || v2==v)
                continue;
              int i1=v1->hessianIndex();
              int i2=v2->hessianIndex();
              if (i1<=i2) {
                schurMatrixLookup->addBlock(i1, i2);
              }
            }
          }
        }
      }
    }
  }

  _Hschur->takePatternFromHash(*schurMatrixLookup);
  delete schurMatrixLookup;
  _Hschur->fillSparseBlockMatrixCCSTransposed(*_HschurTransposedCCS);

  return true;
}

1. 上述块矩阵以及映射关系对应好以后，接下来比较重要的函数就是solver的buildSystem函数，这个函数的主要功能就是将增量方程（ HΔx=−b H Δ x = − b ）中的H矩阵和b向量赋予该有的值，具体过程如下：
   （1） 遍历所有的边，计算该边误差所产生的jacobian矩阵
   （2）根据公式 H=JTWJ H = J T W J 计算每个顶点的_hessian矩阵， b=JTWδ b = J T W δ 计算b向量，如果该边是个二元边，且两个定点都没有被fix的时候，还会根据 H=JT1WJ2 H = J 1 T W J 2 计算相交部分的_hessian矩阵。
   （3） 由于顶点内部类型b并不是映射，因此最后还要遍历所有的顶点的b并将其真值拷贝到增量方程的b内
   这部分的代码如下：

template <typename Traits>
bool BlockSolver::buildSystem()
{
  // clear b vector
# ifdef G2O_OPENMP
# pragma omp parallel for default (shared) if (_optimizer->indexMapping().size() > 1000)
# endif
  for (int i = 0; i < static_cast<int>(_optimizer->indexMapping().size()); ++i) {
    OptimizableGraph::Vertex* v=_optimizer->indexMapping()[i];
    assert(v);
    v->clearQuadraticForm();
  }
  _Hpp->clear();
  if (_doSchur) {
    _Hll->clear();
    _Hpl->clear();
  }

  // resetting the terms for the pairwise constraints
  // built up the current system by storing the Hessian blocks in the edges and vertices
# ifndef G2O_OPENMP
  // no threading, we do not need to copy the workspace
  JacobianWorkspace& jacobianWorkspace = _optimizer->jacobianWorkspace();
# else
  // if running with threads need to produce copies of the workspace for each thread
  JacobianWorkspace jacobianWorkspace = _optimizer->jacobianWorkspace();
# pragma omp parallel for default (shared) firstprivate(jacobianWorkspace) if (_optimizer->activeEdges().size() > 100)
# endif
  for (int k = 0; k < static_cast<int>(_optimizer->activeEdges().size()); ++k) {
    OptimizableGraph::Edge* e = _optimizer->activeEdges()[k];
    e->linearizeOplus(jacobianWorkspace); // jacobian of the nodes' oplus (manifold)
    e->constructQuadraticForm();
#  ifndef NDEBUG
    for (size_t i = 0; i < e->vertices().size(); ++i) {
      const OptimizableGraph::Vertex* v = static_cast<const OptimizableGraph::Vertex*>(e->vertex(i));
      if (! v->fixed()) {
        bool hasANan = arrayHasNaN(jacobianWorkspace.workspaceForVertex(i), e->dimension() * v->dimension());
        if (hasANan) {
          std::cerr << "buildSystem(): NaN within Jacobian for edge " << e << " for vertex " << i << std::endl;
          break;
        }
      }
    }
#  endif
  }

  // flush the current system in a sparse block matrix
# ifdef G2O_OPENMP
# pragma omp parallel for default (shared) if (_optimizer->indexMapping().size() > 1000)
# endif
  for (int i = 0; i < static_cast<int>(_optimizer->indexMapping().size()); ++i) {
    OptimizableGraph::Vertex* v=_optimizer->indexMapping()[i];
    int iBase = v->colInHessian();
    if (v->marginalized())
      iBase+=_sizePoses;
    v->copyB(_b+iBase);
  }

  return 0;
}

1. 上述完成整个矩阵块的构建之后，接下来就是激动人心的求解线性方程的时候了，主要依靠块求解器BlockSolver的solve函数，下面是过程：
   （1） 判断是否要做schur消元，如果不做的话，则认为整个图中没有要margin的点，因此直接求解 HppΔx=−b H p p Δ x = − b 就可以了；
   （2） 如果要进行schur消元，则在后面构建出schur需要的矩阵和向量，由于这部分内容比较大，这里就不做展开，感兴趣的可以看SLAM中的marginalization 和 Schur complement，里面讲的也比较清楚
   该部分源码如下：

template <typename Traits>
bool BlockSolver::solve(){
  //cerr << __PRETTY_FUNCTION__ << endl;
  if (! _doSchur){
    number_t t=get_monotonic_time();
    bool ok = _linearSolver->solve(*_Hpp, _x, _b);
    G2OBatchStatistics* globalStats = G2OBatchStatistics::globalStats();
    if (globalStats) {
      globalStats->timeLinearSolver = get_monotonic_time() - t;
      globalStats->hessianDimension = globalStats->hessianPoseDimension = _Hpp->cols();
    }
    return ok;
  }

  // schur thing

  // backup the coefficient matrix
  number_t t=get_monotonic_time();

  // _Hschur = _Hpp, but keeping the pattern of _Hschur
  _Hschur->clear();
  _Hpp->add(*_Hschur);

  //_DInvSchur->clear();
  memset(_coefficients.get(), 0, _sizePoses*sizeof(number_t));
# ifdef G2O_OPENMP
# pragma omp parallel for default (shared) schedule(dynamic, 10)
# endif
  for (int landmarkIndex = 0; landmarkIndex < static_cast<int>(_Hll->blockCols().size()); ++landmarkIndex) {
    const typename SparseBlockMatrix::IntBlockMap& marginalizeColumn = _Hll->blockCols()[landmarkIndex];
    assert(marginalizeColumn.size() == 1 && "more than one block in _Hll column");

    // calculate inverse block for the landmark
    const LandmarkMatrixType * D = marginalizeColumn.begin()->second;
    assert (D && D->rows()==D->cols() && "Error in landmark matrix");
    LandmarkMatrixType& Dinv = _DInvSchur->diagonal()[landmarkIndex];
    Dinv = D->inverse();

    LandmarkVectorType  db(D->rows());
    for (int j=0; jrows(); ++j) {
      db[j]=_b[_Hll->rowBaseOfBlock(landmarkIndex) + _sizePoses + j];
    }
    db=Dinv*db;

    assert((size_t)landmarkIndex < _HplCCS->blockCols().size() && "Index out of bounds");
    const typename SparseBlockMatrixCCS::SparseColumn& landmarkColumn = _HplCCS->blockCols()[landmarkIndex];

    for (typename SparseBlockMatrixCCS::SparseColumn::const_iterator it_outer = landmarkColumn.begin();
        it_outer != landmarkColumn.end(); ++it_outer) {
      int i1 = it_outer->row;

      const PoseLandmarkMatrixType* Bi = it_outer->block;
      assert(Bi);

      PoseLandmarkMatrixType BDinv = (*Bi)*(Dinv);
      assert(_HplCCS->rowBaseOfBlock(i1) < _sizePoses && "Index out of bounds");
      typename PoseVectorType::MapType Bb(&_coefficients[_HplCCS->rowBaseOfBlock(i1)], Bi->rows());
#    ifdef G2O_OPENMP
      ScopedOpenMPMutex mutexLock(&_coefficientsMutex[i1]);
#    endif
      Bb.noalias() += (*Bi)*db;

      assert(i1 >= 0 && i1 < static_cast<int>(_HschurTransposedCCS->blockCols().size()) && "Index out of bounds");
      typename SparseBlockMatrixCCS::SparseColumn::iterator targetColumnIt = _HschurTransposedCCS->blockCols()[i1].begin();

      typename SparseBlockMatrixCCS::RowBlock aux(i1, 0);
      typename SparseBlockMatrixCCS::SparseColumn::const_iterator it_inner = lower_bound(landmarkColumn.begin(), landmarkColumn.end(), aux);
      for (; it_inner != landmarkColumn.end(); ++it_inner) {
        int i2 = it_inner->row;
        const PoseLandmarkMatrixType* Bj = it_inner->block;
        assert(Bj); 
        while (targetColumnIt->row < i2 /*&& targetColumnIt != _HschurTransposedCCS->blockCols()[i1].end()*/)
          ++targetColumnIt;
        assert(targetColumnIt != _HschurTransposedCCS->blockCols()[i1].end() && targetColumnIt->row == i2 && "invalid iterator, something wrong with the matrix structure");
        PoseMatrixType* Hi1i2 = targetColumnIt->block;//_Hschur->block(i1,i2);
        assert(Hi1i2);
        (*Hi1i2).noalias() -= BDinv*Bj->transpose();
      }
    }
  }
  //cerr << "Solve [marginalize] = " <<  get_monotonic_time()-t << endl;

  // _bschur = _b for calling solver, and not touching _b
  memcpy(_bschur.get(), _b, _sizePoses * sizeof(number_t));
  for (int i=0; i<_sizePoses; ++i){
    _bschur[i]-=_coefficients[i];
  }

  G2OBatchStatistics* globalStats = G2OBatchStatistics::globalStats();
  if (globalStats){
    globalStats->timeSchurComplement = get_monotonic_time() - t;
  }

  t=get_monotonic_time();
  bool solvedPoses = _linearSolver->solve(*_Hschur, _x, _bschur.get());
  if (globalStats) {
    globalStats->timeLinearSolver = get_monotonic_time() - t;
    globalStats->hessianPoseDimension = _Hpp->cols();
    globalStats->hessianLandmarkDimension = _Hll->cols();
    globalStats->hessianDimension = globalStats->hessianPoseDimension + globalStats->hessianLandmarkDimension;
  }
  //cerr << "Solve [decompose and solve] = " <<  get_monotonic_time()-t << endl;

  if (! solvedPoses)
    return false;

  // _x contains the solution for the poses, now applying it to the landmarks to get the new part of the
  // solution;
  number_t* xp = _x;
  number_t* cp = _coefficients.get();

  number_t* xl=_x+_sizePoses;
  number_t* cl=_coefficients.get() + _sizePoses;
  number_t* bl=_b+_sizePoses;

  // cp = -xp
  for (int i=0; i<_sizePoses; ++i)
    cp[i]=-xp[i];

  // cl = bl
  memcpy(cl,bl,_sizeLandmarks*sizeof(number_t));

  // cl = bl - Bt * xp
  //Bt->multiply(cl, cp);
  _HplCCS->rightMultiply(cl, cp);

  // xl = Dinv * cl
  memset(xl,0, _sizeLandmarks*sizeof(number_t));
  _DInvSchur->multiply(xl,cl);
  //_DInvSchur->rightMultiply(xl,cl);
  //cerr << "Solve [landmark delta] = " <<  get_monotonic_time()-t << endl;

  return true;
}

1. 最后就是把得到的 Δx Δ x 对应的调用顶点中的oplusImpl函数对状态变量进行更新，这部分就不细讲啦～

---

总结

以上就是整个g2o在私底下为我们做的事情了，当然其中很多细节这里没有展现出来，本来看这部分源码的意图也是为了验证过程是否和自己想象的一样，最后的感觉是从数学上来讲大致上是相同的，但是要是从程序上觉得还是受益匪浅，作者的很多编程方式和方法还是很值得学习的。这部分内容就先更新到这里，如果以后有更新的发现也会及时的记录下来，一方面是帮助自己总结，一方面希望能帮助刚入门的同学。

以上观点仅仅代表个人学习时候的观点，如有不对的地方还请各位大神指正！这里不胜感激！