g2o
|
Implementation of a solver operating on the blocks of the Hessian. More...
#include <block_solver.h>
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) |
Implementation of a solver operating on the blocks of the Hessian.
Definition at line 96 of file block_solver.h.
typedef Traits::LandmarkHessianType g2o::BlockSolver< Traits >::LandmarkHessianType |
Definition at line 108 of file block_solver.h.
typedef Traits::LandmarkMatrixType g2o::BlockSolver< Traits >::LandmarkMatrixType |
Definition at line 102 of file block_solver.h.
typedef Traits::LandmarkVectorType g2o::BlockSolver< Traits >::LandmarkVectorType |
Definition at line 105 of file block_solver.h.
typedef Traits::LinearSolverType g2o::BlockSolver< Traits >::LinearSolverType |
Definition at line 110 of file block_solver.h.
typedef Traits::PoseHessianType g2o::BlockSolver< Traits >::PoseHessianType |
Definition at line 107 of file block_solver.h.
typedef Traits::PoseLandmarkHessianType g2o::BlockSolver< Traits >::PoseLandmarkHessianType |
Definition at line 109 of file block_solver.h.
typedef Traits::PoseLandmarkMatrixType g2o::BlockSolver< Traits >::PoseLandmarkMatrixType |
Definition at line 103 of file block_solver.h.
typedef Traits::PoseMatrixType g2o::BlockSolver< Traits >::PoseMatrixType |
Definition at line 101 of file block_solver.h.
typedef Traits::PoseVectorType g2o::BlockSolver< Traits >::PoseVectorType |
Definition at line 104 of file block_solver.h.
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.
References g2o::BlockSolver< Traits >::_bschur, g2o::BlockSolver< Traits >::_coefficients, 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::BlockSolver< Traits >::_sizeLandmarks, g2o::BlockSolver< Traits >::_sizePoses, and g2o::Solver::_xSize.
g2o::BlockSolver< Traits >::~BlockSolver | ( | ) |
Definition at line 115 of file block_solver.hpp.
References g2o::BlockSolver< Traits >::_linearSolver, and g2o::BlockSolver< Traits >::deallocate().
|
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().
|
virtual |
build the current system
Implements g2o::Solver.
Definition at line 481 of file block_solver.hpp.
References g2o::Solver::_b, g2o::BlockSolver< Traits >::_doSchur, g2o::BlockSolver< Traits >::_Hll, g2o::BlockSolver< Traits >::_Hpl, g2o::BlockSolver< Traits >::_Hpp, g2o::Solver::_optimizer, g2o::BlockSolver< Traits >::_sizePoses, g2o::SparseOptimizer::activeEdges(), g2o::arrayHasNaN(), g2o::SparseBlockMatrix< MatrixType >::clear(), 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::SparseOptimizer::indexMapping(), g2o::OptimizableGraph::jacobianWorkspace(), g2o::OptimizableGraph::Edge::linearizeOplus(), g2o::OptimizableGraph::Vertex::marginalized(), g2o::HyperGraph::Edge::vertex(), g2o::HyperGraph::Edge::vertices(), and g2o::JacobianWorkspace::workspaceForVertex().
|
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.
|
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().
|
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().
|
inline |
Definition at line 134 of file block_solver.h.
Referenced by g2o::SparseOptimizerIncremental::initSolver().
|
inlinevirtual |
compute dest = H * src
Implements g2o::BlockSolverBase.
Definition at line 141 of file block_solver.h.
|
protected |
Definition at line 62 of file block_solver.hpp.
References g2o::BlockSolver< Traits >::_bschur, g2o::BlockSolver< Traits >::_coefficients, 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 >::_sizePoses, g2o::SparseBlockMatrix< MatrixType >::colBlockIndices(), g2o::BlockSolver< Traits >::deallocate(), g2o::Solver::resizeVector(), and g2o::SparseBlockMatrix< MatrixType >::rowBlockIndices().
Referenced by g2o::BlockSolver< Traits >::buildStructure(), and g2o::SparseOptimizerIncremental::convertTripletUpdateToSparse().
|
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().
|
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().
|
inlinevirtual |
should the solver perform the schur complement or not
Implements g2o::Solver.
Definition at line 131 of file block_solver.h.
|
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().
|
inlinevirtual |
Implements g2o::Solver.
Definition at line 132 of file block_solver.h.
Referenced by g2o::SparseOptimizerIncremental::initSolver().
|
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().
|
virtual |
solve Ax = b
Implements g2o::Solver.
Definition at line 333 of file block_solver.hpp.
References g2o::Solver::_b, g2o::BlockSolver< Traits >::_bschur, g2o::BlockSolver< Traits >::_coefficients, g2o::BlockSolver< Traits >::_DInvSchur, g2o::BlockSolver< Traits >::_doSchur, g2o::BlockSolver< Traits >::_Hll, g2o::BlockSolver< Traits >::_HplCCS, g2o::BlockSolver< Traits >::_Hpp, g2o::BlockSolver< Traits >::_Hschur, g2o::BlockSolver< Traits >::_HschurTransposedCCS, g2o::BlockSolver< Traits >::_linearSolver, g2o::BlockSolver< Traits >::_sizeLandmarks, g2o::BlockSolver< Traits >::_sizePoses, g2o::Solver::_x, g2o::SparseBlockMatrix< MatrixType >::add(), g2o::SparseBlockMatrixCCS< MatrixType >::blockCols(), g2o::SparseBlockMatrix< MatrixType >::blockCols(), g2o::SparseBlockMatrix< MatrixType >::clear(), g2o::SparseBlockMatrix< MatrixType >::cols(), g2o::SparseBlockMatrixDiagonal< MatrixType >::diagonal(), g2o::get_monotonic_time(), g2o::G2OBatchStatistics::globalStats(), g2o::G2OBatchStatistics::hessianDimension, g2o::G2OBatchStatistics::hessianLandmarkDimension, g2o::G2OBatchStatistics::hessianPoseDimension, g2o::SparseBlockMatrixDiagonal< MatrixType >::multiply(), g2o::SparseBlockMatrixCCS< MatrixType >::rightMultiply(), g2o::SparseBlockMatrixCCS< MatrixType >::rowBaseOfBlock(), g2o::SparseBlockMatrix< MatrixType >::rowBaseOfBlock(), g2o::LinearSolver< MatrixType >::solve(), g2o::G2OBatchStatistics::timeLinearSolver, and g2o::G2OBatchStatistics::timeSchurComplement.
|
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.
|
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().
|
inlinevirtual |
Implements g2o::Solver.
Definition at line 137 of file block_solver.h.
|
protected |
Definition at line 171 of file block_solver.h.
Referenced by g2o::BlockSolver< Traits >::BlockSolver(), g2o::BlockSolver< Traits >::deallocate(), g2o::BlockSolver< Traits >::resize(), and g2o::BlockSolver< Traits >::solve().
|
protected |
Definition at line 170 of file block_solver.h.
Referenced by g2o::BlockSolver< Traits >::BlockSolver(), g2o::BlockSolver< Traits >::deallocate(), g2o::BlockSolver< Traits >::resize(), and g2o::BlockSolver< Traits >::solve().
|
protected |
Definition at line 162 of file block_solver.h.
Referenced by g2o::BlockSolver< Traits >::restoreDiagonal(), and g2o::BlockSolver< Traits >::setLambda().
|
protected |
Definition at line 161 of file block_solver.h.
Referenced by g2o::BlockSolver< Traits >::restoreDiagonal(), and g2o::BlockSolver< Traits >::setLambda().
|
protected |
Definition at line 154 of file block_solver.h.
Referenced by g2o::BlockSolver< Traits >::BlockSolver(), g2o::BlockSolver< Traits >::buildStructure(), g2o::BlockSolver< Traits >::deallocate(), g2o::BlockSolver< Traits >::resize(), and g2o::BlockSolver< Traits >::solve().
|
protected |
Definition at line 168 of file block_solver.h.
Referenced by g2o::BlockSolver< Traits >::BlockSolver(), g2o::BlockSolver< Traits >::buildStructure(), g2o::BlockSolver< Traits >::buildSystem(), g2o::BlockSolver< Traits >::resize(), and g2o::BlockSolver< Traits >::solve().
|
protected |
Definition at line 150 of file block_solver.h.
Referenced by g2o::BlockSolver< Traits >::BlockSolver(), g2o::BlockSolver< Traits >::buildStructure(), g2o::BlockSolver< Traits >::buildSystem(), g2o::BlockSolver< Traits >::deallocate(), g2o::BlockSolver< Traits >::init(), g2o::BlockSolver< Traits >::resize(), g2o::BlockSolver< Traits >::restoreDiagonal(), g2o::BlockSolver< Traits >::setLambda(), and g2o::BlockSolver< Traits >::solve().
|
protected |
Definition at line 151 of file block_solver.h.
Referenced by g2o::BlockSolver< Traits >::BlockSolver(), g2o::BlockSolver< Traits >::buildStructure(), g2o::BlockSolver< Traits >::buildSystem(), g2o::BlockSolver< Traits >::deallocate(), g2o::BlockSolver< Traits >::init(), and g2o::BlockSolver< Traits >::resize().
|
protected |
Definition at line 156 of file block_solver.h.
Referenced by g2o::BlockSolver< Traits >::BlockSolver(), g2o::BlockSolver< Traits >::buildStructure(), g2o::BlockSolver< Traits >::deallocate(), g2o::BlockSolver< Traits >::resize(), and g2o::BlockSolver< Traits >::solve().
|
protected |
Definition at line 149 of file block_solver.h.
Referenced by g2o::BlockSolver< Traits >::BlockSolver(), g2o::BlockSolver< Traits >::buildStructure(), g2o::BlockSolver< Traits >::buildSystem(), g2o::BlockSolver< Traits >::computeMarginals(), g2o::BlockSolver< Traits >::deallocate(), g2o::BlockSolver< Traits >::init(), g2o::BlockSolver< Traits >::resize(), g2o::BlockSolver< Traits >::restoreDiagonal(), g2o::BlockSolver< Traits >::saveHessian(), g2o::BlockSolver< Traits >::setLambda(), g2o::BlockSolver< Traits >::solve(), and g2o::BlockSolver< Traits >::updateStructure().
|
protected |
Definition at line 153 of file block_solver.h.
Referenced by g2o::BlockSolver< Traits >::BlockSolver(), g2o::BlockSolver< Traits >::buildStructure(), g2o::BlockSolver< Traits >::deallocate(), g2o::BlockSolver< Traits >::resize(), and g2o::BlockSolver< Traits >::solve().
|
protected |
Definition at line 157 of file block_solver.h.
Referenced by g2o::BlockSolver< Traits >::BlockSolver(), g2o::BlockSolver< Traits >::buildStructure(), g2o::BlockSolver< Traits >::deallocate(), g2o::BlockSolver< Traits >::resize(), and g2o::BlockSolver< Traits >::solve().
|
protected |
Definition at line 159 of file block_solver.h.
Referenced by g2o::BlockSolver< Traits >::computeMarginals(), g2o::BlockSolver< Traits >::init(), g2o::BlockSolver< Traits >::setWriteDebug(), g2o::BlockSolver< Traits >::solve(), and g2o::BlockSolver< Traits >::~BlockSolver().
|
protected |
Definition at line 173 of file block_solver.h.
Referenced by g2o::BlockSolver< Traits >::BlockSolver(), g2o::BlockSolver< Traits >::buildStructure(), g2o::BlockSolver< Traits >::restoreDiagonal(), and g2o::BlockSolver< Traits >::setLambda().
|
protected |
|
protected |
Definition at line 174 of file block_solver.h.
Referenced by g2o::BlockSolver< Traits >::BlockSolver(), g2o::BlockSolver< Traits >::buildStructure(), g2o::BlockSolver< Traits >::solve(), and g2o::BlockSolver< Traits >::updateStructure().
|
protected |
Definition at line 174 of file block_solver.h.
Referenced by g2o::BlockSolver< Traits >::BlockSolver(), g2o::BlockSolver< Traits >::buildStructure(), g2o::BlockSolver< Traits >::buildSystem(), g2o::BlockSolver< Traits >::resize(), g2o::BlockSolver< Traits >::solve(), and g2o::BlockSolver< Traits >::updateStructure().
|
static |
Definition at line 100 of file block_solver.h.
|
static |
Definition at line 99 of file block_solver.h.