Public Types | Public Member Functions | Static Public Attributes | Protected Member Functions | Protected Attributes | List of all members
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 >:
Inheritance graph
[legend]

Public Types

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

Public Member Functions

 BlockSolver (LinearSolverType *linearSolver)
 
virtual bool buildStructure (bool zeroBlocks=false)
 
virtual bool buildSystem ()
 
virtual bool computeMarginals (SparseBlockMatrix< MatrixXd > &spinv, const std::vector< std::pair< int, int > > &blockIndices)
 
virtual bool init (SparseOptimizer *optmizer, bool online=false)
 
LinearSolver< PoseMatrixType > * linearSolver () const
 
virtual void multiplyHessian (double *dest, const double *src) const
 
virtual void restoreDiagonal ()
 
virtual bool saveHessian (const std::string &fileName) const
 write the hessian to disk using the specified file name More...
 
virtual bool schur ()
 should the solver perform the schur complement or not More...
 
virtual bool setLambda (double lambda, bool backup=false)
 
virtual void setSchur (bool s)
 
virtual void setWriteDebug (bool writeDebug)
 
virtual bool solve ()
 
virtual bool supportsSchur ()
 
virtual bool updateStructure (const std::vector< HyperGraph::Vertex * > &vset, const HyperGraph::EdgeSet &edges)
 
virtual bool writeDebug () const
 
 ~BlockSolver ()
 
- Public Member Functions inherited from g2o::BlockSolverBase
virtual ~BlockSolverBase ()
 
- Public Member Functions inherited from g2o::Solver
size_t additionalVectorSpace () const
 
double * b ()
 return b, the right hand side of the system More...
 
const double * b () const
 
bool levenberg () const
 the system is Levenberg-Marquardt More...
 
SparseOptimizeroptimizer () const
 the optimizer (graph) on which the solver works More...
 
void setAdditionalVectorSpace (size_t s)
 
void setLevenberg (bool levenberg)
 
void setOptimizer (SparseOptimizer *optimizer)
 
 Solver ()
 
size_t vectorSize () const
 return the size of the solution vector (x) and b More...
 
double * x ()
 return x, the solution vector More...
 
const double * x () const
 
virtual ~Solver ()
 

Static Public Attributes

static const int LandmarkDim = Traits::LandmarkDim
 
static const int PoseDim = Traits::PoseDim
 

Protected Member Functions

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

Protected Attributes

double * _bschur
 
double * _coefficients
 
std::vector< LandmarkVectorType, Eigen::aligned_allocator< LandmarkVectorType > > _diagonalBackupLandmark
 
std::vector< PoseVectorType, Eigen::aligned_allocator< PoseVectorType > > _diagonalBackupPose
 
SparseBlockMatrixDiagonal< LandmarkMatrixType > * _DInvSchur
 
bool _doSchur
 
SparseBlockMatrix< LandmarkMatrixType > * _Hll
 
SparseBlockMatrix< PoseLandmarkMatrixType > * _Hpl
 
SparseBlockMatrixCCS< PoseLandmarkMatrixType > * _HplCCS
 
SparseBlockMatrix< PoseMatrixType > * _Hpp
 
SparseBlockMatrix< PoseMatrixType > * _Hschur
 
SparseBlockMatrixCCS< PoseMatrixType > * _HschurTransposedCCS
 
LinearSolver< PoseMatrixType > * _linearSolver
 
int _numLandmarks
 
int _numPoses
 
int _sizeLandmarks
 
int _sizePoses
 
- Protected Attributes inherited from g2o::Solver
size_t _additionalVectorSpace
 
double * _b
 
bool _isLevenberg
 the system we gonna solve is a Levenberg-Marquardt system More...
 
size_t _maxXSize
 
SparseOptimizer_optimizer
 
double * _x
 
size_t _xSize
 

Detailed Description

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

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

Definition at line 97 of file block_solver.h.

Member Typedef Documentation

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

Definition at line 109 of file block_solver.h.

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

Definition at line 103 of file block_solver.h.

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

Definition at line 106 of file block_solver.h.

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

Definition at line 111 of file block_solver.h.

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

Definition at line 108 of file block_solver.h.

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

Definition at line 110 of file block_solver.h.

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

Definition at line 104 of file block_solver.h.

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

Definition at line 102 of file block_solver.h.

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

Definition at line 105 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 42 of file block_solver.hpp.

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

Definition at line 136 of file block_solver.hpp.

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

template<typename Traits >
bool g2o::BlockSolver< Traits >::buildSystem ( )
virtual

build the current system

Implements g2o::Solver.

Definition at line 502 of file block_solver.hpp.

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

template<typename Traits >
void g2o::BlockSolver< Traits >::deallocate ( )
protected

Definition at line 95 of file block_solver.hpp.

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

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

Definition at line 135 of file block_solver.h.

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

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

Definition at line 65 of file block_solver.hpp.

template<typename Traits >
void g2o::BlockSolver< Traits >::restoreDiagonal ( )
virtual

restore a previosly made backup of the diagonal

Implements g2o::Solver.

Definition at line 592 of file block_solver.hpp.

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

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

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

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

Implements g2o::Solver.

Definition at line 133 of file block_solver.h.

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

template<typename Traits >
bool g2o::BlockSolver< Traits >::solve ( )
virtual

solve Ax = b

Implements g2o::Solver.

Definition at line 354 of file block_solver.hpp.

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

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

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

Implements g2o::Solver.

Definition at line 138 of file block_solver.h.

Member Data Documentation

template<typename Traits >
double* g2o::BlockSolver< Traits >::_bschur
protected

Definition at line 172 of file block_solver.h.

template<typename Traits >
double* g2o::BlockSolver< Traits >::_coefficients
protected

Definition at line 171 of file block_solver.h.

template<typename Traits >
std::vector<LandmarkVectorType, Eigen::aligned_allocator<LandmarkVectorType> > g2o::BlockSolver< Traits >::_diagonalBackupLandmark
protected

Definition at line 163 of file block_solver.h.

template<typename Traits >
std::vector<PoseVectorType, Eigen::aligned_allocator<PoseVectorType> > g2o::BlockSolver< Traits >::_diagonalBackupPose
protected

Definition at line 162 of file block_solver.h.

template<typename Traits >
SparseBlockMatrixDiagonal<LandmarkMatrixType>* g2o::BlockSolver< Traits >::_DInvSchur
protected

Definition at line 155 of file block_solver.h.

template<typename Traits >
bool g2o::BlockSolver< Traits >::_doSchur
protected

Definition at line 169 of file block_solver.h.

template<typename Traits >
SparseBlockMatrix<LandmarkMatrixType>* g2o::BlockSolver< Traits >::_Hll
protected

Definition at line 151 of file block_solver.h.

template<typename Traits >
SparseBlockMatrix<PoseLandmarkMatrixType>* g2o::BlockSolver< Traits >::_Hpl
protected

Definition at line 152 of file block_solver.h.

template<typename Traits >
SparseBlockMatrixCCS<PoseLandmarkMatrixType>* g2o::BlockSolver< Traits >::_HplCCS
protected

Definition at line 157 of file block_solver.h.

template<typename Traits >
SparseBlockMatrix<PoseMatrixType>* g2o::BlockSolver< Traits >::_Hpp
protected

Definition at line 150 of file block_solver.h.

template<typename Traits >
SparseBlockMatrix<PoseMatrixType>* g2o::BlockSolver< Traits >::_Hschur
protected

Definition at line 154 of file block_solver.h.

template<typename Traits >
SparseBlockMatrixCCS<PoseMatrixType>* g2o::BlockSolver< Traits >::_HschurTransposedCCS
protected

Definition at line 158 of file block_solver.h.

template<typename Traits >
LinearSolver<PoseMatrixType>* g2o::BlockSolver< Traits >::_linearSolver
protected

Definition at line 160 of file block_solver.h.

template<typename Traits >
int g2o::BlockSolver< Traits >::_numLandmarks
protected

Definition at line 174 of file block_solver.h.

template<typename Traits >
int g2o::BlockSolver< Traits >::_numPoses
protected

Definition at line 174 of file block_solver.h.

template<typename Traits >
int g2o::BlockSolver< Traits >::_sizeLandmarks
protected

Definition at line 175 of file block_solver.h.

template<typename Traits >
int g2o::BlockSolver< Traits >::_sizePoses
protected

Definition at line 175 of file block_solver.h.

template<typename Traits >
const int g2o::BlockSolver< Traits >::LandmarkDim = Traits::LandmarkDim
static

Definition at line 101 of file block_solver.h.

template<typename Traits >
const int g2o::BlockSolver< Traits >::PoseDim = Traits::PoseDim
static

Definition at line 100 of file block_solver.h.


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


orb_slam2_ros
Author(s):
autogenerated on Wed Apr 21 2021 02:53:06