g2o
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]
Collaboration diagram for g2o::BlockSolver< Traits >:
Collaboration graph
[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...
 
SparseOptimizeroptimizer () 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.

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.

39  :
40  BlockSolverBase(),
42 {
43  // workspace
44  _Hpp=0;
45  _Hll=0;
46  _Hpl=0;
47  _HplCCS = 0;
49  _Hschur=0;
50  _DInvSchur=0;
51  _coefficients=0;
52  _bschur = 0;
53  _xSize=0;
54  _numPoses=0;
55  _numLandmarks=0;
56  _sizePoses=0;
58  _doSchur=true;
59 }
SparseBlockMatrix< PoseMatrixType > * _Hschur
Definition: block_solver.h:153
SparseBlockMatrix< PoseMatrixType > * _Hpp
Definition: block_solver.h:149
SparseBlockMatrixDiagonal< LandmarkMatrixType > * _DInvSchur
Definition: block_solver.h:154
size_t _xSize
Definition: solver.h:139
SparseBlockMatrixCCS< PoseLandmarkMatrixType > * _HplCCS
Definition: block_solver.h:156
SparseBlockMatrix< LandmarkMatrixType > * _Hll
Definition: block_solver.h:150
double * _coefficients
Definition: block_solver.h:170
SparseBlockMatrixCCS< PoseMatrixType > * _HschurTransposedCCS
Definition: block_solver.h:157
LinearSolver< PoseMatrixType > * linearSolver() const
Definition: block_solver.h:134
SparseBlockMatrix< PoseLandmarkMatrixType > * _Hpl
Definition: block_solver.h:151
LinearSolver< PoseMatrixType > * _linearSolver
Definition: block_solver.h:159
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().

116 {
117  delete _linearSolver;
118  deallocate();
119 }
LinearSolver< PoseMatrixType > * _linearSolver
Definition: block_solver.h:159

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

123 {
124  assert(_optimizer);
125 
126  size_t sparseDim = 0;
127  _numPoses=0;
128  _numLandmarks=0;
129  _sizePoses=0;
130  _sizeLandmarks=0;
131  int* blockPoseIndices = new int[_optimizer->indexMapping().size()];
132  int* blockLandmarkIndices = new int[_optimizer->indexMapping().size()];
133 
134  for (size_t i = 0; i < _optimizer->indexMapping().size(); ++i) {
136  int dim = v->dimension();
137  if (! v->marginalized()){
138  v->setColInHessian(_sizePoses);
139  _sizePoses+=dim;
140  blockPoseIndices[_numPoses]=_sizePoses;
141  ++_numPoses;
142  } else {
143  v->setColInHessian(_sizeLandmarks);
144  _sizeLandmarks+=dim;
145  blockLandmarkIndices[_numLandmarks]=_sizeLandmarks;
146  ++_numLandmarks;
147  }
148  sparseDim += dim;
149  }
150  resize(blockPoseIndices, _numPoses, blockLandmarkIndices, _numLandmarks, sparseDim);
151  delete[] blockLandmarkIndices;
152  delete[] blockPoseIndices;
153 
154  // allocate the diagonal on Hpp and Hll
155  int poseIdx = 0;
156  int landmarkIdx = 0;
157  for (size_t i = 0; i < _optimizer->indexMapping().size(); ++i) {
159  if (! v->marginalized()){
160  //assert(poseIdx == v->hessianIndex());
161  PoseMatrixType* m = _Hpp->block(poseIdx, poseIdx, true);
162  if (zeroBlocks)
163  m->setZero();
164  v->mapHessianMemory(m->data());
165  ++poseIdx;
166  } else {
167  LandmarkMatrixType* m = _Hll->block(landmarkIdx, landmarkIdx, true);
168  if (zeroBlocks)
169  m->setZero();
170  v->mapHessianMemory(m->data());
171  ++landmarkIdx;
172  }
173  }
174  assert(poseIdx == _numPoses && landmarkIdx == _numLandmarks);
175 
176  // temporary structures for building the pattern of the Schur complement
177  SparseBlockMatrixHashMap<PoseMatrixType>* schurMatrixLookup = 0;
178  if (_doSchur) {
179  schurMatrixLookup = new SparseBlockMatrixHashMap<PoseMatrixType>(_Hschur->rowBlockIndices(), _Hschur->colBlockIndices());
180  schurMatrixLookup->blockCols().resize(_Hschur->blockCols().size());
181  }
182 
183  // here we assume that the landmark indices start after the pose ones
184  // create the structure in Hpp, Hll and in Hpl
185  for (SparseOptimizer::EdgeContainer::const_iterator it=_optimizer->activeEdges().begin(); it!=_optimizer->activeEdges().end(); ++it){
186  OptimizableGraph::Edge* e = *it;
187 
188  for (size_t viIdx = 0; viIdx < e->vertices().size(); ++viIdx) {
189  OptimizableGraph::Vertex* v1 = (OptimizableGraph::Vertex*) e->vertex(viIdx);
190  int ind1 = v1->hessianIndex();
191  if (ind1 == -1)
192  continue;
193  int indexV1Bak = ind1;
194  for (size_t vjIdx = viIdx + 1; vjIdx < e->vertices().size(); ++vjIdx) {
195  OptimizableGraph::Vertex* v2 = (OptimizableGraph::Vertex*) e->vertex(vjIdx);
196  int ind2 = v2->hessianIndex();
197  if (ind2 == -1)
198  continue;
199  ind1 = indexV1Bak;
200  bool transposedBlock = ind1 > ind2;
201  if (transposedBlock){ // make sure, we allocate the upper triangle block
202  std::swap(ind1, ind2);
203  }
204  if (! v1->marginalized() && !v2->marginalized()){
205  PoseMatrixType* m = _Hpp->block(ind1, ind2, true);
206  if (zeroBlocks)
207  m->setZero();
208  e->mapHessianMemory(m->data(), viIdx, vjIdx, transposedBlock);
209  if (_Hschur) {// assume this is only needed in case we solve with the schur complement
210  schurMatrixLookup->addBlock(ind1, ind2);
211  }
212  } else if (v1->marginalized() && v2->marginalized()){
213  // RAINER hmm.... should we ever reach this here????
214  LandmarkMatrixType* m = _Hll->block(ind1-_numPoses, ind2-_numPoses, true);
215  if (zeroBlocks)
216  m->setZero();
217  e->mapHessianMemory(m->data(), viIdx, vjIdx, false);
218  } else {
219  if (v1->marginalized()){
220  PoseLandmarkMatrixType* m = _Hpl->block(v2->hessianIndex(),v1->hessianIndex()-_numPoses, true);
221  if (zeroBlocks)
222  m->setZero();
223  e->mapHessianMemory(m->data(), viIdx, vjIdx, true); // transpose the block before writing to it
224  } else {
225  PoseLandmarkMatrixType* m = _Hpl->block(v1->hessianIndex(),v2->hessianIndex()-_numPoses, true);
226  if (zeroBlocks)
227  m->setZero();
228  e->mapHessianMemory(m->data(), viIdx, vjIdx, false); // directly the block
229  }
230  }
231  }
232  }
233  }
234 
235  if (! _doSchur)
236  return true;
237 
238  _DInvSchur->diagonal().resize(landmarkIdx);
240 
241  for (size_t i = 0; i < _optimizer->indexMapping().size(); ++i) {
243  if (v->marginalized()){
244  const HyperGraph::EdgeSet& vedges=v->edges();
245  for (HyperGraph::EdgeSet::const_iterator it1=vedges.begin(); it1!=vedges.end(); ++it1){
246  for (size_t i=0; i<(*it1)->vertices().size(); ++i)
247  {
248  OptimizableGraph::Vertex* v1= (OptimizableGraph::Vertex*) (*it1)->vertex(i);
249  if (v1->hessianIndex()==-1 || v1==v)
250  continue;
251  for (HyperGraph::EdgeSet::const_iterator it2=vedges.begin(); it2!=vedges.end(); ++it2){
252  for (size_t j=0; j<(*it2)->vertices().size(); ++j)
253  {
254  OptimizableGraph::Vertex* v2= (OptimizableGraph::Vertex*) (*it2)->vertex(j);
255  if (v2->hessianIndex()==-1 || v2==v)
256  continue;
257  int i1=v1->hessianIndex();
258  int i2=v2->hessianIndex();
259  if (i1<=i2) {
260  schurMatrixLookup->addBlock(i1, i2);
261  }
262  }
263  }
264  }
265  }
266  }
267  }
268 
269  _Hschur->takePatternFromHash(*schurMatrixLookup);
270  delete schurMatrixLookup;
272 
273  return true;
274 }
const std::vector< int > & rowBlockIndices() const
indices of the row blocks
SparseBlockMatrix< PoseMatrixType > * _Hschur
Definition: block_solver.h:153
int fillSparseBlockMatrixCCS(SparseBlockMatrixCCS< MatrixType > &blockCCS) const
Traits::LandmarkMatrixType LandmarkMatrixType
Definition: block_solver.h:102
SparseBlockMatrix< PoseMatrixType > * _Hpp
Definition: block_solver.h:149
SparseOptimizer * _optimizer
Definition: solver.h:136
SparseBlockMatrixDiagonal< LandmarkMatrixType > * _DInvSchur
Definition: block_solver.h:154
SparseMatrixBlock * block(int r, int c, bool alloc=false)
returns the block at location r,c. if alloc=true he block is created if it does not exist ...
void resize(int *blockPoseIndices, int numPoseBlocks, int *blockLandmarkIndices, int numLandmarkBlocks, int totalDim)
const EdgeContainer & activeEdges() const
the edges active in the current optimization
SparseBlockMatrixCCS< PoseLandmarkMatrixType > * _HplCCS
Definition: block_solver.h:156
SparseBlockMatrix< LandmarkMatrixType > * _Hll
Definition: block_solver.h:150
Traits::PoseMatrixType PoseMatrixType
Definition: block_solver.h:101
const DiagonalVector & diagonal() const
the block matrices per block-column
const std::vector< int > & colBlockIndices() const
indices of the column blocks
std::set< Edge * > EdgeSet
Definition: hyper_graph.h:135
class G2O_CORE_API Vertex
void takePatternFromHash(SparseBlockMatrixHashMap< MatrixType > &hashMatrix)
SparseBlockMatrixCCS< PoseMatrixType > * _HschurTransposedCCS
Definition: block_solver.h:157
class G2O_CORE_API Edge
Traits::PoseLandmarkMatrixType PoseLandmarkMatrixType
Definition: block_solver.h:103
int fillSparseBlockMatrixCCSTransposed(SparseBlockMatrixCCS< MatrixType > &blockCCS) const
const VertexContainer & indexMapping() const
the index mapping of the vertices
const std::vector< IntBlockMap > & blockCols() const
the block matrices per block-column
SparseBlockMatrix< PoseLandmarkMatrixType > * _Hpl
Definition: block_solver.h:151
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.

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

482 {
483  // clear b vector
484 # ifdef G2O_OPENMP
485 # pragma omp parallel for default (shared) if (_optimizer->indexMapping().size() > 1000)
486 # endif
487  for (int i = 0; i < static_cast<int>(_optimizer->indexMapping().size()); ++i) {
489  assert(v);
490  v->clearQuadraticForm();
491  }
492  _Hpp->clear();
493  if (_doSchur) {
494  _Hll->clear();
495  _Hpl->clear();
496  }
497 
498  // resetting the terms for the pairwise constraints
499  // built up the current system by storing the Hessian blocks in the edges and vertices
500 # ifndef G2O_OPENMP
501  // no threading, we do not need to copy the workspace
502  JacobianWorkspace& jacobianWorkspace = _optimizer->jacobianWorkspace();
503 # else
504  // if running with threads need to produce copies of the workspace for each thread
505  JacobianWorkspace jacobianWorkspace = _optimizer->jacobianWorkspace();
506 # pragma omp parallel for default (shared) firstprivate(jacobianWorkspace) if (_optimizer->activeEdges().size() > 100)
507 # endif
508  for (int k = 0; k < static_cast<int>(_optimizer->activeEdges().size()); ++k) {
510  e->linearizeOplus(jacobianWorkspace); // jacobian of the nodes' oplus (manifold)
511  e->constructQuadraticForm();
512 # ifndef NDEBUG
513  for (size_t i = 0; i < e->vertices().size(); ++i) {
514  const OptimizableGraph::Vertex* v = static_cast<const OptimizableGraph::Vertex*>(e->vertex(i));
515  if (! v->fixed()) {
516  bool hasANan = arrayHasNaN(jacobianWorkspace.workspaceForVertex(i), e->dimension() * v->dimension());
517  if (hasANan) {
518  std::cerr << "buildSystem(): NaN within Jacobian for edge " << e << " for vertex " << i << std::endl;
519  break;
520  }
521  }
522  }
523 # endif
524  }
525 
526  // flush the current system in a sparse block matrix
527 # ifdef G2O_OPENMP
528 # pragma omp parallel for default (shared) if (_optimizer->indexMapping().size() > 1000)
529 # endif
530  for (int i = 0; i < static_cast<int>(_optimizer->indexMapping().size()); ++i) {
532  int iBase = v->colInHessian();
533  if (v->marginalized())
534  iBase+=_sizePoses;
535  v->copyB(_b+iBase);
536  }
537 
538  return 0;
539 }
double * _b
Definition: solver.h:138
SparseBlockMatrix< PoseMatrixType > * _Hpp
Definition: block_solver.h:149
SparseOptimizer * _optimizer
Definition: solver.h:136
const EdgeContainer & activeEdges() const
the edges active in the current optimization
SparseBlockMatrix< LandmarkMatrixType > * _Hll
Definition: block_solver.h:150
void clear(bool dealloc=false)
this zeroes all the blocks. If dealloc=true the blocks are removed from memory
bool arrayHasNaN(const double *array, int size, int *nanIndex=0)
Definition: misc.h:177
JacobianWorkspace & jacobianWorkspace()
the workspace for storing the Jacobians of the graph
class G2O_CORE_API Vertex
class G2O_CORE_API Edge
const VertexContainer & indexMapping() const
the index mapping of the vertices
SparseBlockMatrix< PoseLandmarkMatrixType > * _Hpl
Definition: block_solver.h:151
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.

470 {
471  double t = get_monotonic_time();
472  bool ok = _linearSolver->solvePattern(spinv, blockIndices, *_Hpp);
473  G2OBatchStatistics* globalStats = G2OBatchStatistics::globalStats();
474  if (globalStats) {
475  globalStats->timeMarginals = get_monotonic_time() - t;
476  }
477  return ok;
478 }
double get_monotonic_time()
Definition: timeutil.cpp:113
virtual bool solvePattern(SparseBlockMatrix< MatrixXD > &spinv, const std::vector< std::pair< int, int > > &blockIndices, const SparseBlockMatrix< MatrixType > &A)
Definition: linear_solver.h:71
SparseBlockMatrix< PoseMatrixType > * _Hpp
Definition: block_solver.h:149
static G2OBatchStatistics * globalStats()
Definition: batch_stats.h:73
LinearSolver< PoseMatrixType > * _linearSolver
Definition: block_solver.h:159
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().

93 {
94  delete _Hpp;
95  _Hpp=0;
96  delete _Hll;
97  _Hll=0;
98  delete _Hpl;
99  _Hpl = 0;
100  delete _Hschur;
101  _Hschur=0;
102  delete _DInvSchur;
103  _DInvSchur=0;
104  delete[] _coefficients;
105  _coefficients = 0;
106  delete[] _bschur;
107  _bschur = 0;
108  delete _HplCCS;
109  _HplCCS = 0;
110  delete _HschurTransposedCCS;
112 }
SparseBlockMatrix< PoseMatrixType > * _Hschur
Definition: block_solver.h:153
SparseBlockMatrix< PoseMatrixType > * _Hpp
Definition: block_solver.h:149
SparseBlockMatrixDiagonal< LandmarkMatrixType > * _DInvSchur
Definition: block_solver.h:154
SparseBlockMatrixCCS< PoseLandmarkMatrixType > * _HplCCS
Definition: block_solver.h:156
SparseBlockMatrix< LandmarkMatrixType > * _Hll
Definition: block_solver.h:150
double * _coefficients
Definition: block_solver.h:170
SparseBlockMatrixCCS< PoseMatrixType > * _HschurTransposedCCS
Definition: block_solver.h:157
SparseBlockMatrix< PoseLandmarkMatrixType > * _Hpl
Definition: block_solver.h:151
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().

587 {
589  if (! online) {
590  if (_Hpp)
591  _Hpp->clear();
592  if (_Hpl)
593  _Hpl->clear();
594  if (_Hll)
595  _Hll->clear();
596  }
597  _linearSolver->init();
598  return true;
599 }
SparseBlockMatrix< PoseMatrixType > * _Hpp
Definition: block_solver.h:149
virtual bool init()=0
SparseOptimizer * _optimizer
Definition: solver.h:136
SparseBlockMatrix< LandmarkMatrixType > * _Hll
Definition: block_solver.h:150
void clear(bool dealloc=false)
this zeroes all the blocks. If dealloc=true the blocks are removed from memory
SparseOptimizer * optimizer() const
the optimizer (graph) on which the solver works
Definition: solver.h:106
SparseBlockMatrix< PoseLandmarkMatrixType > * _Hpl
Definition: block_solver.h:151
LinearSolver< PoseMatrixType > * _linearSolver
Definition: block_solver.h:159
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;}
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);}
SparseBlockMatrix< PoseMatrixType > * _Hpp
Definition: block_solver.h:149
void multiplySymmetricUpperTriangle(double *&dest, const double *src) const
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.

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

65 {
66  deallocate();
67 
68  resizeVector(s);
69 
70  if (_doSchur) {
71  // the following two are only used in schur
72  assert(_sizePoses > 0 && "allocating with wrong size");
73  _coefficients = new double [s];
74  _bschur = new double[_sizePoses];
75  }
76 
77  _Hpp=new PoseHessianType(blockPoseIndices, blockPoseIndices, numPoseBlocks, numPoseBlocks);
78  if (_doSchur) {
79  _Hschur=new PoseHessianType(blockPoseIndices, blockPoseIndices, numPoseBlocks, numPoseBlocks);
80  _Hll=new LandmarkHessianType(blockLandmarkIndices, blockLandmarkIndices, numLandmarkBlocks, numLandmarkBlocks);
81  _DInvSchur = new SparseBlockMatrixDiagonal<LandmarkMatrixType>(_Hll->colBlockIndices());
82  _Hpl=new PoseLandmarkHessianType(blockPoseIndices, blockLandmarkIndices, numPoseBlocks, numLandmarkBlocks);
83  _HplCCS = new SparseBlockMatrixCCS<PoseLandmarkMatrixType>(_Hpl->rowBlockIndices(), _Hpl->colBlockIndices());
84  _HschurTransposedCCS = new SparseBlockMatrixCCS<PoseMatrixType>(_Hschur->colBlockIndices(), _Hschur->rowBlockIndices());
85 #ifdef G2O_OPENMP
86  _coefficientsMutex.resize(numPoseBlocks);
87 #endif
88  }
89 }
const std::vector< int > & rowBlockIndices() const
indices of the row blocks
SparseBlockMatrix< PoseMatrixType > * _Hschur
Definition: block_solver.h:153
Traits::PoseHessianType PoseHessianType
Definition: block_solver.h:107
SparseBlockMatrix< PoseMatrixType > * _Hpp
Definition: block_solver.h:149
Traits::LandmarkHessianType LandmarkHessianType
Definition: block_solver.h:108
SparseBlockMatrixDiagonal< LandmarkMatrixType > * _DInvSchur
Definition: block_solver.h:154
SparseBlockMatrixCCS< PoseLandmarkMatrixType > * _HplCCS
Definition: block_solver.h:156
SparseBlockMatrix< LandmarkMatrixType > * _Hll
Definition: block_solver.h:150
double * _coefficients
Definition: block_solver.h:170
const std::vector< int > & colBlockIndices() const
indices of the column blocks
SparseBlockMatrixCCS< PoseMatrixType > * _HschurTransposedCCS
Definition: block_solver.h:157
Traits::PoseLandmarkHessianType PoseLandmarkHessianType
Definition: block_solver.h:109
void resizeVector(size_t sx)
Definition: solver.cpp:46
SparseBlockMatrix< PoseLandmarkMatrixType > * _Hpl
Definition: block_solver.h:151
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().

572 {
573  assert((int) _diagonalBackupPose.size() == _numPoses && "Mismatch in dimensions");
574  assert((int) _diagonalBackupLandmark.size() == _numLandmarks && "Mismatch in dimensions");
575  for (int i = 0; i < _numPoses; ++i) {
576  PoseMatrixType *b=_Hpp->block(i,i);
577  b->diagonal() = _diagonalBackupPose[i];
578  }
579  for (int i = 0; i < _numLandmarks; ++i) {
580  LandmarkMatrixType *b=_Hll->block(i,i);
581  b->diagonal() = _diagonalBackupLandmark[i];
582  }
583 }
std::vector< LandmarkVectorType, Eigen::aligned_allocator< LandmarkVectorType > > _diagonalBackupLandmark
Definition: block_solver.h:162
Traits::LandmarkMatrixType LandmarkMatrixType
Definition: block_solver.h:102
SparseBlockMatrix< PoseMatrixType > * _Hpp
Definition: block_solver.h:149
SparseMatrixBlock * block(int r, int c, bool alloc=false)
returns the block at location r,c. if alloc=true he block is created if it does not exist ...
SparseBlockMatrix< LandmarkMatrixType > * _Hll
Definition: block_solver.h:150
Traits::PoseMatrixType PoseMatrixType
Definition: block_solver.h:101
std::vector< PoseVectorType, Eigen::aligned_allocator< PoseVectorType > > _diagonalBackupPose
Definition: block_solver.h:161
double * b()
return b, the right hand side of the system
Definition: solver.h:99
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().

609 {
610  return _Hpp->writeOctave(fileName.c_str(), true);
611 }
SparseBlockMatrix< PoseMatrixType > * _Hpp
Definition: block_solver.h:149
bool writeOctave(const char *filename, bool upperTriangle=true) const
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;}
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().

544 {
545  if (backup) {
548  }
549 # ifdef G2O_OPENMP
550 # pragma omp parallel for default (shared) if (_numPoses > 100)
551 # endif
552  for (int i = 0; i < _numPoses; ++i) {
553  PoseMatrixType *b=_Hpp->block(i,i);
554  if (backup)
555  _diagonalBackupPose[i] = b->diagonal();
556  b->diagonal().array() += lambda;
557  }
558 # ifdef G2O_OPENMP
559 # pragma omp parallel for default (shared) if (_numLandmarks > 100)
560 # endif
561  for (int i = 0; i < _numLandmarks; ++i) {
562  LandmarkMatrixType *b=_Hll->block(i,i);
563  if (backup)
564  _diagonalBackupLandmark[i] = b->diagonal();
565  b->diagonal().array() += lambda;
566  }
567  return true;
568 }
std::vector< LandmarkVectorType, Eigen::aligned_allocator< LandmarkVectorType > > _diagonalBackupLandmark
Definition: block_solver.h:162
Traits::LandmarkMatrixType LandmarkMatrixType
Definition: block_solver.h:102
SparseBlockMatrix< PoseMatrixType > * _Hpp
Definition: block_solver.h:149
SparseMatrixBlock * block(int r, int c, bool alloc=false)
returns the block at location r,c. if alloc=true he block is created if it does not exist ...
SparseBlockMatrix< LandmarkMatrixType > * _Hll
Definition: block_solver.h:150
Traits::PoseMatrixType PoseMatrixType
Definition: block_solver.h:101
std::vector< PoseVectorType, Eigen::aligned_allocator< PoseVectorType > > _diagonalBackupPose
Definition: block_solver.h:161
double * b()
return b, the right hand side of the system
Definition: solver.h:99
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;}
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().

603 {
605 }
virtual void setWriteDebug(bool)
Definition: linear_solver.h:80
LinearSolver< PoseMatrixType > * _linearSolver
Definition: block_solver.h:159
virtual bool writeDebug() const
Definition: block_solver.h:137
template<typename Traits >
bool g2o::BlockSolver< Traits >::solve ( )
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.

333  {
334  //cerr << __PRETTY_FUNCTION__ << endl;
335  if (! _doSchur){
336  double t=get_monotonic_time();
337  bool ok = _linearSolver->solve(*_Hpp, _x, _b);
338  G2OBatchStatistics* globalStats = G2OBatchStatistics::globalStats();
339  if (globalStats) {
340  globalStats->timeLinearSolver = get_monotonic_time() - t;
341  globalStats->hessianDimension = globalStats->hessianPoseDimension = _Hpp->cols();
342  }
343  return ok;
344  }
345 
346  // schur thing
347 
348  // backup the coefficient matrix
349  double t=get_monotonic_time();
350 
351  // _Hschur = _Hpp, but keeping the pattern of _Hschur
352  _Hschur->clear();
353  _Hpp->add(_Hschur);
354 
355  //_DInvSchur->clear();
356  memset (_coefficients, 0, _sizePoses*sizeof(double));
357 # ifdef G2O_OPENMP
358 # pragma omp parallel for default (shared) schedule(dynamic, 10)
359 # endif
360  for (int landmarkIndex = 0; landmarkIndex < static_cast<int>(_Hll->blockCols().size()); ++landmarkIndex) {
361  const typename SparseBlockMatrix<LandmarkMatrixType>::IntBlockMap& marginalizeColumn = _Hll->blockCols()[landmarkIndex];
362  assert(marginalizeColumn.size() == 1 && "more than one block in _Hll column");
363 
364  // calculate inverse block for the landmark
365  const LandmarkMatrixType * D = marginalizeColumn.begin()->second;
366  assert (D && D->rows()==D->cols() && "Error in landmark matrix");
367  LandmarkMatrixType& Dinv = _DInvSchur->diagonal()[landmarkIndex];
368  Dinv = D->inverse();
369 
370  LandmarkVectorType db(D->rows());
371  for (int j=0; j<D->rows(); ++j) {
372  db[j]=_b[_Hll->rowBaseOfBlock(landmarkIndex) + _sizePoses + j];
373  }
374  db=Dinv*db;
375 
376  assert((size_t)landmarkIndex < _HplCCS->blockCols().size() && "Index out of bounds");
377  const typename SparseBlockMatrixCCS<PoseLandmarkMatrixType>::SparseColumn& landmarkColumn = _HplCCS->blockCols()[landmarkIndex];
378 
379  for (typename SparseBlockMatrixCCS<PoseLandmarkMatrixType>::SparseColumn::const_iterator it_outer = landmarkColumn.begin();
380  it_outer != landmarkColumn.end(); ++it_outer) {
381  int i1 = it_outer->row;
382 
383  const PoseLandmarkMatrixType* Bi = it_outer->block;
384  assert(Bi);
385 
386  PoseLandmarkMatrixType BDinv = (*Bi)*(Dinv);
387  assert(_HplCCS->rowBaseOfBlock(i1) < _sizePoses && "Index out of bounds");
388  typename PoseVectorType::MapType Bb(&_coefficients[_HplCCS->rowBaseOfBlock(i1)], Bi->rows());
389 # ifdef G2O_OPENMP
390  ScopedOpenMPMutex mutexLock(&_coefficientsMutex[i1]);
391 # endif
392  Bb.noalias() += (*Bi)*db;
393 
394  assert(i1 >= 0 && i1 < static_cast<int>(_HschurTransposedCCS->blockCols().size()) && "Index out of bounds");
395  typename SparseBlockMatrixCCS<PoseMatrixType>::SparseColumn::iterator targetColumnIt = _HschurTransposedCCS->blockCols()[i1].begin();
396 
397  typename SparseBlockMatrixCCS<PoseLandmarkMatrixType>::RowBlock aux(i1, 0);
398  typename SparseBlockMatrixCCS<PoseLandmarkMatrixType>::SparseColumn::const_iterator it_inner = lower_bound(landmarkColumn.begin(), landmarkColumn.end(), aux);
399  for (; it_inner != landmarkColumn.end(); ++it_inner) {
400  int i2 = it_inner->row;
401  const PoseLandmarkMatrixType* Bj = it_inner->block;
402  assert(Bj);
403  while (targetColumnIt->row < i2 /*&& targetColumnIt != _HschurTransposedCCS->blockCols()[i1].end()*/)
404  ++targetColumnIt;
405  assert(targetColumnIt != _HschurTransposedCCS->blockCols()[i1].end() && targetColumnIt->row == i2 && "invalid iterator, something wrong with the matrix structure");
406  PoseMatrixType* Hi1i2 = targetColumnIt->block;//_Hschur->block(i1,i2);
407  assert(Hi1i2);
408  (*Hi1i2).noalias() -= BDinv*Bj->transpose();
409  }
410  }
411  }
412  //cerr << "Solve [marginalize] = " << get_monotonic_time()-t << endl;
413 
414  // _bschur = _b for calling solver, and not touching _b
415  memcpy(_bschur, _b, _sizePoses * sizeof(double));
416  for (int i=0; i<_sizePoses; ++i){
417  _bschur[i]-=_coefficients[i];
418  }
419 
420  G2OBatchStatistics* globalStats = G2OBatchStatistics::globalStats();
421  if (globalStats){
422  globalStats->timeSchurComplement = get_monotonic_time() - t;
423  }
424 
425  t=get_monotonic_time();
426  bool solvedPoses = _linearSolver->solve(*_Hschur, _x, _bschur);
427  if (globalStats) {
428  globalStats->timeLinearSolver = get_monotonic_time() - t;
429  globalStats->hessianPoseDimension = _Hpp->cols();
430  globalStats->hessianLandmarkDimension = _Hll->cols();
431  globalStats->hessianDimension = globalStats->hessianPoseDimension + globalStats->hessianLandmarkDimension;
432  }
433  //cerr << "Solve [decompose and solve] = " << get_monotonic_time()-t << endl;
434 
435  if (! solvedPoses)
436  return false;
437 
438  // _x contains the solution for the poses, now applying it to the landmarks to get the new part of the
439  // solution;
440  double* xp = _x;
441  double* cp = _coefficients;
442 
443  double* xl=_x+_sizePoses;
444  double* cl=_coefficients + _sizePoses;
445  double* bl=_b+_sizePoses;
446 
447  // cp = -xp
448  for (int i=0; i<_sizePoses; ++i)
449  cp[i]=-xp[i];
450 
451  // cl = bl
452  memcpy(cl,bl,_sizeLandmarks*sizeof(double));
453 
454  // cl = bl - Bt * xp
455  //Bt->multiply(cl, cp);
456  _HplCCS->rightMultiply(cl, cp);
457 
458  // xl = Dinv * cl
459  memset(xl,0, _sizeLandmarks*sizeof(double));
460  _DInvSchur->multiply(xl,cl);
461  //_DInvSchur->rightMultiply(xl,cl);
462  //cerr << "Solve [landmark delta] = " << get_monotonic_time()-t << endl;
463 
464  return true;
465 }
double * _b
Definition: solver.h:138
SparseBlockMatrix< PoseMatrixType > * _Hschur
Definition: block_solver.h:153
double get_monotonic_time()
Definition: timeutil.cpp:113
Traits::LandmarkMatrixType LandmarkMatrixType
Definition: block_solver.h:102
virtual bool solve(const SparseBlockMatrix< MatrixType > &A, double *x, double *b)=0
int cols() const
columns of the matrix
SparseBlockMatrix< PoseMatrixType > * _Hpp
Definition: block_solver.h:149
void rightMultiply(double *&dest, const double *src) const
SparseBlockMatrixDiagonal< LandmarkMatrixType > * _DInvSchur
Definition: block_solver.h:154
SparseBlockMatrixCCS< PoseLandmarkMatrixType > * _HplCCS
Definition: block_solver.h:156
SparseBlockMatrix< LandmarkMatrixType > * _Hll
Definition: block_solver.h:150
void clear(bool dealloc=false)
this zeroes all the blocks. If dealloc=true the blocks are removed from memory
Traits::PoseMatrixType PoseMatrixType
Definition: block_solver.h:101
double * _coefficients
Definition: block_solver.h:170
double * _x
Definition: solver.h:137
const DiagonalVector & diagonal() const
the block matrices per block-column
SparseBlockMatrixCCS< PoseMatrixType > * _HschurTransposedCCS
Definition: block_solver.h:157
Traits::PoseLandmarkMatrixType PoseLandmarkMatrixType
Definition: block_solver.h:103
Traits::LandmarkVectorType LandmarkVectorType
Definition: block_solver.h:105
static G2OBatchStatistics * globalStats()
Definition: batch_stats.h:73
void multiply(double *&dest, const double *src) const
std::map< int, SparseMatrixBlock * > IntBlockMap
const std::vector< IntBlockMap > & blockCols() const
the block matrices per block-column
LinearSolver< PoseMatrixType > * _linearSolver
Definition: block_solver.h:159
bool add(SparseBlockMatrix< MatrixType > *&dest) const
adds the current matrix to the destination
const std::vector< SparseColumn > & blockCols() const
the block matrices per block-column
int rowBaseOfBlock(int r) const
where does the row at block-row r start?
int rowBaseOfBlock(int r) const
where does the row at block-row r starts?
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().

278 {
279  for (std::vector<HyperGraph::Vertex*>::const_iterator vit = vset.begin(); vit != vset.end(); ++vit) {
280  OptimizableGraph::Vertex* v = static_cast<OptimizableGraph::Vertex*>(*vit);
281  int dim = v->dimension();
282  if (! v->marginalized()){
283  v->setColInHessian(_sizePoses);
284  _sizePoses+=dim;
285  _Hpp->rowBlockIndices().push_back(_sizePoses);
286  _Hpp->colBlockIndices().push_back(_sizePoses);
288  ++_numPoses;
289  int ind = v->hessianIndex();
290  PoseMatrixType* m = _Hpp->block(ind, ind, true);
291  v->mapHessianMemory(m->data());
292  } else {
293  std::cerr << "updateStructure(): Schur not supported" << std::endl;
294  abort();
295  }
296  }
298 
299  for (HyperGraph::EdgeSet::const_iterator it = edges.begin(); it != edges.end(); ++it) {
300  OptimizableGraph::Edge* e = static_cast<OptimizableGraph::Edge*>(*it);
301 
302  for (size_t viIdx = 0; viIdx < e->vertices().size(); ++viIdx) {
303  OptimizableGraph::Vertex* v1 = (OptimizableGraph::Vertex*) e->vertex(viIdx);
304  int ind1 = v1->hessianIndex();
305  int indexV1Bak = ind1;
306  if (ind1 == -1)
307  continue;
308  for (size_t vjIdx = viIdx + 1; vjIdx < e->vertices().size(); ++vjIdx) {
309  OptimizableGraph::Vertex* v2 = (OptimizableGraph::Vertex*) e->vertex(vjIdx);
310  int ind2 = v2->hessianIndex();
311  if (ind2 == -1)
312  continue;
313  ind1 = indexV1Bak;
314  bool transposedBlock = ind1 > ind2;
315  if (transposedBlock) // make sure, we allocate the upper triangular block
316  std::swap(ind1, ind2);
317 
318  if (! v1->marginalized() && !v2->marginalized()) {
319  PoseMatrixType* m = _Hpp->block(ind1, ind2, true);
320  e->mapHessianMemory(m->data(), viIdx, vjIdx, transposedBlock);
321  } else {
322  std::cerr << __PRETTY_FUNCTION__ << ": not supported" << std::endl;
323  }
324  }
325  }
326 
327  }
328 
329  return true;
330 }
const std::vector< int > & rowBlockIndices() const
indices of the row blocks
#define __PRETTY_FUNCTION__
Definition: macros.h:89
SparseBlockMatrix< PoseMatrixType > * _Hpp
Definition: block_solver.h:149
SparseMatrixBlock * block(int r, int c, bool alloc=false)
returns the block at location r,c. if alloc=true he block is created if it does not exist ...
Traits::PoseMatrixType PoseMatrixType
Definition: block_solver.h:101
const std::vector< int > & colBlockIndices() const
indices of the column blocks
class G2O_CORE_API Vertex
class G2O_CORE_API Edge
void resizeVector(size_t sx)
Definition: solver.cpp:46
std::map< int, SparseMatrixBlock * > IntBlockMap
const std::vector< IntBlockMap > & blockCols() const
the block matrices per block-column
template<typename Traits>
virtual bool g2o::BlockSolver< Traits >::writeDebug ( ) const
inlinevirtual

Implements g2o::Solver.

Definition at line 137 of file block_solver.h.

137 {return _linearSolver->writeDebug();}
virtual bool writeDebug() const
write a debug dump of the system matrix if it is not PSD in solve
Definition: linear_solver.h:79
LinearSolver< PoseMatrixType > * _linearSolver
Definition: block_solver.h:159

Member Data Documentation

template<typename Traits>
double* g2o::BlockSolver< Traits >::_bschur
protected
template<typename Traits>
double* g2o::BlockSolver< Traits >::_coefficients
protected
template<typename Traits>
std::vector<LandmarkVectorType, Eigen::aligned_allocator<LandmarkVectorType> > g2o::BlockSolver< Traits >::_diagonalBackupLandmark
protected
template<typename Traits>
std::vector<PoseVectorType, Eigen::aligned_allocator<PoseVectorType> > g2o::BlockSolver< Traits >::_diagonalBackupPose
protected
template<typename Traits>
SparseBlockMatrixDiagonal<LandmarkMatrixType>* g2o::BlockSolver< Traits >::_DInvSchur
protected
template<typename Traits>
bool g2o::BlockSolver< Traits >::_doSchur
protected
template<typename Traits>
SparseBlockMatrix<LandmarkMatrixType>* g2o::BlockSolver< Traits >::_Hll
protected
template<typename Traits>
SparseBlockMatrix<PoseLandmarkMatrixType>* g2o::BlockSolver< Traits >::_Hpl
protected
template<typename Traits>
SparseBlockMatrixCCS<PoseLandmarkMatrixType>* g2o::BlockSolver< Traits >::_HplCCS
protected
template<typename Traits>
SparseBlockMatrix<PoseMatrixType>* g2o::BlockSolver< Traits >::_Hpp
protected
template<typename Traits>
SparseBlockMatrix<PoseMatrixType>* g2o::BlockSolver< Traits >::_Hschur
protected
template<typename Traits>
SparseBlockMatrixCCS<PoseMatrixType>* g2o::BlockSolver< Traits >::_HschurTransposedCCS
protected
template<typename Traits>
LinearSolver<PoseMatrixType>* g2o::BlockSolver< Traits >::_linearSolver
protected
template<typename Traits>
int g2o::BlockSolver< Traits >::_numLandmarks
protected
template<typename Traits>
int g2o::BlockSolver< Traits >::_numPoses
protected
template<typename Traits>
int g2o::BlockSolver< Traits >::_sizeLandmarks
protected
template<typename Traits>
int g2o::BlockSolver< Traits >::_sizePoses
protected
template<typename Traits>
const int g2o::BlockSolver< Traits >::LandmarkDim = Traits::LandmarkDim
static

Definition at line 100 of file block_solver.h.

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

Definition at line 99 of file block_solver.h.


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