g2o::BlockSolver< Traits > Class Template Reference

Implementation of a solver operating on the blocks of the Hessian. More...

#include <block_solver.h>

Inheritance diagram for g2o::BlockSolver< Traits >:

Collaboration diagram for g2o::BlockSolver< Traits >:

Public Types
typedef Traits::PoseMatrixType	PoseMatrixType
typedef Traits::LandmarkMatrixType	LandmarkMatrixType
typedef Traits::PoseLandmarkMatrixType	PoseLandmarkMatrixType
typedef Traits::PoseVectorType	PoseVectorType
typedef Traits::LandmarkVectorType	LandmarkVectorType
typedef Traits::PoseHessianType	PoseHessianType
typedef Traits::LandmarkHessianType	LandmarkHessianType
typedef Traits::PoseLandmarkHessianType	PoseLandmarkHessianType
typedef Traits::LinearSolverType	LinearSolverType

Public Member Functions
	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 (double lambda, bool backup=false)
virtual void	restoreDiagonal ()
virtual bool	supportsSchur ()
virtual bool	schur ()
	should the solver perform the schur complement or not
virtual void	setSchur (bool s)
LinearSolver< PoseMatrixType > &	linearSolver () const
virtual void	setWriteDebug (bool writeDebug)
virtual bool	writeDebug () const
virtual bool	saveHessian (const std::string &fileName) const
	write the hessian to disk using the specified file name
virtual void	multiplyHessian (double *dest, const double *src) const
Public Member Functions inherited from g2o::BlockSolverBase
virtual	~BlockSolverBase ()
Public Member Functions inherited from g2o::Solver
	Solver ()
virtual	~Solver ()
double *	x ()
	return x, the solution vector
const double *	x () const
double *	b ()
	return b, the right hand side of the system
const double *	b () const
size_t	vectorSize () const
	return the size of the solution vector (x) and b
SparseOptimizer *	optimizer () const
	the optimizer (graph) on which the solver works
void	setOptimizer (SparseOptimizer *optimizer)
bool	levenberg () const
	the system is Levenberg-Marquardt
void	setLevenberg (bool levenberg)
size_t	additionalVectorSpace () const
void	setAdditionalVectorSpace (size_t s)

Static Public Attributes
static const int	PoseDim = Traits::PoseDim
static const int	LandmarkDim = Traits::LandmarkDim

Protected Member Functions
void	resize (int *blockPoseIndices, int numPoseBlocks, int *blockLandmarkIndices, int numLandmarkBlocks, int totalDim)
void	deallocate ()
Protected Member Functions inherited from g2o::Solver
void	resizeVector (size_t sx)

Protected Attributes
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 >	_diagonalBackupPose
std::vector< LandmarkVectorType >	_diagonalBackupLandmark
bool	_doSchur
std::unique_ptr< double[], aligned_deleter< double > >	_coefficients
std::unique_ptr< double[], aligned_deleter< double > >	_bschur
int	_numPoses
int	_numLandmarks
int	_sizePoses
int	_sizeLandmarks
Protected Attributes inherited from g2o::Solver
SparseOptimizer *	_optimizer
double *	_x
double *	_b
size_t	_xSize
size_t	_maxXSize
bool	_isLevenberg
size_t	_additionalVectorSpace

Detailed Description

template<typename Traits>
class g2o::BlockSolver< Traits >

Implementation of a solver operating on the blocks of the Hessian.

Definition at line 103 of file block_solver.h.

Member Typedef Documentation

LandmarkHessianType

template<typename Traits >

typedef Traits::LandmarkHessianType g2o::BlockSolver< Traits >::LandmarkHessianType

Definition at line 114 of file block_solver.h.

LandmarkMatrixType

template<typename Traits >

typedef Traits::LandmarkMatrixType g2o::BlockSolver< Traits >::LandmarkMatrixType

Definition at line 108 of file block_solver.h.

LandmarkVectorType

template<typename Traits >

typedef Traits::LandmarkVectorType g2o::BlockSolver< Traits >::LandmarkVectorType

Definition at line 111 of file block_solver.h.

LinearSolverType

template<typename Traits >

typedef Traits::LinearSolverType g2o::BlockSolver< Traits >::LinearSolverType

Definition at line 116 of file block_solver.h.

PoseHessianType

template<typename Traits >

typedef Traits::PoseHessianType g2o::BlockSolver< Traits >::PoseHessianType

Definition at line 113 of file block_solver.h.

PoseLandmarkHessianType

template<typename Traits >

typedef Traits::PoseLandmarkHessianType g2o::BlockSolver< Traits >::PoseLandmarkHessianType

Definition at line 115 of file block_solver.h.

PoseLandmarkMatrixType

template<typename Traits >

typedef Traits::PoseLandmarkMatrixType g2o::BlockSolver< Traits >::PoseLandmarkMatrixType

Definition at line 109 of file block_solver.h.

PoseMatrixType

template<typename Traits >

typedef Traits::PoseMatrixType g2o::BlockSolver< Traits >::PoseMatrixType

Definition at line 107 of file block_solver.h.

PoseVectorType

template<typename Traits >

typedef Traits::PoseVectorType g2o::BlockSolver< Traits >::PoseVectorType

Definition at line 110 of file block_solver.h.

Constructor & Destructor Documentation

BlockSolver()

template<typename Traits >

g2o::BlockSolver< Traits >::BlockSolver

(

std::unique_ptr< LinearSolverType >

linearSolver

)

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.

Definition at line 41 of file block_solver.hpp.

    : BlockSolverBase(), _linearSolver(std::move(linearSolver)) {
  // workspace
  _xSize = 0;
  _numPoses = 0;
  _numLandmarks = 0;
  _sizePoses = 0;
  _sizeLandmarks = 0;
  _doSchur = true;
}

References g2o::BlockSolver< Traits >::_doSchur, g2o::BlockSolver< Traits >::_numLandmarks, g2o::BlockSolver< Traits >::_numPoses, g2o::BlockSolver< Traits >::_sizeLandmarks, g2o::BlockSolver< Traits >::_sizePoses, and g2o::Solver::_xSize.

~BlockSolver()

template<typename Traits >

g2o::BlockSolver< Traits >::~BlockSolver

(

)

Definition at line 107 of file block_solver.hpp.

{}

Member Function Documentation

buildStructure()

template<typename Traits >

bool g2o::BlockSolver< Traits >::buildStructure ( bool  zeroBlocks = false )

virtual

build the structure of the system

Implements g2o::Solver.

Definition at line 110 of file block_solver.hpp.

                                                        {
  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;
}

References g2o::SparseBlockMatrixHashMap< MatrixType >::addBlock(), g2o::SparseBlockMatrixHashMap< MatrixType >::blockCols(), g2o::OptimizableGraph::Vertex::dimension(), g2o::OptimizableGraph::Vertex::hessianIndex(), g2o::OptimizableGraph::Vertex::mapHessianMemory(), g2o::OptimizableGraph::Edge::mapHessianMemory(), g2o::OptimizableGraph::Vertex::marginalized(), g2o::OptimizableGraph::Vertex::setColInHessian(), g2o::HyperGraph::Edge::vertex(), and g2o::HyperGraph::Edge::vertices().

buildSystem()

template<typename Traits >

bool g2o::BlockSolver< Traits >::buildSystem ( )

virtual

build the current system

Implements g2o::Solver.

Definition at line 492 of file block_solver.hpp.

                                      {
  // 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) {
          G2O_WARN(
              "buildSystem(): NaN within Jacobian for edge {} for vertex {}",
              static_cast<void*>(e), i);
          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 false;
}

References g2o::arrayHasNaN(), g2o::OptimizableGraph::Vertex::clearQuadraticForm(), g2o::OptimizableGraph::Vertex::colInHessian(), g2o::OptimizableGraph::Edge::constructQuadraticForm(), g2o::OptimizableGraph::Vertex::copyB(), g2o::OptimizableGraph::Vertex::dimension(), g2o::OptimizableGraph::Edge::dimension(), g2o::OptimizableGraph::Vertex::fixed(), G2O_WARN, g2o::OptimizableGraph::Edge::linearizeOplus(), g2o::OptimizableGraph::Vertex::marginalized(), g2o::HyperGraph::Edge::vertex(), g2o::HyperGraph::Edge::vertices(), and g2o::JacobianWorkspace::workspaceForVertex().

computeMarginals()

template<typename Traits >

bool g2o::BlockSolver< Traits >::computeMarginals ( SparseBlockMatrix< MatrixX > &  spinv, const std::vector< std::pair< int, int > > &  blockIndices  )

virtual

computes the block diagonal elements of the pattern specified in the input and stores them in given SparseBlockMatrix

Implements g2o::Solver.

Definition at line 479 of file block_solver.hpp.

                                                      {
  double t = get_monotonic_time();
  bool ok = _linearSolver->solvePattern(spinv, blockIndices, *_Hpp);
  G2OBatchStatistics* globalStats = G2OBatchStatistics::globalStats();
  if (globalStats) {
    globalStats->timeMarginals = get_monotonic_time() - t;
  }
  return ok;
}

References g2o::get_monotonic_time(), g2o::G2OBatchStatistics::globalStats(), and g2o::G2OBatchStatistics::timeMarginals.

deallocate()

template<typename Traits >

void g2o::BlockSolver< Traits >::deallocate ( )

protected

Definition at line 93 of file block_solver.hpp.

                                     {
  _Hpp.reset();
  _Hll.reset();
  _Hpl.reset();
  _Hschur.reset();
  _DInvSchur.reset();
  _coefficients.reset();
  _bschur.reset();
 
  _HplCCS.reset();
  _HschurTransposedCCS.reset();
}

init()

template<typename Traits >

bool g2o::BlockSolver< Traits >::init ( SparseOptimizer *  optimizer, bool  online = false  )

virtual

initialize the solver, called once before the first iteration

Implements g2o::Solver.

Definition at line 604 of file block_solver.hpp.

                                                                      {
  _optimizer = optimizer;
  if (!online) {
    if (_Hpp) _Hpp->clear();
    if (_Hpl) _Hpl->clear();
    if (_Hll) _Hll->clear();
  }
  _linearSolver->init();
  return true;
}

References g2o::SparseOptimizer::clear().

linearSolver()

template<typename Traits >

LinearSolver< PoseMatrixType > & g2o::BlockSolver< Traits >::linearSolver ( ) const

inline

Definition at line 142 of file block_solver.h.

{ return *_linearSolver; }

References g2o::BlockSolver< Traits >::_linearSolver.

Referenced by g2o::SparseOptimizerIncremental::initSolver().

multiplyHessian()

template<typename Traits >

virtual void g2o::BlockSolver< Traits >::multiplyHessian ( double *  dest, const double *  src  ) const

inlinevirtual

compute dest = H * src

Implements g2o::BlockSolverBase.

Definition at line 149 of file block_solver.h.

                                                                      {
    _Hpp->multiplySymmetricUpperTriangle(dest, src);
  }

References g2o::BlockSolver< Traits >::_Hpp.

resize()

template<typename Traits >

void g2o::BlockSolver< Traits >::resize ( int *  blockPoseIndices, int  numPoseBlocks, int *  blockLandmarkIndices, int  numLandmarkBlocks, int  totalDim  )

protected

Definition at line 53 of file block_solver.hpp.

                                                               {
  deallocate();
 
  resizeVector(s);
 
  if (_doSchur) {
    // the following two are only used in schur
    assert(_sizePoses > 0 && "allocating with wrong size");
    _coefficients.reset(allocate_aligned<double>(s));
    _bschur.reset(allocate_aligned<double>(_sizePoses));
  }
 
  _Hpp = std::make_unique<PoseHessianType>(blockPoseIndices, blockPoseIndices,
                                           numPoseBlocks, numPoseBlocks);
  if (_doSchur) {
    _Hschur = std::make_unique<PoseHessianType>(
        blockPoseIndices, blockPoseIndices, numPoseBlocks, numPoseBlocks);
    _Hll = std::make_unique<LandmarkHessianType>(
        blockLandmarkIndices, blockLandmarkIndices, numLandmarkBlocks,
        numLandmarkBlocks);
    _DInvSchur =
        std::make_unique<SparseBlockMatrixDiagonal<LandmarkMatrixType>>(
            _Hll->colBlockIndices());
    _Hpl = std::make_unique<PoseLandmarkHessianType>(
        blockPoseIndices, blockLandmarkIndices, numPoseBlocks,
        numLandmarkBlocks);
    _HplCCS = std::make_unique<SparseBlockMatrixCCS<PoseLandmarkMatrixType>>(
        _Hpl->rowBlockIndices(), _Hpl->colBlockIndices());
    _HschurTransposedCCS =
        std::make_unique<SparseBlockMatrixCCS<PoseMatrixType>>(
            _Hschur->colBlockIndices(), _Hschur->rowBlockIndices());
#ifdef G2O_OPENMP
    _coefficientsMutex.resize(numPoseBlocks);
#endif
  }
}

restoreDiagonal()

template<typename Traits >

void g2o::BlockSolver< Traits >::restoreDiagonal ( )

virtual

restore a previously made backup of the diagonal

Implements g2o::Solver.

Definition at line 588 of file block_solver.hpp.

                                          {
  assert((int)_diagonalBackupPose.size() == _numPoses &&
         "Mismatch in dimensions");
  assert((int)_diagonalBackupLandmark.size() == _numLandmarks &&
         "Mismatch in dimensions");
  for (int i = 0; i < _numPoses; ++i) {
    PoseMatrixType* b = _Hpp->block(i, i);
    b->diagonal() = _diagonalBackupPose[i];
  }
  for (int i = 0; i < _numLandmarks; ++i) {
    LandmarkMatrixType* b = _Hll->block(i, i);
    b->diagonal() = _diagonalBackupLandmark[i];
  }
}

saveHessian()

template<typename Traits >

bool g2o::BlockSolver< Traits >::saveHessian ( const std::string &  ) const

virtual

write the hessian to disk using the specified file name

Implements g2o::Solver.

Definition at line 621 of file block_solver.hpp.

                                                                     {
  return _Hpp->writeOctave(fileName.c_str(), true);
}

schur()

template<typename Traits >

virtual bool g2o::BlockSolver< Traits >::schur ( )

inlinevirtual

should the solver perform the schur complement or not

Implements g2o::Solver.

Definition at line 139 of file block_solver.h.

{ return _doSchur; }

References g2o::BlockSolver< Traits >::_doSchur.

setLambda()

template<typename Traits >

bool g2o::BlockSolver< Traits >::setLambda ( double  lambda, bool  backup = false  )

virtual

update the system while performing Levenberg, i.e., modifying the diagonal components of A by doing += lambda along the main diagonal of the Matrix. Note that this function may be called with a positive and a negative lambda. The latter is used to undo a former modification. If backup is true, then the solver should store a backup of the diagonal, which can be restored by restoreDiagonal()

Implements g2o::Solver.

Definition at line 563 of file block_solver.hpp.

                                                              {
  if (backup) {
    _diagonalBackupPose.resize(_numPoses);
    _diagonalBackupLandmark.resize(_numLandmarks);
  }
#ifdef G2O_OPENMP
#pragma omp parallel for default(shared) if (_numPoses > 100)
#endif
  for (int i = 0; i < _numPoses; ++i) {
    PoseMatrixType* b = _Hpp->block(i, i);
    if (backup) _diagonalBackupPose[i] = b->diagonal();
    b->diagonal().array() += lambda;
  }
#ifdef G2O_OPENMP
#pragma omp parallel for default(shared) if (_numLandmarks > 100)
#endif
  for (int i = 0; i < _numLandmarks; ++i) {
    LandmarkMatrixType* b = _Hll->block(i, i);
    if (backup) _diagonalBackupLandmark[i] = b->diagonal();
    b->diagonal().array() += lambda;
  }
  return true;
}

setSchur()

template<typename Traits >

virtual void g2o::BlockSolver< Traits >::setSchur ( bool  s )

inlinevirtual

Implements g2o::Solver.

Definition at line 140 of file block_solver.h.

{ _doSchur = s; }

References g2o::BlockSolver< Traits >::_doSchur.

Referenced by g2o::SparseOptimizerIncremental::initSolver().

setWriteDebug()

template<typename Traits >

void g2o::BlockSolver< Traits >::setWriteDebug ( bool  )

virtual

write debug output of the Hessian if system is not positive definite

Implements g2o::Solver.

Definition at line 616 of file block_solver.hpp.

                                                       {
  _linearSolver->setWriteDebug(writeDebug);
}

solve()

template<typename Traits >

bool g2o::BlockSolver< Traits >::solve ( )

virtual

solve Ax = b

Implements g2o::Solver.

Definition at line 331 of file block_solver.hpp.

                                {
  if (!_doSchur) {
    double 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
  double 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(double));
#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<LandmarkMatrixType>::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; j < D->rows(); ++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<PoseLandmarkMatrixType>::SparseColumn&
        landmarkColumn = _HplCCS->blockCols()[landmarkIndex];
 
    for (typename SparseBlockMatrixCCS<
             PoseLandmarkMatrixType>::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<PoseMatrixType>::SparseColumn::iterator
          targetColumnIt = _HschurTransposedCCS->blockCols()[i1].begin();
 
      typename SparseBlockMatrixCCS<PoseLandmarkMatrixType>::RowBlock aux(i1,
                                                                          0);
      typename SparseBlockMatrixCCS<
          PoseLandmarkMatrixType>::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();
      }
    }
  }
 
  // _bschur = _b for calling solver, and not touching _b
  memcpy(_bschur.get(), _b, _sizePoses * sizeof(double));
  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;
  }
 
  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;
  double* xp = _x;
  double* cp = _coefficients.get();
 
  double* xl = _x + _sizePoses;
  double* cl = _coefficients.get() + _sizePoses;
  double* bl = _b + _sizePoses;
 
  // cp = -xp
  for (int i = 0; i < _sizePoses; ++i) cp[i] = -xp[i];
 
  // cl = bl
  memcpy(cl, bl, _sizeLandmarks * sizeof(double));
 
  // cl = bl - Bt * xp
  // Bt->multiply(cl, cp);
  _HplCCS->rightMultiply(cl, cp);
 
  // xl = Dinv * cl
  memset(xl, 0, _sizeLandmarks * sizeof(double));
  _DInvSchur->multiply(xl, cl);
  //_DInvSchur->rightMultiply(xl,cl);
 
  return true;
}

References g2o::SparseBlockMatrixCCS< MatrixType >::blockCols(), g2o::get_monotonic_time(), g2o::G2OBatchStatistics::globalStats(), g2o::G2OBatchStatistics::hessianDimension, g2o::G2OBatchStatistics::hessianLandmarkDimension, g2o::G2OBatchStatistics::hessianPoseDimension, g2o::G2OBatchStatistics::timeLinearSolver, and g2o::G2OBatchStatistics::timeSchurComplement.

supportsSchur()

template<typename Traits >

virtual bool g2o::BlockSolver< Traits >::supportsSchur ( )

inlinevirtual

does this solver support the Schur complement for solving a system consisting of poses and landmarks. Re-implement in a derived solver, if your solver supports it.

Reimplemented from g2o::Solver.

Definition at line 138 of file block_solver.h.

{ return true; }

updateStructure()

template<typename Traits >

bool g2o::BlockSolver< Traits >::updateStructure ( const std::vector< HyperGraph::Vertex * > &  vset, const HyperGraph::EdgeSet &  edges  )

virtual

update the structures for online processing

Implements g2o::Solver.

Definition at line 271 of file block_solver.hpp.

                                    {
  for (std::vector<HyperGraph::Vertex*>::const_iterator vit = vset.begin();
       vit != vset.end(); ++vit) {
    OptimizableGraph::Vertex* v = static_cast<OptimizableGraph::Vertex*>(*vit);
    int dim = v->dimension();
    if (!v->marginalized()) {
      v->setColInHessian(_sizePoses);
      _sizePoses += dim;
      _Hpp->rowBlockIndices().push_back(_sizePoses);
      _Hpp->colBlockIndices().push_back(_sizePoses);
      _Hpp->blockCols().push_back(
          typename SparseBlockMatrix<PoseMatrixType>::IntBlockMap());
      ++_numPoses;
      int ind = v->hessianIndex();
      PoseMatrixType* m = _Hpp->block(ind, ind, true);
      v->mapHessianMemory(m->data());
    } else {
      G2O_ERROR("updateStructure(): Schur not supported");
      abort();
    }
  }
  resizeVector(_sizePoses + _sizeLandmarks);
 
  for (HyperGraph::EdgeSet::const_iterator it = edges.begin();
       it != edges.end(); ++it) {
    OptimizableGraph::Edge* e = static_cast<OptimizableGraph::Edge*>(*it);
 
    for (size_t viIdx = 0; viIdx < e->vertices().size(); ++viIdx) {
      OptimizableGraph::Vertex* v1 =
          (OptimizableGraph::Vertex*)e->vertex(viIdx);
      int ind1 = v1->hessianIndex();
      int indexV1Bak = ind1;
      if (ind1 == -1) continue;
      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 triangular
                              // block
          std::swap(ind1, ind2);
 
        if (!v1->marginalized() && !v2->marginalized()) {
          PoseMatrixType* m = _Hpp->block(ind1, ind2, true);
          e->mapHessianMemory(m->data(), viIdx, vjIdx, transposedBlock);
        } else {
          G2O_ERROR("{}: not supported", __PRETTY_FUNCTION__);
        }
      }
    }
  }
 
  return true;
}

References __PRETTY_FUNCTION__, g2o::OptimizableGraph::Vertex::dimension(), G2O_ERROR, g2o::OptimizableGraph::Vertex::hessianIndex(), g2o::OptimizableGraph::Vertex::mapHessianMemory(), g2o::OptimizableGraph::Edge::mapHessianMemory(), g2o::OptimizableGraph::Vertex::marginalized(), g2o::OptimizableGraph::Vertex::setColInHessian(), g2o::HyperGraph::Edge::vertex(), and g2o::HyperGraph::Edge::vertices().

writeDebug()

template<typename Traits >

virtual bool g2o::BlockSolver< Traits >::writeDebug ( ) const

inlinevirtual

Implements g2o::Solver.

Definition at line 145 of file block_solver.h.

{ return _linearSolver->writeDebug(); }

References g2o::BlockSolver< Traits >::_linearSolver.

Member Data Documentation

_bschur

template<typename Traits >

std::unique_ptr<double[], aligned_deleter<double> > g2o::BlockSolver< Traits >::_bschur

protected

Definition at line 181 of file block_solver.h.

_coefficients

template<typename Traits >

std::unique_ptr<double[], aligned_deleter<double> > g2o::BlockSolver< Traits >::_coefficients

protected

Definition at line 180 of file block_solver.h.

_diagonalBackupLandmark

template<typename Traits >

std::vector<LandmarkVectorType> g2o::BlockSolver< Traits >::_diagonalBackupLandmark

protected

Definition at line 172 of file block_solver.h.

_diagonalBackupPose

template<typename Traits >

std::vector<PoseVectorType> g2o::BlockSolver< Traits >::_diagonalBackupPose

protected

Definition at line 171 of file block_solver.h.

_DInvSchur

template<typename Traits >

std::unique_ptr<SparseBlockMatrixDiagonal<LandmarkMatrixType> > g2o::BlockSolver< Traits >::_DInvSchur

protected

Definition at line 164 of file block_solver.h.

_doSchur

template<typename Traits >

bool g2o::BlockSolver< Traits >::_doSchur

protected

Definition at line 178 of file block_solver.h.

Referenced by g2o::BlockSolver< Traits >::BlockSolver(), g2o::BlockSolver< Traits >::schur(), and g2o::BlockSolver< Traits >::setSchur().

_Hll

template<typename Traits >

std::unique_ptr<SparseBlockMatrix<LandmarkMatrixType> > g2o::BlockSolver< Traits >::_Hll

protected

Definition at line 160 of file block_solver.h.

_Hpl

template<typename Traits >

std::unique_ptr<SparseBlockMatrix<PoseLandmarkMatrixType> > g2o::BlockSolver< Traits >::_Hpl

protected

Definition at line 161 of file block_solver.h.

_HplCCS

template<typename Traits >

std::unique_ptr<SparseBlockMatrixCCS<PoseLandmarkMatrixType> > g2o::BlockSolver< Traits >::_HplCCS

protected

Definition at line 166 of file block_solver.h.

_Hpp

template<typename Traits >

std::unique_ptr<SparseBlockMatrix<PoseMatrixType> > g2o::BlockSolver< Traits >::_Hpp

protected

Definition at line 159 of file block_solver.h.

Referenced by g2o::BlockSolver< Traits >::multiplyHessian().

_Hschur

template<typename Traits >

std::unique_ptr<SparseBlockMatrix<PoseMatrixType> > g2o::BlockSolver< Traits >::_Hschur

protected

Definition at line 163 of file block_solver.h.

_HschurTransposedCCS

template<typename Traits >

std::unique_ptr<SparseBlockMatrixCCS<PoseMatrixType> > g2o::BlockSolver< Traits >::_HschurTransposedCCS

protected

Definition at line 167 of file block_solver.h.

_linearSolver

template<typename Traits >

std::unique_ptr<LinearSolverType> g2o::BlockSolver< Traits >::_linearSolver

protected

Definition at line 169 of file block_solver.h.

Referenced by g2o::BlockSolver< Traits >::linearSolver(), and g2o::BlockSolver< Traits >::writeDebug().

_numLandmarks

template<typename Traits >

int g2o::BlockSolver< Traits >::_numLandmarks

protected

Definition at line 183 of file block_solver.h.

Referenced by g2o::BlockSolver< Traits >::BlockSolver().

_numPoses

template<typename Traits >

int g2o::BlockSolver< Traits >::_numPoses

protected

Definition at line 183 of file block_solver.h.

Referenced by g2o::BlockSolver< Traits >::BlockSolver().

_sizeLandmarks

template<typename Traits >

int g2o::BlockSolver< Traits >::_sizeLandmarks

protected

Definition at line 184 of file block_solver.h.

Referenced by g2o::BlockSolver< Traits >::BlockSolver().

_sizePoses

template<typename Traits >

int g2o::BlockSolver< Traits >::_sizePoses

protected

Definition at line 184 of file block_solver.h.

Referenced by g2o::BlockSolver< Traits >::BlockSolver().

LandmarkDim

template<typename Traits >

const int g2o::BlockSolver< Traits >::LandmarkDim = Traits::LandmarkDim

static

Definition at line 106 of file block_solver.h.

PoseDim

template<typename Traits >

const int g2o::BlockSolver< Traits >::PoseDim = Traits::PoseDim

static

Definition at line 105 of file block_solver.h.

---

The documentation for this class was generated from the following files:

• /build/reproducible-path/g2o-0~20230806/g2o/core/block_solver.h
• /build/reproducible-path/g2o-0~20230806/g2o/core/block_solver.hpp