G2O Learning--solver of vertices and edges

write in front

Recently learned G2O program, followed by routines to do a few programs, in fact, most of them to note is the vertex and edge of some things, this blog is designed to record those not seen in the process, that is, g2o help us do what things, the main reference is the following site:
Http://docs.ros.org/fuerte/api/re_vision/html/namespaceg2o.html
This site has a more comprehensive g2o of the class and function of the explanation, very convenient.
Then here is a relatively superficial summary, and not very deep, is generally recorded g2o in the internal process of solving the problem. block Solver (blocksolver_p_l)

This place I mentioned once in the previous blog, here again to mention, the first to sacrifice his source code:

Template<int p, int l>
using BLOCKSOLVERPL = blocksolver< blocksolvertraits<p, l> >;

Then go up and we'll see a more detailed Blocksovertraits class

Template <int _posedim, int _landmarkdim> struct blocksolvertraits {static const int posedim = _posedim;
  static const int Landmarkdim = _landmarkdim;
  typedef eigen::matrix<number_t, Posedim, Posedim, eigen::colmajor> Posematrixtype;
  typedef eigen::matrix<number_t, Landmarkdim, Landmarkdim, eigen::colmajor> Landmarkmatrixtype;
  typedef eigen::matrix<number_t, Posedim, Landmarkdim, eigen::colmajor> Poselandmarkmatrixtype;
  typedef eigen::matrix<number_t, Posedim, 1, eigen::colmajor> Posevectortype;

  typedef eigen::matrix<number_t, Landmarkdim, 1, eigen::colmajor> Landmarkvectortype;
  typedef sparseblockmatrix<posematrixtype> POSEHESSIANTYPE;
  typedef sparseblockmatrix<landmarkmatrixtype> LANDMARKHESSIANTYPE;
  typedef sparseblockmatrix<poselandmarkmatrixtype> POSELANDMARKHESSIANTYPE;
typedef linearsolver<posematrixtype> LINEARSOLVERTYPE;

}; /** * \brief traits to summarize the properties of the dynamic size opTimization problem */template <> struct Blocksolvertraits<eigen::D ynamic, Eigen::D ynamic> {static const I
  NT Posedim = Eigen::D ynamic;
  static const int Landmarkdim = Eigen::D ynamic;
  typedef MatrixX POSEMATRIXTYPE;
  typedef MatrixX LANDMARKMATRIXTYPE;
  typedef MatrixX POSELANDMARKMATRIXTYPE;
  typedef Vectorx POSEVECTORTYPE;

  typedef Vectorx LANDMARKVECTORTYPE;
  typedef sparseblockmatrix<posematrixtype> POSEHESSIANTYPE;
  typedef sparseblockmatrix<landmarkmatrixtype> LANDMARKHESSIANTYPE;
  typedef sparseblockmatrix<poselandmarkmatrixtype> POSELANDMARKHESSIANTYPE;
typedef linearsolver<posematrixtype> LINEARSOLVERTYPE; };

followed by a more detailed Blocksolver class

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<LinearSolverType> linearSolver);
    ~BlockSolver();

    virtual bool init(SparseOptimizer* optmizer, bool online = false);
    virtual bool buildStructure(bool zeroBlocks = false);
    virtual bool updateStructure(const std::vector<HyperGraph::Vertex*>& vset, const HyperGraph::EdgeSet& edges);
    virtual bool buildSystem();
    virtual bool solve();
    virtual bool computeMarginals(SparseBlockMatrix<MatrixX>& 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<PoseMatrixType>& 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<SparseBlockMatrix<PoseMatrixType>> _Hpp;
    std::unique_ptr<SparseBlockMatrix<LandmarkMatrixType>> _Hll;
    std::unique_ptr<SparseBlockMatrix<PoseLandmarkMatrixType>> _Hpl;

    std::unique_ptr<SparseBlockMatrix<PoseMatrixType>> _Hschur;
    std::unique_ptr<SparseBlockMatrixDiagonal<LandmarkMatrixType>> _DInvSchur;

    std::unique_ptr<SparseBlockMatrixCCS<PoseLandmarkMatrixType>> _HplCCS;
    std::unique_ptr<SparseBlockMatrixCCS<PoseMatrixType>> _HschurTransposedCCS;

    std::unique_ptr<LinearSolverType> _linearSolver;

    std::vector<PoseVectorType, Eigen::aligned_allocator<PoseVectorType> > _diagonalBackupPose;
    std::vector<LandmarkVectorType, Eigen::aligned_allocator<LandmarkVectorType> > _diagonalBackupLandmark;

#    ifdef G2O_OPENMP
    std::vector<OpenMPMutex> _coefficientsMutex;
#    endif

    bool _doSchur;

    std::unique_ptr<number_t[], aligned_deleter<number_t>> _coefficients;
    std::unique_ptr<number_t[], aligned_deleter<number_t>> _bschur;

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

Then with the above information, the role of the entire BlockSolver_P_L is clearer, or the old saying: P represents the dimension of Pose (note that it must be the smallest representation under the manifold), L represents the dimension of Landmark (here It doesn't involve anything popular.) Think about a problem here. If we say that in an application, our P and L are not certain at the beginning of the program (for example, the edges of the mapping relationship in the program, as well as the edges between the poses, Hpp at this time) The H pp H_{pp} matrix will not be so sparse), then how is this block solver defined? In fact, it is relatively simple, g2o has helped us define an indefinite BlockSolverTraits, all parameters are determined in the middle process, so why g2o also helps us define some commonly used types. One possibility is to save the initialization time, but the personal estimate is not very reliable. After all, C++ should apply for a piece of memory should be very fast, there is no need to entangle this time; then another may be the author I want to define the commonly used types. - What the optimizer did initially (initializeOptimization)

Presumably this sentence should be used frequently by everyone.

optimizer.initializeOptimization();

Don't look at him if he doesn't go through the wind, in fact, the use of the whole program is really very big (this is not nonsense, which function is very useful), let's take a closer look at what this function does. :
1. Insert all the vertices (Vertex) into the collection of vsets (Mom doesn't have to worry about inserting the same fixed point)
2. Traverse the vset collection and take the edges of each vertex (here each side has a concept of level. By default, g2o only processes the edge with level=0. In orbslam, if you determine the reprojection error of an edge Too large, set the level to 1, which is to discard the effect of this edge on the overall optimization), and determine whether the vertex connected to the edge is valid (in vset), and if so, consider this to be a valid Edges and vertices, and added to _activeEdges and _activeVertices respectively (Mom doesn't have to worry about having fewer vertices or vertices with no edges in the graph)
3. Sort the above _activeEdges and _activeVertices according to the ID number, where the ID number of Vertex is set by itself, and the ID number of Edge is a variable inside g2o for assignment.
4. For the above _activeVertices, after removing the fixed point, put all the vertices according to ** not by **margin, and sort them into vector by the order of the margin, the variable is _ivMap, this variable Very important, basically all the programs behind are traversed with this variable.

The above is all the initialization process, the specific code is as follows:

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<Edge*> 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<OptimizableGraph::Edge*>(*it);
      if (level < 0 || e->level() == level) {

        bool allVerticesOK = true;
        for (vector<HyperGraph::Vertex*>::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<Edge*>::iterator it = auxEdgeSet.begin(); it != auxEdgeSet.end(); ++it)
    _activeEdges.push_back(*it);

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

Optimization of the optimizer (optimizer)
After the initialization, we can basically call the optimize function to optimize the graph. If you only look at the code of the optimize function, it is very simple. First, judge whether the _ivMap is empty. Then directly call the solve function of _algorithm, and finally print some information. Then, after a certain number of cycles, you are done. So the following content is mainly around the solver function of _algorithm. When the solver function of the algorithm is iterated for the first time, g2o needs to initialize the entire matrix block, mainly relying on the block solver's buildStructure() function, so I think everyone knows it, since it is in the block solver. , then it must be the initialization of the matrix block to be used, indeed, we expand this function slightly:
(1) Traversing _ivMap, if the margin flag of the vertex is false, it is considered to be Pose, otherwise, it is considered to be Landmark, so be careful when using g2o, don't take it for granted that g2o will help us distinguish Pose and Landmark
(2) Construct the long-awaited matrix of Hpp, Hll H p p , H l l H_{pp}, H_{ll}, each of which is of type SparseBlockMatrix
(3) Start the corresponding link. This process first traverses _ivMap, and maps the matrix blocks corresponding to the vertex in Hpp and Hll H pp and H ll H_{pp} and H_{ll} into the _hessian matrix inside the vertex. The role of this step has already been mentioned in my last blog. Interested can see g2o learning - look at the vertices and edges, then start traversing all the edges, take the two vertices of the edge, if two vertices If it is not margin, then the memory in the place where the two vertices in Hpp H pp H_{pp} intersect is mapped to the inner _hessian matrix of the edge; if one of the two margins is not margin, then