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 >:

[legend]

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 (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< MatrixXD > &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 More...

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 More...

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 More...

const double *	x () const

double *	b ()
	return b, the right hand side of the system More...

const double *	b () const

size_t	vectorSize () const
	return the size of the solution vector (x) and b More...

SparseOptimizer *	optimizer () const
	the optimizer (graph) on which the solver works More...

void	setOptimizer (SparseOptimizer *optimizer)

bool	levenberg () const
	the system is Levenberg-Marquardt More...

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
SparseBlockMatrix< PoseMatrixType > *	_Hpp

SparseBlockMatrix< LandmarkMatrixType > *	_Hll

SparseBlockMatrix< PoseLandmarkMatrixType > *	_Hpl

SparseBlockMatrix< PoseMatrixType > *	_Hschur

SparseBlockMatrixDiagonal< LandmarkMatrixType > *	_DInvSchur

SparseBlockMatrixCCS< PoseLandmarkMatrixType > *	_HplCCS

SparseBlockMatrixCCS< PoseMatrixType > *	_HschurTransposedCCS

LinearSolver< PoseMatrixType > *	_linearSolver

std::vector< PoseVectorType, Eigen::aligned_allocator< PoseVectorType > >	_diagonalBackupPose

std::vector< LandmarkVectorType, Eigen::aligned_allocator< LandmarkVectorType > >	_diagonalBackupLandmark

bool	_doSchur

double *	_coefficients

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
	the system we gonna solve is a Levenberg-Marquardt system More...

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 96 of file block_solver.h.

Member Typedef Documentation

template<typename Traits>

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

Definition at line 108 of file block_solver.h.

template<typename Traits>

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

Definition at line 102 of file block_solver.h.

template<typename Traits>

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

Definition at line 105 of file block_solver.h.

template<typename Traits>

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

Definition at line 110 of file block_solver.h.

template<typename Traits>

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

Definition at line 107 of file block_solver.h.

template<typename Traits>

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

Definition at line 109 of file block_solver.h.

template<typename Traits>

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

Definition at line 103 of file block_solver.h.

template<typename Traits>

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

Definition at line 101 of file block_solver.h.

template<typename Traits>

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

Definition at line 104 of file block_solver.h.

Constructor & Destructor Documentation

template<typename Traits >

g2o::BlockSolver< Traits >::BlockSolver ( 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 39 of file block_solver.hpp.

                                                                :
   BlockSolverBase(),
   _linearSolver(linearSolver)
 {
   // workspace
   _Hpp=0;
   _Hll=0;
   _Hpl=0;
   _HplCCS = 0;
   _HschurTransposedCCS = 0;
   _Hschur=0;
   _DInvSchur=0;
   _coefficients=0;
   _bschur = 0;
   _xSize=0;
   _numPoses=0;
   _numLandmarks=0;
   _sizePoses=0;
   _sizeLandmarks=0;
   _doSchur=true;
 }

template<typename Traits >

g2o::BlockSolver< Traits >::~BlockSolver ( )

Definition at line 115 of file block_solver.hpp.

References g2o::BlockSolver< Traits >::_linearSolver, and g2o::BlockSolver< Traits >::deallocate().

 {
   delete _linearSolver;
   deallocate();
 }

Member Function Documentation

template<typename Traits >

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

virtual

build the structure of the system

Implements g2o::Solver.

Definition at line 122 of file block_solver.hpp.

References g2o::BlockSolver< Traits >::_DInvSchur, g2o::BlockSolver< Traits >::_doSchur, g2o::BlockSolver< Traits >::_Hll, g2o::BlockSolver< Traits >::_Hpl, g2o::BlockSolver< Traits >::_HplCCS, g2o::BlockSolver< Traits >::_Hpp, g2o::BlockSolver< Traits >::_Hschur, g2o::BlockSolver< Traits >::_HschurTransposedCCS, g2o::BlockSolver< Traits >::_numLandmarks, g2o::BlockSolver< Traits >::_numPoses, g2o::Solver::_optimizer, g2o::BlockSolver< Traits >::_sizeLandmarks, g2o::BlockSolver< Traits >::_sizePoses, g2o::SparseOptimizer::activeEdges(), g2o::SparseBlockMatrixHashMap< MatrixType >::addBlock(), g2o::SparseBlockMatrix< MatrixType >::block(), g2o::SparseBlockMatrix< MatrixType >::blockCols(), g2o::SparseBlockMatrixHashMap< MatrixType >::blockCols(), g2o::SparseBlockMatrix< MatrixType >::colBlockIndices(), g2o::SparseBlockMatrixDiagonal< MatrixType >::diagonal(), g2o::OptimizableGraph::Vertex::dimension(), g2o::HyperGraph::Vertex::edges(), g2o::SparseBlockMatrix< MatrixType >::fillSparseBlockMatrixCCS(), g2o::SparseBlockMatrix< MatrixType >::fillSparseBlockMatrixCCSTransposed(), g2o::OptimizableGraph::Vertex::hessianIndex(), g2o::SparseOptimizer::indexMapping(), g2o::OptimizableGraph::Vertex::mapHessianMemory(), g2o::OptimizableGraph::Edge::mapHessianMemory(), g2o::OptimizableGraph::Vertex::marginalized(), g2o::BlockSolver< Traits >::resize(), g2o::SparseBlockMatrix< MatrixType >::rowBlockIndices(), g2o::OptimizableGraph::Vertex::setColInHessian(), g2o::SparseBlockMatrix< MatrixType >::takePatternFromHash(), g2o::HyperGraph::Edge::vertex(), and g2o::HyperGraph::Edge::vertices().

 {
   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)
     return true;
 
   _DInvSchur->diagonal().resize(landmarkIdx);
   _Hpl->fillSparseBlockMatrixCCS(*_HplCCS);
 
   for (size_t i = 0; i < _optimizer->indexMapping().size(); ++i) {
     OptimizableGraph::Vertex* v = _optimizer->indexMapping()[i];
     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;
 }

template<typename Traits >

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

virtual

build the current system

Implements g2o::Solver.

Definition at line 481 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) {
           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;
 }

template<typename Traits >

bool g2o::BlockSolver< Traits >::computeMarginals	(	SparseBlockMatrix< MatrixXD > &	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 469 of file block_solver.hpp.

References g2o::BlockSolver< Traits >::_Hpp, g2o::BlockSolver< Traits >::_linearSolver, g2o::get_monotonic_time(), g2o::G2OBatchStatistics::globalStats(), g2o::LinearSolver< MatrixType >::solvePattern(), and g2o::G2OBatchStatistics::timeMarginals.

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

template<typename Traits >

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

protected

Definition at line 92 of file block_solver.hpp.

References g2o::BlockSolver< Traits >::_bschur, g2o::BlockSolver< Traits >::_coefficients, g2o::BlockSolver< Traits >::_DInvSchur, g2o::BlockSolver< Traits >::_Hll, g2o::BlockSolver< Traits >::_Hpl, g2o::BlockSolver< Traits >::_HplCCS, g2o::BlockSolver< Traits >::_Hpp, g2o::BlockSolver< Traits >::_Hschur, and g2o::BlockSolver< Traits >::_HschurTransposedCCS.

Referenced by g2o::BlockSolver< Traits >::resize(), and g2o::BlockSolver< Traits >::~BlockSolver().

 {
   delete _Hpp;
   _Hpp=0;
   delete _Hll;
   _Hll=0;
   delete _Hpl;
   _Hpl = 0;
   delete _Hschur;
   _Hschur=0;
   delete _DInvSchur;
   _DInvSchur=0;
   delete[] _coefficients;
   _coefficients = 0;
   delete[] _bschur;
   _bschur = 0;
   delete _HplCCS;
   _HplCCS = 0;
   delete _HschurTransposedCCS;
   _HschurTransposedCCS = 0;
 }

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 586 of file block_solver.hpp.

References g2o::BlockSolver< Traits >::_Hll, g2o::BlockSolver< Traits >::_Hpl, g2o::BlockSolver< Traits >::_Hpp, g2o::BlockSolver< Traits >::_linearSolver, g2o::Solver::_optimizer, g2o::SparseBlockMatrix< MatrixType >::clear(), g2o::LinearSolver< MatrixType >::init(), and g2o::Solver::optimizer().

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

template<typename Traits>

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

inline

Definition at line 134 of file block_solver.h.

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

134 { return _linearSolver;}

g2o::BlockSolver::_linearSolver

LinearSolver< PoseMatrixType > * _linearSolver

Definition: block_solver.h:159

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 141 of file block_solver.h.

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

g2o::BlockSolver::_Hpp

SparseBlockMatrix< PoseMatrixType > * _Hpp

Definition: block_solver.h:149

g2o::SparseBlockMatrix::multiplySymmetricUpperTriangle

void multiplySymmetricUpperTriangle(double *&dest, const double *src) const

Definition: sparse_block_matrix.hpp:257

template<typename Traits >

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

protected

Definition at line 62 of file block_solver.hpp.

Referenced by g2o::BlockSolver< Traits >::buildStructure(), and g2o::SparseOptimizerIncremental::convertTripletUpdateToSparse().

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

template<typename Traits >

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

virtual

restore a previosly made backup of the diagonal

Implements g2o::Solver.

Definition at line 571 of file block_solver.hpp.

References g2o::BlockSolver< Traits >::_diagonalBackupLandmark, g2o::BlockSolver< Traits >::_diagonalBackupPose, g2o::BlockSolver< Traits >::_Hll, g2o::BlockSolver< Traits >::_Hpp, g2o::BlockSolver< Traits >::_numLandmarks, g2o::BlockSolver< Traits >::_numPoses, g2o::Solver::b(), and g2o::SparseBlockMatrix< MatrixType >::block().

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

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 608 of file block_solver.hpp.

References g2o::BlockSolver< Traits >::_Hpp, and g2o::SparseBlockMatrix< MatrixType >::writeOctave().

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

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 131 of file block_solver.h.

131 { return _doSchur;}

g2o::BlockSolver::_doSchur

bool _doSchur

Definition: block_solver.h:168

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 543 of file block_solver.hpp.

References g2o::BlockSolver< Traits >::_diagonalBackupLandmark, g2o::BlockSolver< Traits >::_diagonalBackupPose, g2o::BlockSolver< Traits >::_Hll, g2o::BlockSolver< Traits >::_Hpp, g2o::BlockSolver< Traits >::_numLandmarks, g2o::BlockSolver< Traits >::_numPoses, g2o::Solver::b(), and g2o::SparseBlockMatrix< MatrixType >::block().

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

template<typename Traits>

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

inlinevirtual

Implements g2o::Solver.

Definition at line 132 of file block_solver.h.

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

132 { _doSchur = s;}

g2o::BlockSolver::_doSchur

bool _doSchur

Definition: block_solver.h:168

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 602 of file block_solver.hpp.

References g2o::BlockSolver< Traits >::_linearSolver, and g2o::LinearSolver< MatrixType >::setWriteDebug().

 {
   _linearSolver->setWriteDebug(writeDebug);
 }

template<typename Traits >

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

virtual

solve Ax = b

Implements g2o::Solver.

Definition at line 333 of file block_solver.hpp.

                                {
   //cerr << __PRETTY_FUNCTION__ << endl;
   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, 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();
       }
     }
   }
   //cerr << "Solve [marginalize] = " <<  get_monotonic_time()-t << endl;
 
   // _bschur = _b for calling solver, and not touching _b
   memcpy(_bschur, _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);
   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;
   double* xp = _x;
   double* cp = _coefficients;
 
   double* xl=_x+_sizePoses;
   double* cl=_coefficients + _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);
   //cerr << "Solve [landmark delta] = " <<  get_monotonic_time()-t << endl;
 
   return true;
 }

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-implemement in a derived solver, if your solver supports it.

Reimplemented from g2o::Solver.

Definition at line 130 of file block_solver.h.

130 {return true;}

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 277 of file block_solver.hpp.

References __PRETTY_FUNCTION__, g2o::BlockSolver< Traits >::_Hpp, g2o::BlockSolver< Traits >::_numPoses, g2o::BlockSolver< Traits >::_sizeLandmarks, g2o::BlockSolver< Traits >::_sizePoses, g2o::SparseBlockMatrix< MatrixType >::block(), g2o::SparseBlockMatrix< MatrixType >::blockCols(), g2o::SparseBlockMatrix< MatrixType >::colBlockIndices(), g2o::OptimizableGraph::Vertex::dimension(), g2o::OptimizableGraph::Vertex::hessianIndex(), g2o::OptimizableGraph::Vertex::mapHessianMemory(), g2o::OptimizableGraph::Edge::mapHessianMemory(), g2o::OptimizableGraph::Vertex::marginalized(), g2o::Solver::resizeVector(), g2o::SparseBlockMatrix< MatrixType >::rowBlockIndices(), g2o::OptimizableGraph::Vertex::setColInHessian(), g2o::HyperGraph::Edge::vertex(), and g2o::HyperGraph::Edge::vertices().

 {
   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 {
       std::cerr << "updateStructure(): Schur not supported" << std::endl;
       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 { 
           std::cerr << __PRETTY_FUNCTION__ << ": not supported" << std::endl;
         }
       }
     }
 
   }
 
   return true;
 }