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

#include <block_solver.h>

Inheritance diagram for g2o::BlockSolver< Traits >:

[legend]

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

References _doSchur, _linearSolver, _numLandmarks, _numPoses, _sizeLandmarks, _sizePoses, g2o::Solver::_xSize, and linearSolver().

◆ ~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

◆ buildSystem()

template<typename Traits>

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

virtual

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

References _Hpp, _linearSolver, 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.

References _bschur, _coefficients, _DInvSchur, _Hll, _Hpl, _HplCCS, _Hpp, _Hschur, and _HschurTransposedCCS.

Referenced by resize().

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

References _Hll, _Hpl, _Hpp, _linearSolver, g2o::Solver::_optimizer, and g2o::Solver::optimizer().

◆ linearSolver()

template<typename Traits>

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

inline

Definition at line 142 of file block_solver.h.

Referenced by BlockSolver(), and 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.

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

References _bschur, _coefficients, _DInvSchur, _doSchur, _Hll, _Hpl, _HplCCS, _Hpp, _Hschur, _HschurTransposedCCS, _sizePoses, g2o::allocate_aligned(), deallocate(), and g2o::Solver::resizeVector().

Referenced by buildStructure().

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

References _diagonalBackupLandmark, _diagonalBackupPose, _Hll, _Hpp, _numLandmarks, _numPoses, and g2o::Solver::b().

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

References _Hpp.

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

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

References _diagonalBackupLandmark, _diagonalBackupPose, _Hll, _Hpp, _numLandmarks, _numPoses, and g2o::Solver::b().

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

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.

References _linearSolver, and 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.

References g2o::Solver::_b, _bschur, _coefficients, _DInvSchur, _doSchur, _Hll, _HplCCS, _Hpp, _Hschur, _HschurTransposedCCS, _linearSolver, _sizeLandmarks, _sizePoses, g2o::Solver::_x, 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.

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

References __PRETTY_FUNCTION__, _Hpp, _numPoses, _sizeLandmarks, _sizePoses, g2o::OptimizableGraph::Vertex::dimension(), G2O_ERROR, g2o::OptimizableGraph::Vertex::hessianIndex(), g2o::OptimizableGraph::Edge::mapHessianMemory(), g2o::OptimizableGraph::Vertex::mapHessianMemory(), g2o::OptimizableGraph::Vertex::marginalized(), g2o::Solver::resizeVector(), 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.

Referenced by setWriteDebug().

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.

Referenced by deallocate(), resize(), and solve().

◆ _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.

Referenced by deallocate(), resize(), and solve().

◆ _diagonalBackupLandmark

template<typename Traits>

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

protected

Definition at line 172 of file block_solver.h.

Referenced by restoreDiagonal(), and setLambda().

◆ _diagonalBackupPose

template<typename Traits>

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

protected

Definition at line 171 of file block_solver.h.

Referenced by restoreDiagonal(), and setLambda().

◆ _DInvSchur

template<typename Traits>

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

protected

Definition at line 164 of file block_solver.h.

Referenced by buildStructure(), deallocate(), resize(), and solve().

◆ _doSchur

template<typename Traits>

bool g2o::BlockSolver< Traits >::_doSchur

protected

Definition at line 178 of file block_solver.h.

Referenced by BlockSolver(), buildStructure(), buildSystem(), resize(), and solve().

◆ _Hll

template<typename Traits>

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

protected

Definition at line 160 of file block_solver.h.

Referenced by buildStructure(), buildSystem(), deallocate(), init(), resize(), restoreDiagonal(), setLambda(), and solve().

◆ _Hpl

template<typename Traits>

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

protected

Definition at line 161 of file block_solver.h.

Referenced by buildStructure(), buildSystem(), deallocate(), init(), and resize().

◆ _HplCCS

template<typename Traits>

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

protected

Definition at line 166 of file block_solver.h.

Referenced by buildStructure(), deallocate(), resize(), and solve().

◆ _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 buildStructure(), buildSystem(), computeMarginals(), deallocate(), init(), resize(), restoreDiagonal(), saveHessian(), setLambda(), solve(), and updateStructure().

◆ _Hschur

template<typename Traits>

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

protected

Definition at line 163 of file block_solver.h.

Referenced by buildStructure(), deallocate(), resize(), and solve().

◆ _HschurTransposedCCS

template<typename Traits>

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

protected

Definition at line 167 of file block_solver.h.

Referenced by buildStructure(), deallocate(), resize(), and solve().

◆ _linearSolver

template<typename Traits>

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

protected

Definition at line 169 of file block_solver.h.

Referenced by BlockSolver(), computeMarginals(), init(), setWriteDebug(), and solve().

◆ _numLandmarks

template<typename Traits>

int g2o::BlockSolver< Traits >::_numLandmarks

protected

Definition at line 183 of file block_solver.h.

Referenced by BlockSolver(), buildStructure(), restoreDiagonal(), and setLambda().

◆ _numPoses

template<typename Traits>

int g2o::BlockSolver< Traits >::_numPoses

protected

Definition at line 183 of file block_solver.h.

Referenced by BlockSolver(), buildStructure(), restoreDiagonal(), setLambda(), and updateStructure().

◆ _sizeLandmarks

template<typename Traits>

int g2o::BlockSolver< Traits >::_sizeLandmarks

protected

Definition at line 184 of file block_solver.h.

Referenced by BlockSolver(), buildStructure(), solve(), and updateStructure().

◆ _sizePoses

template<typename Traits>

int g2o::BlockSolver< Traits >::_sizePoses

protected

Definition at line 184 of file block_solver.h.

Referenced by BlockSolver(), buildStructure(), buildSystem(), resize(), solve(), and updateStructure().

◆ 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

Public Types

Public Member Functions

Static Public Attributes

Protected Member Functions

Protected Attributes

Detailed Description

Member Typedef Documentation

◆ LandmarkHessianType

◆ LandmarkMatrixType

◆ LandmarkVectorType

◆ LinearSolverType

◆ PoseHessianType

◆ PoseLandmarkHessianType

◆ PoseLandmarkMatrixType

◆ PoseMatrixType

◆ PoseVectorType

Constructor & Destructor Documentation

◆ BlockSolver()

◆ ~BlockSolver()

Member Function Documentation

◆ buildStructure()

◆ buildSystem()

◆ computeMarginals()

◆ deallocate()

◆ init()

◆ linearSolver()

◆ multiplyHessian()

◆ resize()

◆ restoreDiagonal()

◆ saveHessian()

◆ schur()

◆ setLambda()

◆ setSchur()

◆ setWriteDebug()

◆ solve()

◆ supportsSchur()

◆ updateStructure()

◆ writeDebug()

Member Data Documentation

◆ _bschur

◆ _coefficients

◆ _diagonalBackupLandmark

◆ _diagonalBackupPose

◆ _DInvSchur

◆ _doSchur

◆ _Hll

◆ _Hpl

◆ _HplCCS

◆ _Hpp

◆ _Hschur

◆ _HschurTransposedCCS

◆ _linearSolver

◆ _numLandmarks

◆ _numPoses

◆ _sizeLandmarks

◆ _sizePoses

◆ LandmarkDim

◆ PoseDim