Sparse matrix which uses blocks. More...

#include <sparse_block_matrix.h>

Public Types
typedef MatrixType	SparseMatrixBlock
	this is the type of the elementary block, it is an Eigen::Matrix. More...

typedef std::map< int, SparseMatrixBlock * >	IntBlockMap

Public Member Functions
int	cols () const
	columns of the matrix More...

int	rows () const
	rows of the matrix More...

	SparseBlockMatrix (const int rbi, const int cbi, int rb, int cb, bool hasStorage=true)

	SparseBlockMatrix ()

	~SparseBlockMatrix ()

void	clear (bool dealloc=false)
	this zeroes all the blocks. If dealloc=true the blocks are removed from memory More...

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

const SparseMatrixBlock *	block (int r, int c) const
	returns the block at location r,c More...

int	rowsOfBlock (int r) const
	how many rows does the block at block-row r has? More...

int	colsOfBlock (int c) const
	how many cols does the block at block-col c has? More...

int	rowBaseOfBlock (int r) const
	where does the row at block-row r starts? More...

int	colBaseOfBlock (int c) const
	where does the col at block-col r starts? More...

size_t	nonZeros () const
	number of non-zero elements More...

size_t	nonZeroBlocks () const
	number of allocated blocks More...

SparseBlockMatrix *	clone () const
	deep copy of a sparse-block-matrix; More...

SparseBlockMatrix *	slice (int rmin, int rmax, int cmin, int cmax, bool alloc=true) const

template<class MatrixTransposedType >
bool	transpose (SparseBlockMatrix< MatrixTransposedType > *&dest) const
	transposes a block matrix, The transposed type should match the argument false on failure More...

bool	add (SparseBlockMatrix< MatrixType > *&dest) const
	adds the current matrix to the destination More...

template<class MatrixResultType , class MatrixFactorType >
bool	multiply (SparseBlockMatrix< MatrixResultType > &dest, const SparseBlockMatrix< MatrixFactorType > M) const
	dest = (this) M More...

void	multiply (double &dest, const double src) const
	dest = (this) src More...

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

void	rightMultiply (double &dest, const double src) const
	dest = M * (*this) More...

void	scale (double a)
	this = a More...

bool	symmPermutation (SparseBlockMatrix< MatrixType > &dest, const int pinv, bool onlyUpper=false) const

int	fillCCS (int Cp, int Ci, double *Cx, bool upperTriangle=false) const

int	fillCCS (double *Cx, bool upperTriangle=false) const

void	fillBlockStructure (MatrixStructure &ms) const
	exports the non zero blocks in the structure matrix ms More...

const std::vector< IntBlockMap > &	blockCols () const
	the block matrices per block-column More...

std::vector< IntBlockMap > &	blockCols ()

const std::vector< int > &	rowBlockIndices () const
	indices of the row blocks More...

std::vector< int > &	rowBlockIndices ()

const std::vector< int > &	colBlockIndices () const
	indices of the column blocks More...

std::vector< int > &	colBlockIndices ()

bool	writeOctave (const char *filename, bool upperTriangle=true) const

int	fillSparseBlockMatrixCCS (SparseBlockMatrixCCS< MatrixType > &blockCCS) const

int	fillSparseBlockMatrixCCSTransposed (SparseBlockMatrixCCS< MatrixType > &blockCCS) const

void	takePatternFromHash (SparseBlockMatrixHashMap< MatrixType > &hashMatrix)

Protected Attributes
std::vector< int >	_rowBlockIndices
	vector of the indices of the blocks along the rows. More...

std::vector< int >	_colBlockIndices

std::vector< IntBlockMap >	_blockCols

bool	_hasStorage

Detailed Description

template<class MatrixType = MatrixXD>
class g2o::SparseBlockMatrix< MatrixType >

Sparse matrix which uses blocks.

Template class that specifies a sparse block matrix. A block matrix is a sparse matrix made of dense blocks. These blocks cannot have a random pattern, but follow a (variable) grid structure. This structure is specified by a partition of the rows and the columns of the matrix. The blocks are represented by the Eigen::Matrix structure, thus they can be statically or dynamically allocated. For efficiency reasons it is convenient to allocate them statically, when possible. A static block matrix has all blocks of the same size, and the size of the block is specified by the template argument. If this is not the case, and you have different block sizes than you have to use a dynamic-block matrix (default template argument).

Definition at line 61 of file sparse_block_matrix.h.

Member Typedef Documentation

template<class MatrixType = MatrixXD>

typedef std::map<int, SparseMatrixBlock*> g2o::SparseBlockMatrix< MatrixType >::IntBlockMap

Definition at line 72 of file sparse_block_matrix.h.

template<class MatrixType = MatrixXD>

typedef MatrixType g2o::SparseBlockMatrix< MatrixType >::SparseMatrixBlock

this is the type of the elementary block, it is an Eigen::Matrix.

Definition at line 65 of file sparse_block_matrix.h.

Constructor & Destructor Documentation

template<class MatrixType >

g2o::SparseBlockMatrix< MatrixType >::SparseBlockMatrix	(	const int *	rbi,
		const int *	cbi,
		int	rb,
		int	cb,
		bool	hasStorage = `true`
	)

constructs a sparse block matrix having a specific layout

Parameters

rbi	array of int containing the row layout of the blocks. the component i of the array should contain the index of the first row of the block i+1.
rbi	array of int containing the column layout of the blocks. the component i of the array should contain the index of the first col of the block i+1.
rb	number of row blocks
cb	number of col blocks
hasStorage	set it to true if the matrix "owns" the blocks, thus it deletes it on destruction. if false the matrix is only a "view" over an existing structure.

Definition at line 53 of file sparse_block_matrix.hpp.

                                                                                                                    :
     _rowBlockIndices(rbi,rbi+rb),
     _colBlockIndices(cbi,cbi+cb),
     _blockCols(cb), _hasStorage(hasStorage) 
   {
   }

template<class MatrixType >

g2o::SparseBlockMatrix< MatrixType >::SparseBlockMatrix ( )

Definition at line 61 of file sparse_block_matrix.hpp.

Referenced by g2o::SparseBlockMatrix< MatrixType >::add(), g2o::SparseBlockMatrix< MatrixType >::clone(), g2o::SparseBlockMatrix< MatrixType >::slice(), and g2o::SparseBlockMatrix< MatrixType >::symmPermutation().

                                                    :
     _blockCols(0), _hasStorage(true) 
   {
   }

template<class MatrixType >

g2o::SparseBlockMatrix< MatrixType >::~SparseBlockMatrix ( )

Definition at line 85 of file sparse_block_matrix.hpp.

References g2o::SparseBlockMatrix< MatrixType >::_hasStorage, and g2o::SparseBlockMatrix< MatrixType >::clear().

                                                    {
     if (_hasStorage)
       clear(true);
   }

Member Function Documentation

template<class MatrixType>

bool g2o::SparseBlockMatrix< MatrixType >::add ( SparseBlockMatrix< MatrixType > *& dest ) const

adds the current matrix to the destination

Definition at line 168 of file sparse_block_matrix.hpp.

References g2o::SparseBlockMatrix< MatrixType >::_blockCols, g2o::SparseBlockMatrix< MatrixType >::_colBlockIndices, g2o::SparseBlockMatrix< MatrixType >::_hasStorage, g2o::SparseBlockMatrix< MatrixType >::_rowBlockIndices, g2o::SparseBlockMatrix< MatrixType >::block(), and g2o::SparseBlockMatrix< MatrixType >::SparseBlockMatrix().

Referenced by g2o::SparseBlockMatrix< PoseMatrixType >::colBaseOfBlock(), main(), and g2o::BlockSolver< Traits >::solve().

                                                                         {
     if (! dest){
       dest=new SparseBlockMatrix(&_rowBlockIndices[0], &_colBlockIndices[0], _rowBlockIndices.size(), _colBlockIndices.size());
     } else {
       if (! dest->_hasStorage)
         return false;
       if(_rowBlockIndices.size()!=dest->_rowBlockIndices.size())
         return false;
       if (_colBlockIndices.size()!=dest->_colBlockIndices.size())
         return  false;
       for (size_t i=0; i<_rowBlockIndices.size(); ++i){
         if(_rowBlockIndices[i]!=dest->_rowBlockIndices[i])
           return false;
       }
       for (size_t i=0; i<_colBlockIndices.size(); ++i){
         if(_colBlockIndices[i]!=dest->_colBlockIndices[i])
           return false;
       }
     }
     for (size_t i=0; i<_blockCols.size(); ++i){
       for (typename SparseBlockMatrix<MatrixType>::IntBlockMap::const_iterator it=_blockCols[i].begin(); it!=_blockCols[i].end(); ++it){
         typename SparseBlockMatrix<MatrixType>::SparseMatrixBlock* s=it->second;
         typename SparseBlockMatrix<MatrixType>::SparseMatrixBlock* d=dest->block(it->first,i,true);
         (*d)+=*s;
       }
     }
     return true;
   }

template<class MatrixType >

SparseBlockMatrix< MatrixType >::SparseMatrixBlock * g2o::SparseBlockMatrix< MatrixType >::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

Definition at line 91 of file sparse_block_matrix.hpp.

References g2o::SparseBlockMatrix< MatrixType >::_blockCols, g2o::SparseBlockMatrix< MatrixType >::_hasStorage, g2o::SparseBlockMatrix< MatrixType >::colsOfBlock(), and g2o::SparseBlockMatrix< MatrixType >::rowsOfBlock().

Referenced by g2o::SparseBlockMatrix< MatrixType >::add(), g2o::BlockSolver< Traits >::buildStructure(), g2o::MarginalCovarianceCholesky::computeCovariance(), g2o::EdgeLabeler::labelEdge(), main(), g2o::SparseBlockMatrix< MatrixType >::multiply(), g2o::BlockSolver< Traits >::restoreDiagonal(), g2o::BlockSolver< Traits >::setLambda(), g2o::SparseBlockMatrix< MatrixType >::symmPermutation(), testMarginals(), g2o::SparseBlockMatrix< MatrixType >::transpose(), and g2o::BlockSolver< Traits >::updateStructure().

                                                                                                                         {
     typename SparseBlockMatrix<MatrixType>::IntBlockMap::iterator it =_blockCols[c].find(r);
     typename SparseBlockMatrix<MatrixType>::SparseMatrixBlock* _block=0;
     if (it==_blockCols[c].end()){
       if (!_hasStorage && ! alloc )
         return 0;
       else {
         int rb=rowsOfBlock(r);
         int cb=colsOfBlock(c);
         _block=new typename SparseBlockMatrix<MatrixType>::SparseMatrixBlock(rb,cb);
         _block->setZero();
         std::pair < typename SparseBlockMatrix<MatrixType>::IntBlockMap::iterator, bool> result
           =_blockCols[c].insert(std::make_pair(r,_block)); (void) result;
         assert (result.second);
       }
     } else {
       _block=it->second;
     }
     return _block;
   }

template<class MatrixType >

const SparseBlockMatrix< MatrixType >::SparseMatrixBlock * g2o::SparseBlockMatrix< MatrixType >::block	(	int	r,
		int	c
	)		const

returns the block at location r,c

Definition at line 113 of file sparse_block_matrix.hpp.

References g2o::SparseBlockMatrix< MatrixType >::_blockCols.

                                                                                                                         {
     typename SparseBlockMatrix<MatrixType>::IntBlockMap::const_iterator it =_blockCols[c].find(r);
     if (it==_blockCols[c].end())
   return 0;
     return it->second;
   }

template<class MatrixType = MatrixXD>

const std::vector<IntBlockMap>& g2o::SparseBlockMatrix< MatrixType >::blockCols ( ) const

inline

the block matrices per block-column

Definition at line 177 of file sparse_block_matrix.h.

Referenced by g2o::BlockSolver< Traits >::buildStructure(), g2o::LinearSolverEigen< MatrixType >::computeSymbolicDecomposition(), g2o::SparseBlockMatrix< MatrixType >::fillSparseBlockMatrixCCS(), g2o::SparseBlockMatrix< MatrixType >::fillSparseBlockMatrixCCSTransposed(), g2o::LinearSolverEigen< MatrixType >::fillSparseMatrix(), g2o::operator<<(), g2o::LinearSolverDense< MatrixType >::solve(), g2o::BlockSolver< Traits >::solve(), g2o::SparseBlockMatrix< MatrixType >::takePatternFromHash(), and g2o::BlockSolver< Traits >::updateStructure().

177 { return _blockCols;}

g2o::SparseBlockMatrix::_blockCols

std::vector< IntBlockMap > _blockCols

Definition: sparse_block_matrix.h:217

template<class MatrixType = MatrixXD>

std::vector<IntBlockMap>& g2o::SparseBlockMatrix< MatrixType >::blockCols ( )

inline

Definition at line 178 of file sparse_block_matrix.h.

178 { return _blockCols;}

g2o::SparseBlockMatrix::_blockCols

std::vector< IntBlockMap > _blockCols

Definition: sparse_block_matrix.h:217

template<class MatrixType >

void g2o::SparseBlockMatrix< MatrixType >::clear ( bool dealloc = false )

this zeroes all the blocks. If dealloc=true the blocks are removed from memory

Definition at line 67 of file sparse_block_matrix.hpp.

References g2o::SparseBlockMatrix< MatrixType >::_blockCols, and g2o::SparseBlockMatrix< MatrixType >::_hasStorage.

Referenced by g2o::BlockSolver< Traits >::buildSystem(), g2o::BlockSolver< Traits >::init(), main(), g2o::BlockSolver< Traits >::solve(), g2o::SparseBlockMatrix< MatrixType >::symmPermutation(), and g2o::SparseBlockMatrix< MatrixType >::~SparseBlockMatrix().

                                                         {
 #   ifdef G2O_OPENMP
 #   pragma omp parallel for default (shared) if (_blockCols.size() > 100)
 #   endif
     for (int i=0; i < static_cast<int>(_blockCols.size()); ++i) {
       for (typename SparseBlockMatrix<MatrixType>::IntBlockMap::const_iterator it=_blockCols[i].begin(); it!=_blockCols[i].end(); ++it){
         typename SparseBlockMatrix<MatrixType>::SparseMatrixBlock* b=it->second;
         if (_hasStorage && dealloc)
           delete b;
         else
           b->setZero();
       }
       if (_hasStorage && dealloc)
         _blockCols[i].clear();
     }
   }

template<class MatrixType >

SparseBlockMatrix< MatrixType > * g2o::SparseBlockMatrix< MatrixType >::clone ( ) const

deep copy of a sparse-block-matrix;

Definition at line 122 of file sparse_block_matrix.hpp.

References g2o::SparseBlockMatrix< MatrixType >::_blockCols, g2o::SparseBlockMatrix< MatrixType >::_colBlockIndices, g2o::SparseBlockMatrix< MatrixType >::_hasStorage, g2o::SparseBlockMatrix< MatrixType >::_rowBlockIndices, and g2o::SparseBlockMatrix< MatrixType >::SparseBlockMatrix().

Referenced by g2o::SparseBlockMatrix< PoseMatrixType >::colBaseOfBlock().

                                                                             {
     SparseBlockMatrix* ret= new SparseBlockMatrix(&_rowBlockIndices[0], &_colBlockIndices[0], _rowBlockIndices.size(), _colBlockIndices.size());
     for (size_t i=0; i<_blockCols.size(); ++i){
       for (typename SparseBlockMatrix<MatrixType>::IntBlockMap::const_iterator it=_blockCols[i].begin(); it!=_blockCols[i].end(); ++it){
         typename SparseBlockMatrix<MatrixType>::SparseMatrixBlock* b=new typename SparseBlockMatrix<MatrixType>::SparseMatrixBlock(*it->second);
         ret->_blockCols[i].insert(std::make_pair(it->first, b));
       }
     }
     ret->_hasStorage=true;
     return ret;
   }

template<class MatrixType = MatrixXD>

int g2o::SparseBlockMatrix< MatrixType >::colBaseOfBlock ( int c ) const

inline

where does the col at block-col r starts?

Definition at line 110 of file sparse_block_matrix.h.

Referenced by g2o::MarginalCovarianceCholesky::computeCovariance(), g2o::LinearSolverEigen< MatrixType >::computeSymbolicDecomposition(), g2o::LinearSolverCholmodOnline< MatrixType >::computeSymbolicDecomposition(), g2o::LinearSolverCSparse< MatrixType >::computeSymbolicDecomposition(), g2o::LinearSolverCholmod< MatrixType >::computeSymbolicDecomposition(), g2o::LinearSolverEigen< MatrixType >::fillSparseMatrix(), g2o::SparseBlockMatrix< MatrixType >::multiplySymmetricUpperTriangle(), g2o::SparseBlockMatrix< MatrixType >::rightMultiply(), g2o::LinearSolverDense< MatrixType >::solve(), and g2o::SparseBlockMatrix< MatrixType >::writeOctave().

110 { return c ? _colBlockIndices[c-1] : 0 ; }

g2o::SparseBlockMatrix::_colBlockIndices

std::vector< int > _colBlockIndices

Definition: sparse_block_matrix.h:214

template<class MatrixType = MatrixXD>

const std::vector<int>& g2o::SparseBlockMatrix< MatrixType >::colBlockIndices ( ) const

inline

indices of the column blocks

Definition at line 185 of file sparse_block_matrix.h.

Referenced by g2o::BlockSolver< Traits >::buildStructure(), g2o::LinearSolverCCS< MatrixType >::initMatrixStructure(), g2o::BlockSolver< Traits >::resize(), and g2o::BlockSolver< Traits >::updateStructure().

185 { return _colBlockIndices;}

g2o::SparseBlockMatrix::_colBlockIndices

std::vector< int > _colBlockIndices

Definition: sparse_block_matrix.h:214

template<class MatrixType = MatrixXD>

std::vector<int>& g2o::SparseBlockMatrix< MatrixType >::colBlockIndices ( )

inline

Definition at line 186 of file sparse_block_matrix.h.

186 { return _colBlockIndices;}

g2o::SparseBlockMatrix::_colBlockIndices

std::vector< int > _colBlockIndices

Definition: sparse_block_matrix.h:214

template<class MatrixType = MatrixXD>

int g2o::SparseBlockMatrix< MatrixType >::cols ( ) const

inline

columns of the matrix

Definition at line 68 of file sparse_block_matrix.h.

Referenced by g2o::SparseOptimizerIncremental::computeCholeskyUpdate(), g2o::LinearSolverEigen< MatrixType >::computeSymbolicDecomposition(), g2o::LinearSolverCholmodOnline< MatrixType >::fillCholmodExt(), g2o::LinearSolverCholmod< MatrixType >::fillCholmodExt(), g2o::LinearSolverCSparse< MatrixType >::fillCSparse(), g2o::SparseBlockMatrix< MatrixType >::multiply(), g2o::SparseBlockMatrix< MatrixType >::multiplySymmetricUpperTriangle(), g2o::SparseBlockMatrix< MatrixType >::nonZeros(), g2o::SparseBlockMatrix< MatrixType >::rightMultiply(), g2o::LinearSolverDense< MatrixType >::solve(), g2o::LinearSolverEigen< MatrixType >::solve(), g2o::BlockSolver< Traits >::solve(), g2o::SparseBlockMatrix< MatrixType >::symmPermutation(), and g2o::SparseBlockMatrix< MatrixType >::writeOctave().

68 {return _colBlockIndices.size() ? _colBlockIndices.back() : 0;}

g2o::SparseBlockMatrix::_colBlockIndices

std::vector< int > _colBlockIndices

Definition: sparse_block_matrix.h:214

template<class MatrixType = MatrixXD>

int g2o::SparseBlockMatrix< MatrixType >::colsOfBlock ( int c ) const

inline

how many cols does the block at block-col c has?

Definition at line 104 of file sparse_block_matrix.h.

Referenced by g2o::SparseBlockMatrix< MatrixType >::block(), g2o::LinearSolverEigen< MatrixType >::computeSymbolicDecomposition(), g2o::LinearSolverCholmodOnline< MatrixType >::computeSymbolicDecomposition(), g2o::LinearSolverCSparse< MatrixType >::computeSymbolicDecomposition(), g2o::LinearSolverCholmod< MatrixType >::computeSymbolicDecomposition(), g2o::SparseBlockMatrix< MatrixType >::fillCCS(), g2o::SparseBlockMatrix< MatrixType >::slice(), g2o::LinearSolverDense< MatrixType >::solve(), g2o::LinearSolverCSparse< MatrixType >::solveBlocks(), and g2o::LinearSolverCholmod< MatrixType >::solveBlocks().

104 { return c ? _colBlockIndices[c] - _colBlockIndices[c-1] : _colBlockIndices[0]; }

g2o::SparseBlockMatrix::_colBlockIndices

std::vector< int > _colBlockIndices

Definition: sparse_block_matrix.h:214

template<class MatrixType >

void g2o::SparseBlockMatrix< MatrixType >::fillBlockStructure ( MatrixStructure & ms ) const

exports the non zero blocks in the structure matrix ms

Definition at line 519 of file sparse_block_matrix.hpp.

References g2o::SparseBlockMatrix< MatrixType >::_blockCols, g2o::SparseBlockMatrix< MatrixType >::_colBlockIndices, g2o::SparseBlockMatrix< MatrixType >::_rowBlockIndices, g2o::MatrixStructure::Aii, g2o::MatrixStructure::alloc(), g2o::MatrixStructure::Ap, g2o::MatrixStructure::m, and g2o::SparseBlockMatrix< MatrixType >::nonZeroBlocks().

Referenced by g2o::SparseBlockMatrix< PoseMatrixType >::colBaseOfBlock(), g2o::LinearSolverCholmodOnline< MatrixType >::computeSymbolicDecomposition(), g2o::LinearSolverCSparse< MatrixType >::computeSymbolicDecomposition(), and g2o::LinearSolverCholmod< MatrixType >::computeSymbolicDecomposition().

   {
     int n     = _colBlockIndices.size();
     int nzMax = (int)nonZeroBlocks();
 
     ms.alloc(n, nzMax);
     ms.m = _rowBlockIndices.size();
 
     int nz = 0;
     int* Cp = ms.Ap;
     int* Ci = ms.Aii;
     for (int i = 0; i < static_cast<int>(_blockCols.size()); ++i){
       *Cp = nz;
       const int& c = i;
       for (typename SparseBlockMatrix<MatrixType>::IntBlockMap::const_iterator it=_blockCols[i].begin(); it!=_blockCols[i].end(); ++it) {
         const int& r = it->first;
         if (r <= c) {
           *Ci++ = r;
           ++nz;
         }
       }
       Cp++;
     }
     *Cp=nz;
     assert(nz <= nzMax);
   }

template<class MatrixType >

int g2o::SparseBlockMatrix< MatrixType >::fillCCS	(	int *	Cp,
		int *	Ci,
		double *	Cx,
		bool	upperTriangle = `false`
	)		const

fill the CCS arrays of a matrix, arrays have to be allocated beforehand

Definition at line 489 of file sparse_block_matrix.hpp.

References g2o::SparseBlockMatrix< MatrixType >::_blockCols, g2o::SparseBlockMatrix< MatrixType >::_colBlockIndices, g2o::SparseBlockMatrix< MatrixType >::_rowBlockIndices, g2o::SparseBlockMatrix< MatrixType >::colsOfBlock(), and g2o::SparseBlockMatrix< MatrixType >::rows().

Referenced by g2o::SparseBlockMatrix< PoseMatrixType >::colBaseOfBlock(), g2o::SparseOptimizerIncremental::computeCholeskyUpdate(), g2o::LinearSolverCholmodOnline< MatrixType >::fillCholmodExt(), and g2o::LinearSolverEigen< MatrixType >::fillSparseMatrix().

   {
     assert(Cp && Ci && Cx && "Target destination is NULL");
     int nz=0;
     for (size_t i=0; i<_blockCols.size(); ++i){
       int cstart=i ? _colBlockIndices[i-1] : 0;
       int csize=colsOfBlock(i);
       for (int c=0; c<csize; ++c) {
         *Cp=nz;
         for (typename SparseBlockMatrix<MatrixType>::IntBlockMap::const_iterator it=_blockCols[i].begin(); it!=_blockCols[i].end(); ++it){
           const typename SparseBlockMatrix<MatrixType>::SparseMatrixBlock* b=it->second;
           int rstart=it->first ? _rowBlockIndices[it->first-1] : 0;
 
           int elemsToCopy = b->rows();
           if (upperTriangle && rstart == cstart)
             elemsToCopy = c + 1;
           for (int r=0; r<elemsToCopy; ++r){
             *Cx++ = (*b)(r,c);
             *Ci++ = rstart++;
             ++nz;
           }
         }
         ++Cp;
       }
     }
     *Cp=nz;
     return nz;
   }

template<class MatrixType >

int g2o::SparseBlockMatrix< MatrixType >::fillCCS	(	double *	Cx,
		bool	upperTriangle = `false`
	)		const

fill the CCS arrays of a matrix, arrays have to be allocated beforehand. This function only writes the values and assumes that column and row structures have already been written.

Definition at line 464 of file sparse_block_matrix.hpp.

References g2o::SparseBlockMatrix< MatrixType >::_blockCols, g2o::SparseBlockMatrix< MatrixType >::_colBlockIndices, g2o::SparseBlockMatrix< MatrixType >::_rowBlockIndices, g2o::SparseBlockMatrix< MatrixType >::colsOfBlock(), and g2o::SparseBlockMatrix< MatrixType >::rows().

   {
     assert(Cx && "Target destination is NULL");
     double* CxStart = Cx;
     for (size_t i=0; i<_blockCols.size(); ++i){
       int cstart=i ? _colBlockIndices[i-1] : 0;
       int csize=colsOfBlock(i);
       for (int c=0; c<csize; ++c) {
         for (typename SparseBlockMatrix<MatrixType>::IntBlockMap::const_iterator it=_blockCols[i].begin(); it!=_blockCols[i].end(); ++it){
           const typename SparseBlockMatrix<MatrixType>::SparseMatrixBlock* b=it->second;
           int rstart=it->first ? _rowBlockIndices[it->first-1] : 0;
 
           int elemsToCopy = b->rows();
           if (upperTriangle && rstart == cstart)
             elemsToCopy = c + 1;
           memcpy(Cx, b->data() + c*b->rows(), elemsToCopy * sizeof(double));
           Cx += elemsToCopy;
 
         }
       }
     }
     return Cx - CxStart;
   }

template<class MatrixType>

int g2o::SparseBlockMatrix< MatrixType >::fillSparseBlockMatrixCCS ( SparseBlockMatrixCCS< MatrixType > & blockCCS ) const

copy into CCS structure

Returns: number of processed blocks, -1 on error

Definition at line 591 of file sparse_block_matrix.hpp.

References g2o::SparseBlockMatrixCCS< MatrixType >::blockCols(), and g2o::SparseBlockMatrix< MatrixType >::blockCols().

Referenced by g2o::BlockSolver< Traits >::buildStructure(), g2o::SparseBlockMatrix< PoseMatrixType >::colBlockIndices(), and g2o::LinearSolverCCS< MatrixType >::initMatrixStructure().

   {
     blockCCS.blockCols().resize(blockCols().size());
     int numblocks = 0;
     for (size_t i = 0; i < blockCols().size(); ++i) {
       const IntBlockMap& row = blockCols()[i];
       typename SparseBlockMatrixCCS<MatrixType>::SparseColumn& dest = blockCCS.blockCols()[i];
       dest.clear();
       dest.reserve(row.size());
       for (typename IntBlockMap::const_iterator it = row.begin(); it != row.end(); ++it) {
         dest.push_back(typename SparseBlockMatrixCCS<MatrixType>::RowBlock(it->first, it->second));
         ++numblocks;
       }
     }
     return numblocks;
   }

template<class MatrixType>

int g2o::SparseBlockMatrix< MatrixType >::fillSparseBlockMatrixCCSTransposed ( SparseBlockMatrixCCS< MatrixType > & blockCCS ) const

copy as transposed into a CCS structure

Returns: number of processed blocks, -1 on error

Definition at line 609 of file sparse_block_matrix.hpp.

References g2o::SparseBlockMatrix< MatrixType >::_rowBlockIndices, g2o::SparseBlockMatrixCCS< MatrixType >::blockCols(), and g2o::SparseBlockMatrix< MatrixType >::blockCols().

Referenced by g2o::BlockSolver< Traits >::buildStructure(), and g2o::SparseBlockMatrix< PoseMatrixType >::colBlockIndices().

   {
     blockCCS.blockCols().clear();
     blockCCS.blockCols().resize(_rowBlockIndices.size());
     int numblocks = 0;
     for (size_t i = 0; i < blockCols().size(); ++i) {
       const IntBlockMap& row = blockCols()[i];
       for (typename IntBlockMap::const_iterator it = row.begin(); it != row.end(); ++it) {
         typename SparseBlockMatrixCCS<MatrixType>::SparseColumn& dest = blockCCS.blockCols()[it->first];
         dest.push_back(typename SparseBlockMatrixCCS<MatrixType>::RowBlock(i, it->second));
         ++numblocks;
       }
     }
     return numblocks;
   }

template<class MatrixType >

template<class MatrixResultType , class MatrixFactorType >

bool g2o::SparseBlockMatrix< MatrixType >::multiply	(	SparseBlockMatrix< MatrixResultType > *&	dest,
		const SparseBlockMatrix< MatrixFactorType > *	M
	)		const

dest = (*this) * M

Definition at line 199 of file sparse_block_matrix.hpp.

References g2o::SparseBlockMatrix< MatrixType >::_blockCols, g2o::SparseBlockMatrix< MatrixType >::_colBlockIndices, g2o::SparseBlockMatrix< MatrixType >::_hasStorage, g2o::SparseBlockMatrix< MatrixType >::_rowBlockIndices, g2o::SparseBlockMatrix< MatrixType >::block(), g2o::SparseBlockMatrix< MatrixType >::cols(), and g2o::SparseBlockMatrix< MatrixType >::rows().

Referenced by g2o::SparseBlockMatrix< PoseMatrixType >::colBaseOfBlock(), and main().

                                                                                                                                               {
     // sanity check
     if (_colBlockIndices.size()!=M->_rowBlockIndices.size())
       return false;
     for (size_t i=0; i<_colBlockIndices.size(); ++i){
       if (_colBlockIndices[i]!=M->_rowBlockIndices[i])
         return false;
     }
     if (! dest) {
       dest=new SparseBlockMatrix<MatrixResultType>(&_rowBlockIndices[0],&(M->_colBlockIndices[0]), _rowBlockIndices.size(), M->_colBlockIndices.size() );
     }
     if (! dest->_hasStorage)
       return false;
     for (size_t i=0; i<M->_blockCols.size(); ++i){
       for (typename SparseBlockMatrix<MatrixFactorType>::IntBlockMap::const_iterator it=M->_blockCols[i].begin(); it!=M->_blockCols[i].end(); ++it){
         // look for a non-zero block in a row of column it
         int colM=i;
         const typename SparseBlockMatrix<MatrixFactorType>::SparseMatrixBlock *b=it->second;
         typename SparseBlockMatrix<MatrixType>::IntBlockMap::const_iterator rbt=_blockCols[it->first].begin();
         while(rbt!=_blockCols[it->first].end()){
           //int colA=it->first;
           int rowA=rbt->first;
           typename SparseBlockMatrix<MatrixType>::SparseMatrixBlock *a=rbt->second;
           typename SparseBlockMatrix<MatrixResultType>::SparseMatrixBlock *c=dest->block(rowA,colM,true);
           assert (c->rows()==a->rows());
           assert (c->cols()==b->cols());
           ++rbt;
           (*c)+=(*a)*(*b);
         }
       }
     }
     return false;
   }

template<class MatrixType >

void g2o::SparseBlockMatrix< MatrixType >::multiply	(	double *&	dest,
		const double *	src
	)		const

dest = (*this) * src

Definition at line 234 of file sparse_block_matrix.hpp.

References g2o::SparseBlockMatrix< MatrixType >::_blockCols, g2o::SparseBlockMatrix< MatrixType >::_colBlockIndices, g2o::SparseBlockMatrix< MatrixType >::_rowBlockIndices, g2o::SparseBlockMatrix< MatrixType >::cols(), and g2o::SparseBlockMatrix< MatrixType >::rows().

                                                                                      {
     if (! dest){
       dest=new double [_rowBlockIndices[_rowBlockIndices.size()-1] ];
       memset(dest,0, _rowBlockIndices[_rowBlockIndices.size()-1]*sizeof(double));
     }
 
     // map the memory by Eigen
     Eigen::Map<VectorXD> destVec(dest, rows());
     const Eigen::Map<const VectorXD> srcVec(src, cols());
 
     for (size_t i=0; i<_blockCols.size(); ++i){
       int srcOffset = i ? _colBlockIndices[i-1] : 0;
 
       for (typename SparseBlockMatrix<MatrixType>::IntBlockMap::const_iterator it=_blockCols[i].begin(); it!=_blockCols[i].end(); ++it){
         const typename SparseBlockMatrix<MatrixType>::SparseMatrixBlock* a=it->second;
         int destOffset = it->first ? _rowBlockIndices[it->first - 1] : 0;
         // destVec += *a * srcVec (according to the sub-vector parts)
         internal::template axpy<typename SparseBlockMatrix<MatrixType>::SparseMatrixBlock>(*a, srcVec, srcOffset, destVec, destOffset);
       }
     }
   }

template<class MatrixType >

void g2o::SparseBlockMatrix< MatrixType >::multiplySymmetricUpperTriangle	(	double *&	dest,
		const double *	src
	)		const

compute dest = (*this) * src However, assuming that this is a symmetric matrix where only the upper triangle is stored

Definition at line 257 of file sparse_block_matrix.hpp.

References g2o::SparseBlockMatrix< MatrixType >::_blockCols, g2o::SparseBlockMatrix< MatrixType >::_rowBlockIndices, g2o::SparseBlockMatrix< MatrixType >::colBaseOfBlock(), g2o::SparseBlockMatrix< MatrixType >::cols(), g2o::SparseBlockMatrix< MatrixType >::rowBaseOfBlock(), and g2o::SparseBlockMatrix< MatrixType >::rows().

Referenced by g2o::SparseBlockMatrix< PoseMatrixType >::colBaseOfBlock().

   {
     if (! dest){
       dest=new double [_rowBlockIndices[_rowBlockIndices.size()-1] ];
       memset(dest,0, _rowBlockIndices[_rowBlockIndices.size()-1]*sizeof(double));
     }
 
     // map the memory by Eigen
     Eigen::Map<VectorXD> destVec(dest, rows());
     const Eigen::Map<const VectorXD> srcVec(src, cols());
 
     for (size_t i=0; i<_blockCols.size(); ++i){
       int srcOffset = colBaseOfBlock(i);
       for (typename SparseBlockMatrix<MatrixType>::IntBlockMap::const_iterator it=_blockCols[i].begin(); it!=_blockCols[i].end(); ++it){
         const typename SparseBlockMatrix<MatrixType>::SparseMatrixBlock* a=it->second;
         int destOffset = rowBaseOfBlock(it->first);
         if (destOffset > srcOffset) // only upper triangle
           break;
         // destVec += *a * srcVec (according to the sub-vector parts)
         internal::template axpy<typename SparseBlockMatrix<MatrixType>::SparseMatrixBlock>(*a, srcVec, srcOffset, destVec, destOffset);
         if (destOffset < srcOffset)
           internal::template atxpy<typename SparseBlockMatrix<MatrixType>::SparseMatrixBlock>(*a, srcVec, destOffset, destVec, srcOffset);
       }
     }
   }

template<class MatrixType >

size_t g2o::SparseBlockMatrix< MatrixType >::nonZeroBlocks ( ) const

number of allocated blocks

Definition at line 353 of file sparse_block_matrix.hpp.

References g2o::SparseBlockMatrix< MatrixType >::_blockCols.

Referenced by g2o::SparseBlockMatrix< PoseMatrixType >::colBaseOfBlock(), g2o::SparseBlockMatrix< MatrixType >::fillBlockStructure(), and g2o::SparseBlockMatrix< MatrixType >::nonZeros().

                                                             {
     size_t count=0;
     for (size_t i=0; i<_blockCols.size(); ++i)
       count+=_blockCols[i].size();
     return count;
   }

template<class MatrixType >

size_t g2o::SparseBlockMatrix< MatrixType >::nonZeros ( ) const

number of non-zero elements

Definition at line 361 of file sparse_block_matrix.hpp.

References g2o::SparseBlockMatrix< MatrixType >::_blockCols, g2o::SparseBlockMatrix< MatrixType >::cols(), g2o::SparseBlockMatrix< MatrixType >::nonZeroBlocks(), and g2o::SparseBlockMatrix< MatrixType >::rows().

Referenced by g2o::SparseBlockMatrix< PoseMatrixType >::colBaseOfBlock(), g2o::SparseOptimizerIncremental::computeCholeskyUpdate(), g2o::LinearSolverCholmodOnline< MatrixType >::fillCholmodExt(), g2o::LinearSolverCholmod< MatrixType >::fillCholmodExt(), g2o::LinearSolverCSparse< MatrixType >::fillCSparse(), and g2o::LinearSolverEigen< MatrixType >::fillSparseMatrix().

                                                       {
     if (MatrixType::SizeAtCompileTime != Eigen::Dynamic) {
       size_t nnz = nonZeroBlocks() * MatrixType::SizeAtCompileTime;
       return nnz;
     } else {
       size_t count=0;
       for (size_t i=0; i<_blockCols.size(); ++i){
         for (typename SparseBlockMatrix<MatrixType>::IntBlockMap::const_iterator it=_blockCols[i].begin(); it!=_blockCols[i].end(); ++it){
           const typename SparseBlockMatrix<MatrixType>::SparseMatrixBlock* a=it->second;
           count += a->cols()*a->rows();
         }
       }
       return count;
     }
   }

template<class MatrixType >

void g2o::SparseBlockMatrix< MatrixType >::rightMultiply	(	double *&	dest,
		const double *	src
	)		const

dest = M * (*this)

Definition at line 284 of file sparse_block_matrix.hpp.

References g2o::SparseBlockMatrix< MatrixType >::_blockCols, g2o::SparseBlockMatrix< MatrixType >::colBaseOfBlock(), g2o::SparseBlockMatrix< MatrixType >::cols(), g2o::SparseBlockMatrix< MatrixType >::rowBaseOfBlock(), and g2o::SparseBlockMatrix< MatrixType >::rows().

Referenced by g2o::SparseBlockMatrix< PoseMatrixType >::colBaseOfBlock().

                                                                                           {
     int destSize=cols();
 
     if (! dest){
       dest=new double [ destSize ];
       memset(dest,0, destSize*sizeof(double));
     }
 
     // map the memory by Eigen
     Eigen::Map<VectorXD> destVec(dest, destSize);
     Eigen::Map<const VectorXD> srcVec(src, rows());
 
 #   ifdef G2O_OPENMP
 #   pragma omp parallel for default (shared) schedule(dynamic, 10)
 #   endif
     for (int i=0; i < static_cast<int>(_blockCols.size()); ++i){
       int destOffset = colBaseOfBlock(i);
       for (typename SparseBlockMatrix<MatrixType>::IntBlockMap::const_iterator it=_blockCols[i].begin(); 
           it!=_blockCols[i].end(); 
           ++it){
         const typename SparseBlockMatrix<MatrixType>::SparseMatrixBlock* a=it->second;
         int srcOffset = rowBaseOfBlock(it->first);
         // destVec += *a.transpose() * srcVec (according to the sub-vector parts)
         internal::template atxpy<typename SparseBlockMatrix<MatrixType>::SparseMatrixBlock>(*a, srcVec, srcOffset, destVec, destOffset);
       }
     }
     
   }

template<class MatrixType = MatrixXD>

int g2o::SparseBlockMatrix< MatrixType >::rowBaseOfBlock ( int r ) const

inline

where does the row at block-row r starts?

Definition at line 107 of file sparse_block_matrix.h.

Referenced by g2o::MarginalCovarianceCholesky::computeCovariance(), g2o::LinearSolverEigen< MatrixType >::fillSparseMatrix(), g2o::SparseBlockMatrix< MatrixType >::multiplySymmetricUpperTriangle(), g2o::SparseBlockMatrix< MatrixType >::rightMultiply(), g2o::LinearSolverDense< MatrixType >::solve(), g2o::BlockSolver< Traits >::solve(), and g2o::SparseBlockMatrix< MatrixType >::writeOctave().

107 { return r ? _rowBlockIndices[r-1] : 0 ; }

g2o::SparseBlockMatrix::_rowBlockIndices

std::vector< int > _rowBlockIndices

vector of the indices of the blocks along the rows.

Definition: sparse_block_matrix.h:213

template<class MatrixType = MatrixXD>

const std::vector<int>& g2o::SparseBlockMatrix< MatrixType >::rowBlockIndices ( ) const

inline

indices of the row blocks

Definition at line 181 of file sparse_block_matrix.h.

Referenced by g2o::BlockSolver< Traits >::buildStructure(), g2o::LinearSolverCCS< MatrixType >::initMatrixStructure(), g2o::BlockSolver< Traits >::resize(), g2o::LinearSolverCSparse< MatrixType >::solveBlocks(), g2o::LinearSolverCholmod< MatrixType >::solveBlocks(), g2o::LinearSolverCSparse< MatrixType >::solvePattern(), g2o::LinearSolverCholmod< MatrixType >::solvePattern(), and g2o::BlockSolver< Traits >::updateStructure().

181 { return _rowBlockIndices;}

g2o::SparseBlockMatrix::_rowBlockIndices

std::vector< int > _rowBlockIndices

vector of the indices of the blocks along the rows.

Definition: sparse_block_matrix.h:213

template<class MatrixType = MatrixXD>

std::vector<int>& g2o::SparseBlockMatrix< MatrixType >::rowBlockIndices ( )

inline

Definition at line 182 of file sparse_block_matrix.h.

182 { return _rowBlockIndices;}

g2o::SparseBlockMatrix::_rowBlockIndices

std::vector< int > _rowBlockIndices

vector of the indices of the blocks along the rows.

Definition: sparse_block_matrix.h:213

template<class MatrixType = MatrixXD>

int g2o::SparseBlockMatrix< MatrixType >::rows ( ) const

inline

rows of the matrix

Definition at line 70 of file sparse_block_matrix.h.

Referenced by g2o::SparseOptimizerIncremental::computeCholeskyUpdate(), g2o::LinearSolverEigen< MatrixType >::computeSymbolicDecomposition(), g2o::SparseBlockMatrix< MatrixType >::fillCCS(), g2o::LinearSolverCholmodOnline< MatrixType >::fillCholmodExt(), g2o::LinearSolverCholmod< MatrixType >::fillCholmodExt(), g2o::LinearSolverCSparse< MatrixType >::fillCSparse(), g2o::SparseBlockMatrix< MatrixType >::multiply(), g2o::SparseBlockMatrix< MatrixType >::multiplySymmetricUpperTriangle(), g2o::SparseBlockMatrix< MatrixType >::nonZeros(), g2o::SparseBlockMatrix< MatrixType >::rightMultiply(), g2o::LinearSolverEigen< MatrixType >::solve(), g2o::LinearSolverCSparse< MatrixType >::solveBlocks(), g2o::LinearSolverCholmod< MatrixType >::solveBlocks(), g2o::SparseBlockMatrix< MatrixType >::symmPermutation(), and g2o::SparseBlockMatrix< MatrixType >::writeOctave().

70 {return _rowBlockIndices.size() ? _rowBlockIndices.back() : 0;}

g2o::SparseBlockMatrix::_rowBlockIndices

std::vector< int > _rowBlockIndices

vector of the indices of the blocks along the rows.

Definition: sparse_block_matrix.h:213

template<class MatrixType = MatrixXD>

int g2o::SparseBlockMatrix< MatrixType >::rowsOfBlock ( int r ) const

inline

how many rows does the block at block-row r has?

Definition at line 101 of file sparse_block_matrix.h.

Referenced by g2o::SparseBlockMatrix< MatrixType >::block(), g2o::SparseBlockMatrix< MatrixType >::slice(), g2o::LinearSolverDense< MatrixType >::solve(), g2o::LinearSolverCSparse< MatrixType >::solveBlocks(), and g2o::LinearSolverCholmod< MatrixType >::solveBlocks().

101 { return r ? _rowBlockIndices[r] - _rowBlockIndices[r-1] : _rowBlockIndices[0] ; }

g2o::SparseBlockMatrix::_rowBlockIndices

std::vector< int > _rowBlockIndices

vector of the indices of the blocks along the rows.

Definition: sparse_block_matrix.h:213

template<class MatrixType >

void g2o::SparseBlockMatrix< MatrixType >::scale ( double a )

*this *= a

Definition at line 314 of file sparse_block_matrix.hpp.

References g2o::SparseBlockMatrix< MatrixType >::_blockCols.

Referenced by g2o::SparseBlockMatrix< PoseMatrixType >::colBaseOfBlock().

                                                      {
     for (size_t i=0; i<_blockCols.size(); ++i){
       for (typename SparseBlockMatrix<MatrixType>::IntBlockMap::const_iterator it=_blockCols[i].begin(); it!=_blockCols[i].end(); ++it){
         typename SparseBlockMatrix<MatrixType>::SparseMatrixBlock* a=it->second;
         *a *= a_;
       }
     }
   }

template<class MatrixType >

SparseBlockMatrix< MatrixType > * g2o::SparseBlockMatrix< MatrixType >::slice	(	int	rmin,
		int	rmax,
		int	cmin,
		int	cmax,
		bool	alloc = `true`
	)		const

returns a view or a copy of the block matrix

Parameters

rmin	starting block row
rmax	ending block row
cmin	starting block col
cmax	ending block col
alloc	if true it makes a deep copy, if false it creates a view.

Definition at line 324 of file sparse_block_matrix.hpp.

References g2o::SparseBlockMatrix< MatrixType >::_blockCols, g2o::SparseBlockMatrix< MatrixType >::_hasStorage, g2o::SparseBlockMatrix< MatrixType >::colsOfBlock(), g2o::SparseBlockMatrix< MatrixType >::rowsOfBlock(), and g2o::SparseBlockMatrix< MatrixType >::SparseBlockMatrix().

Referenced by g2o::SparseBlockMatrix< PoseMatrixType >::colBaseOfBlock().

                                                                                                                                {
     int m=rmax-rmin;
     int n=cmax-cmin;
     int rowIdx [m];
     rowIdx[0] = rowsOfBlock(rmin);
     for (int i=1; i<m; ++i){
       rowIdx[i]=rowIdx[i-1]+rowsOfBlock(rmin+i);
     }
 
     int colIdx [n];
     colIdx[0] = colsOfBlock(cmin);
     for (int i=1; i<n; ++i){
       colIdx[i]=colIdx[i-1]+colsOfBlock(cmin+i);
     }
     typename SparseBlockMatrix<MatrixType>::SparseBlockMatrix* s=new SparseBlockMatrix(rowIdx, colIdx, m, n, true);
     for (int i=0; i<n; ++i){
       int mc=cmin+i;
       for (typename SparseBlockMatrix<MatrixType>::IntBlockMap::const_iterator it=_blockCols[mc].begin(); it!=_blockCols[mc].end(); ++it){
         if (it->first >= rmin && it->first < rmax){
           typename SparseBlockMatrix<MatrixType>::SparseMatrixBlock* b = alloc ? new typename SparseBlockMatrix<MatrixType>::SparseMatrixBlock (* (it->second) ) : it->second;
           s->_blockCols[i].insert(std::make_pair(it->first-rmin, b));
         }
       }
     }
     s->_hasStorage=alloc;
     return s;
   }

template<class MatrixType>

bool g2o::SparseBlockMatrix< MatrixType >::symmPermutation	(	SparseBlockMatrix< MatrixType > *&	dest,
		const int *	pinv,
		bool	onlyUpper = `false`
	)		const

writes in dest a block permutaton specified by pinv.

Parameters

pinv	array such that new_block[i] = old_block[pinv[i]]

Definition at line 399 of file sparse_block_matrix.hpp.

References g2o::SparseBlockMatrix< MatrixType >::_blockCols, g2o::SparseBlockMatrix< MatrixType >::_colBlockIndices, g2o::SparseBlockMatrix< MatrixType >::_rowBlockIndices, g2o::SparseBlockMatrix< MatrixType >::block(), g2o::SparseBlockMatrix< MatrixType >::clear(), g2o::SparseBlockMatrix< MatrixType >::cols(), g2o::SparseBlockMatrix< MatrixType >::rows(), g2o::SparseBlockMatrix< MatrixType >::SparseBlockMatrix(), and g2o::SparseBlockMatrix< MatrixType >::transpose().

Referenced by g2o::SparseBlockMatrix< PoseMatrixType >::colBaseOfBlock(), and main().

                                                                                                                                      {
     // compute the permuted version of the new row/column layout
     size_t n=_rowBlockIndices.size();
     // computed the block sizes
     int blockSizes[_rowBlockIndices.size()];
     blockSizes[0]=_rowBlockIndices[0];
     for (size_t i=1; i<n; ++i){
       blockSizes[i]=_rowBlockIndices[i]-_rowBlockIndices[i-1];
     }
     // permute them
     int pBlockIndices[_rowBlockIndices.size()];
     for (size_t i=0; i<n; ++i){
       pBlockIndices[pinv[i]]=blockSizes[i];
     }
     for (size_t i=1; i<n; ++i){
       pBlockIndices[i]+=pBlockIndices[i-1];
     }
     // allocate C, or check the structure;
     if (! dest){
       dest=new SparseBlockMatrix(pBlockIndices, pBlockIndices, n, n);
     } else {
       if (dest->_rowBlockIndices.size()!=n)
         return false;
       if (dest->_colBlockIndices.size()!=n)
         return false;
       for (size_t i=0; i<n; ++i){
         if (dest->_rowBlockIndices[i]!=pBlockIndices[i])
           return false;
         if (dest->_colBlockIndices[i]!=pBlockIndices[i])
           return false;
       }
       dest->clear();
     }
     // now ready to permute the columns
     for (size_t i=0; i<n; ++i){
       //cerr << PVAR(i) <<  " ";
       int pi=pinv[i];
       for (typename SparseBlockMatrix<MatrixType>::IntBlockMap::const_iterator it=_blockCols[i].begin(); 
           it!=_blockCols[i].end(); ++it){
         int pj=pinv[it->first];
 
         const typename SparseBlockMatrix<MatrixType>::SparseMatrixBlock* s=it->second;
 
         typename SparseBlockMatrix<MatrixType>::SparseMatrixBlock* b=0;
         if (! upperTriangle || pj<=pi) {
           b=dest->block(pj,pi,true);
           assert(b->cols()==s->cols());
           assert(b->rows()==s->rows());
           *b=*s;
         } else {
           b=dest->block(pi,pj,true);
           assert(b);
           assert(b->rows()==s->cols());
           assert(b->cols()==s->rows());
           *b=s->transpose();
         }
       }
       //cerr << endl;
       // within each row, 
     }
     return true;
     
   }

template<class MatrixType>

void g2o::SparseBlockMatrix< MatrixType >::takePatternFromHash ( SparseBlockMatrixHashMap< MatrixType > & hashMatrix )

take over the memory and matrix pattern from a hash matrix. The structure of the hash matrix will be cleared.

Definition at line 626 of file sparse_block_matrix.hpp.

References g2o::SparseBlockMatrix< MatrixType >::blockCols(), and g2o::SparseBlockMatrixHashMap< MatrixType >::blockCols().

Referenced by g2o::BlockSolver< Traits >::buildStructure(), and g2o::SparseBlockMatrix< PoseMatrixType >::colBlockIndices().

   {
     // sort the sparse columns and add them to the map structures by
     // exploiting that we are inserting a sorted structure
     typedef std::pair<int, MatrixType*> SparseColumnPair;
     typedef typename SparseBlockMatrixHashMap<MatrixType>::SparseColumn HashSparseColumn;
     for (size_t i = 0; i < hashMatrix.blockCols().size(); ++i) {
       // prepare a temporary vector for sorting
       HashSparseColumn& column = hashMatrix.blockCols()[i];
       if (column.size() == 0)
         continue;
       std::vector<SparseColumnPair> sparseRowSorted; // temporary structure
       sparseRowSorted.reserve(column.size());
       for (typename HashSparseColumn::const_iterator it = column.begin(); it != column.end(); ++it)
         sparseRowSorted.push_back(*it);
       std::sort(sparseRowSorted.begin(), sparseRowSorted.end(), CmpPairFirst<int, MatrixType*>());
       // try to free some memory early
       HashSparseColumn aux;
       std::swap(aux, column);
       // now insert sorted vector to the std::map structure
       IntBlockMap& destColumnMap = blockCols()[i];
       destColumnMap.insert(sparseRowSorted[0]);
       for (size_t j = 1; j < sparseRowSorted.size(); ++j) {
         typename SparseBlockMatrix<MatrixType>::IntBlockMap::iterator hint = destColumnMap.end();
         --hint; // cppreference says the element goes after the hint (until C++11)
         destColumnMap.insert(hint, sparseRowSorted[j]);
       }
     }
   }

template<class MatrixType >

template<class MatrixTransposedType >

bool g2o::SparseBlockMatrix< MatrixType >::transpose ( SparseBlockMatrix< MatrixTransposedType > *& dest ) const

transposes a block matrix, The transposed type should match the argument false on failure

Definition at line 137 of file sparse_block_matrix.hpp.

References g2o::SparseBlockMatrix< MatrixType >::_blockCols, g2o::SparseBlockMatrix< MatrixType >::_colBlockIndices, g2o::SparseBlockMatrix< MatrixType >::_hasStorage, g2o::SparseBlockMatrix< MatrixType >::_rowBlockIndices, g2o::SparseBlockMatrix< MatrixType >::block(), and g2o::SparseBlockMatrix< MatrixType >::transpose().

Referenced by g2o::SparseBlockMatrix< PoseMatrixType >::colBaseOfBlock(), main(), g2o::LinearSolverDense< MatrixType >::solve(), g2o::SparseBlockMatrix< MatrixType >::symmPermutation(), and g2o::SparseBlockMatrix< MatrixType >::transpose().

                                                                                                     {
     if (! dest){
       dest=new SparseBlockMatrix<MatrixTransposedType>(&_colBlockIndices[0], &_rowBlockIndices[0], _colBlockIndices.size(), _rowBlockIndices.size());
     } else {
       if (! dest->_hasStorage)
         return false;
       if(_rowBlockIndices.size()!=dest->_colBlockIndices.size())
         return false;
       if (_colBlockIndices.size()!=dest->_rowBlockIndices.size())
         return  false;
       for (size_t i=0; i<_rowBlockIndices.size(); ++i){
         if(_rowBlockIndices[i]!=dest->_colBlockIndices[i])
           return false;
       }
       for (size_t i=0; i<_colBlockIndices.size(); ++i){
         if(_colBlockIndices[i]!=dest->_rowBlockIndices[i])
           return false;
       }
     }
 
     for (size_t i=0; i<_blockCols.size(); ++i){
       for (typename SparseBlockMatrix<MatrixType>::IntBlockMap::const_iterator it=_blockCols[i].begin(); it!=_blockCols[i].end(); ++it){
         typename SparseBlockMatrix<MatrixType>::SparseMatrixBlock* s=it->second;
         typename SparseBlockMatrix<MatrixTransposedType>::SparseMatrixBlock* d=dest->block(i,it->first,true);
         *d = s->transpose();
       }
     }
     return true;
   }

template<class MatrixType >

bool g2o::SparseBlockMatrix< MatrixType >::writeOctave	(	const char *	filename,
		bool	upperTriangle = `true`
	)		const

write the content of this matrix to a stream loadable by Octave

Parameters

upperTriangle does this matrix store only the upper triangular blocks

Definition at line 547 of file sparse_block_matrix.hpp.

References g2o::SparseBlockMatrix< MatrixType >::_blockCols, g2o::SparseBlockMatrix< MatrixType >::colBaseOfBlock(), g2o::SparseBlockMatrix< MatrixType >::cols(), g2o::SparseBlockMatrix< MatrixType >::rowBaseOfBlock(), and g2o::SparseBlockMatrix< MatrixType >::rows().

Referenced by g2o::SparseBlockMatrix< PoseMatrixType >::colBlockIndices(), g2o::BlockSolver< Traits >::saveHessian(), and g2o::LinearSolverEigen< MatrixType >::solve().

   {
     std::string name = filename;
     std::string::size_type lastDot = name.find_last_of('.');
     if (lastDot != std::string::npos) 
       name = name.substr(0, lastDot);
 
     std::vector<TripletEntry> entries;
     for (size_t i = 0; i<_blockCols.size(); ++i){
       const int& c = i;
       for (typename SparseBlockMatrix<MatrixType>::IntBlockMap::const_iterator it=_blockCols[i].begin(); it!=_blockCols[i].end(); ++it) {
         const int& r = it->first;
         const MatrixType& m = *(it->second);
         for (int cc = 0; cc < m.cols(); ++cc)
           for (int rr = 0; rr < m.rows(); ++rr) {
             int aux_r = rowBaseOfBlock(r) + rr;
             int aux_c = colBaseOfBlock(c) + cc;
             entries.push_back(TripletEntry(aux_r, aux_c, m(rr, cc)));
             if (upperTriangle && r != c) {
               entries.push_back(TripletEntry(aux_c, aux_r, m(rr, cc)));
             }
           }
       }
     }
 
     int nz = entries.size();
     std::sort(entries.begin(), entries.end(), TripletColSort());
 
     std::ofstream fout(filename);
     fout << "# name: " << name << std::endl;
     fout << "# type: sparse matrix" << std::endl;
     fout << "# nnz: " << nz << std::endl;
     fout << "# rows: " << rows() << std::endl;
     fout << "# columns: " << cols() << std::endl;
     fout << std::setprecision(9) << std::fixed << std::endl;
 
     for (std::vector<TripletEntry>::const_iterator it = entries.begin(); it != entries.end(); ++it) {
       const TripletEntry& entry = *it;
       fout << entry.r+1 << " " << entry.c+1 << " " << entry.x << std::endl;
     }
     return fout.good();
   }

Member Data Documentation

template<class MatrixType = MatrixXD>

std::vector<IntBlockMap> g2o::SparseBlockMatrix< MatrixType >::_blockCols

protected

array of maps of blocks. The index of the array represent a block column of the matrix and the block column is stored as a map row_block -> matrix_block_ptr.

Definition at line 217 of file sparse_block_matrix.h.

template<class MatrixType = MatrixXD>

std::vector<int> g2o::SparseBlockMatrix< MatrixType >::_colBlockIndices

protected

vector of the indices of the blocks along the cols

Definition at line 214 of file sparse_block_matrix.h.

template<class MatrixType = MatrixXD>

bool g2o::SparseBlockMatrix< MatrixType >::_hasStorage

protected

Definition at line 218 of file sparse_block_matrix.h.

Referenced by g2o::SparseBlockMatrix< MatrixType >::add(), g2o::SparseBlockMatrix< MatrixType >::block(), g2o::SparseBlockMatrix< MatrixType >::clear(), g2o::SparseBlockMatrix< MatrixType >::clone(), g2o::SparseBlockMatrix< MatrixType >::multiply(), g2o::SparseBlockMatrix< MatrixType >::slice(), g2o::SparseBlockMatrix< MatrixType >::transpose(), and g2o::SparseBlockMatrix< MatrixType >::~SparseBlockMatrix().

template<class MatrixType = MatrixXD>

std::vector<int> g2o::SparseBlockMatrix< MatrixType >::_rowBlockIndices

protected

vector of the indices of the blocks along the rows.

Definition at line 213 of file sparse_block_matrix.h.

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

/home/xuezhisd/CLionProjects/g2o/g2o/core/sparse_block_matrix.h
/home/xuezhisd/CLionProjects/g2o/g2o/core/sparse_block_matrix.hpp

Public Types

Public Member Functions

Protected Attributes

Detailed Description

template<class MatrixType = MatrixXD> class g2o::SparseBlockMatrix< MatrixType >

Member Typedef Documentation

Constructor & Destructor Documentation

Member Function Documentation

Member Data Documentation

template<class MatrixType = MatrixXD>
class g2o::SparseBlockMatrix< MatrixType >