g2o
Loading...
Searching...
No Matches
g2o::SparseBlockMatrix< MatrixType > Class Template Reference

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.
typedef std::map< int, SparseMatrixBlock * > IntBlockMap

Public Member Functions

int cols () const
 columns of the matrix
int rows () const
 rows of the matrix
 SparseBlockMatrix (const int *rbi, const int *cbi, int rb, int cb, bool hasStorage=true)
 SparseBlockMatrix ()
 ~SparseBlockMatrix ()
void clear (bool dealloc=false)
SparseMatrixBlock * block (int r, int c, bool alloc=false)
const SparseMatrixBlock * block (int r, int c) const
 returns the block at location r,c
int rowsOfBlock (int r) const
 how many rows does the block at block-row r has?
int colsOfBlock (int c) const
 how many cols does the block at block-col c has?
int rowBaseOfBlock (int r) const
 where does the row at block-row r starts?
int colBaseOfBlock (int c) const
 where does the col at block-col r starts?
size_t nonZeros () const
 number of non-zero elements
size_t nonZeroBlocks () const
 number of allocated blocks
SparseBlockMatrix * clone () const
 deep copy of a sparse-block-matrix;
SparseBlockMatrix * slice (int rmin, int rmax, int cmin, int cmax, bool alloc=true) const
template<class MatrixTransposedType>
bool transpose (SparseBlockMatrix< MatrixTransposedType > &dest) const
template<class MatrixTransposedType>
std::unique_ptr< SparseBlockMatrix< MatrixTransposedType > > transposed () const
bool add (SparseBlockMatrix< MatrixType > &dest) const
 adds the current matrix to the destination
std::unique_ptr< SparseBlockMatrix< MatrixType > > added () const
template<class MatrixResultType, class MatrixFactorType>
bool multiply (SparseBlockMatrix< MatrixResultType > *&dest, const SparseBlockMatrix< MatrixFactorType > *M) const
 dest = (*this) * M
void multiply (double *&dest, const double *src) const
 dest = (*this) * src
void multiplySymmetricUpperTriangle (double *&dest, const double *src) const
void rightMultiply (double *&dest, const double *src) const
 dest = M * (*this)
void scale (double a)
 *this *= a
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
void fillBlockStructure (int *Cp, int *Ci) const
 exports the non zero blocks into a column compressed structure
const std::vector< IntBlockMap > & blockCols () const
 the block matrices per block-column
std::vector< IntBlockMap > & blockCols ()
const std::vector< int > & rowBlockIndices () const
 indices of the row blocks
std::vector< int > & rowBlockIndices ()
const std::vector< int > & colBlockIndices () const
 indices of the column blocks
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
std::vector< int > _colBlockIndices
std::vector< IntBlockMap > _blockCols
bool _hasStorage

Private Member Functions

template<class MatrixTransposedType>
void transpose_internal (SparseBlockMatrix< MatrixTransposedType > &dest) const
void add_internal (SparseBlockMatrix< MatrixType > &dest) const

Detailed Description

template<class MatrixType = MatrixX>
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 64 of file sparse_block_matrix.h.

Member Typedef Documentation

◆ IntBlockMap

template<class MatrixType = MatrixX>
typedef std::map<int, SparseMatrixBlock*> g2o::SparseBlockMatrix< MatrixType >::IntBlockMap

Definition at line 78 of file sparse_block_matrix.h.

◆ SparseMatrixBlock

template<class MatrixType = MatrixX>
typedef MatrixType g2o::SparseBlockMatrix< MatrixType >::SparseMatrixBlock

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

Definition at line 67 of file sparse_block_matrix.h.

Constructor & Destructor Documentation

◆ SparseBlockMatrix() [1/2]

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
rbiarray 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.
cbiarray 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.
rbnumber of row blocks
cbnumber of col blocks
hasStorageset 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 32 of file sparse_block_matrix.hpp.

Sparse matrix which uses blocks.
std::vector< int > _rowBlockIndices
std::vector< int > _colBlockIndices
std::vector< IntBlockMap > _blockCols

References _blockCols, _colBlockIndices, _hasStorage, and _rowBlockIndices.

Referenced by add(), add_internal(), block(), block(), clear(), clone(), fillCCS(), fillCCS(), multiply(), multiplySymmetricUpperTriangle(), nonZeros(), rightMultiply(), scale(), slice(), symmPermutation(), takePatternFromHash(), transpose(), transpose_internal(), and writeOctave().

◆ SparseBlockMatrix() [2/2]

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

Definition at line 41 of file sparse_block_matrix.hpp.

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

References _blockCols, and _hasStorage.

◆ ~SparseBlockMatrix()

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

Definition at line 64 of file sparse_block_matrix.hpp.

64 {
65 if (_hasStorage) clear(true);
66}
void clear(bool dealloc=false)

References _hasStorage, and clear().

Member Function Documentation

◆ add()

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

adds the current matrix to the destination

Definition at line 184 of file sparse_block_matrix.hpp.

185 {
186 if (!dest._hasStorage) return false;
187 if (_rowBlockIndices.size() != dest._rowBlockIndices.size()) return false;
188 if (_colBlockIndices.size() != dest._colBlockIndices.size()) return false;
189 for (size_t i = 0; i < _rowBlockIndices.size(); ++i) {
190 if (_rowBlockIndices[i] != dest._rowBlockIndices[i]) return false;
191 }
192 for (size_t i = 0; i < _colBlockIndices.size(); ++i) {
193 if (_colBlockIndices[i] != dest._colBlockIndices[i]) return false;
194 }
195
197 return true;
198}
void add_internal(SparseBlockMatrix< MatrixType > &dest) const

References _colBlockIndices, _hasStorage, _rowBlockIndices, and SparseBlockMatrix().

◆ add_internal()

template<class MatrixType>
void g2o::SparseBlockMatrix< MatrixType >::add_internal ( SparseBlockMatrix< MatrixType > & dest) const
private

Definition at line 169 of file sparse_block_matrix.hpp.

170 {
171 for (size_t i = 0; i < _blockCols.size(); ++i) {
173 it = _blockCols[i].begin();
174 it != _blockCols[i].end(); ++it) {
177 dest.block(it->first, i, true);
178 (*d) += *s;
179 }
180 }
181}
SparseMatrixBlock * block(int r, int c, bool alloc=false)

References _blockCols, and SparseBlockMatrix().

◆ added()

template<class MatrixType>
std::unique_ptr< SparseBlockMatrix< MatrixType > > g2o::SparseBlockMatrix< MatrixType >::added ( ) const

Definition at line 202 of file sparse_block_matrix.hpp.

202 {
205 _colBlockIndices.size());
206 add_internal(*a);
207 return a;
208}

References _colBlockIndices, and _rowBlockIndices.

◆ block() [1/2]

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 97 of file sparse_block_matrix.hpp.

97 {
99 _blockCols[c].find(r);
100 if (it == _blockCols[c].end()) return nullptr;
101 return it->second;
102}

References SparseBlockMatrix().

◆ block() [2/2]

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 70 of file sparse_block_matrix.hpp.

70 {
72 _blockCols[c].find(r);
74 if (it == _blockCols[c].end()) {
75 if (!_hasStorage && !alloc)
76 return nullptr;
77 else {
78 int rb = rowsOfBlock(r);
79 int cb = colsOfBlock(c);
80 _block =
82 _block->setZero();
84 bool>
86 (void)result;
87 assert(result.second);
88 }
89 } else {
90 _block = it->second;
91 }
92 return _block;
93}
int rowsOfBlock(int r) const
how many rows does the block at block-row r has?
MatrixType SparseMatrixBlock
this is the type of the elementary block, it is an Eigen::Matrix.
int colsOfBlock(int c) const
how many cols does the block at block-col c has?

References _blockCols, _hasStorage, colsOfBlock(), rowsOfBlock(), and SparseBlockMatrix().

Referenced by g2o::MarginalCovarianceCholesky::computeCovariance(), g2o::EdgeLabeler::labelEdge(), main(), main(), g2o::SolverSLAM2DLinear::solveOrientation(), symmPermutation(), testMarginals(), and transpose_internal().

◆ blockCols() [1/2]

template<class MatrixType = MatrixX>
std::vector< IntBlockMap > & g2o::SparseBlockMatrix< MatrixType >::blockCols ( )
inline

Definition at line 212 of file sparse_block_matrix.h.

212{ return _blockCols; }

◆ blockCols() [2/2]

template<class MatrixType = MatrixX>
const std::vector< IntBlockMap > & g2o::SparseBlockMatrix< MatrixType >::blockCols ( ) const
inline

◆ clear()

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 45 of file sparse_block_matrix.hpp.

45 {
46#ifdef G2O_OPENMP
47#pragma omp parallel for default(shared) if (_blockCols.size() > 100)
48#endif
49 for (int i = 0; i < static_cast<int>(_blockCols.size()); ++i) {
51 it = _blockCols[i].begin();
52 it != _blockCols[i].end(); ++it) {
54 if (_hasStorage && dealloc)
55 delete b;
56 else
57 b->setZero();
58 }
59 if (_hasStorage && dealloc) _blockCols[i].clear();
60 }
61}

References _blockCols, _hasStorage, and SparseBlockMatrix().

Referenced by symmPermutation(), and ~SparseBlockMatrix().

◆ clone()

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

deep copy of a sparse-block-matrix;

Definition at line 105 of file sparse_block_matrix.hpp.

105 {
108 _rowBlockIndices.size(), _colBlockIndices.size());
109 for (size_t i = 0; i < _blockCols.size(); ++i) {
111 it = _blockCols[i].begin();
112 it != _blockCols[i].end(); ++it) {
115 *it->second);
116 ret->_blockCols[i].insert(std::make_pair(it->first, b));
117 }
118 }
119 ret->_hasStorage = true;
120 return ret;
121}
SparseBlockMatrix(const int *rbi, const int *cbi, int rb, int cb, bool hasStorage=true)

References SparseBlockMatrix().

◆ colBaseOfBlock()

◆ colBlockIndices() [1/2]

template<class MatrixType = MatrixX>
std::vector< int > & g2o::SparseBlockMatrix< MatrixType >::colBlockIndices ( )
inline

Definition at line 220 of file sparse_block_matrix.h.

220{ return _colBlockIndices; }

◆ colBlockIndices() [2/2]

template<class MatrixType = MatrixX>
const std::vector< int > & g2o::SparseBlockMatrix< MatrixType >::colBlockIndices ( ) const
inline

◆ cols()

◆ colsOfBlock()

template<class MatrixType = MatrixX>
int g2o::SparseBlockMatrix< MatrixType >::colsOfBlock ( int c) const
inline

◆ fillBlockStructure() [1/2]

template<class MatrixType>
void g2o::SparseBlockMatrix< MatrixType >::fillBlockStructure ( int * Cp,
int * Ci ) const

exports the non zero blocks into a column compressed structure

Definition at line 582 of file sparse_block_matrix.hpp.

582 {
583 int nz = 0;
584 for (int c = 0; c < static_cast<int>(_blockCols.size()); ++c) {
585 *Cp = nz;
586 for (auto it = _blockCols[c].begin(); it != _blockCols[c].end(); ++it) {
587 const int& r = it->first;
588 if (r <= c) {
589 *Ci++ = r;
590 ++nz;
591 }
592 }
593 Cp++;
594 }
595 *Cp = nz;
596 assert(nz <= static_cast<int>(nonZeroBlocks()));
597}
size_t nonZeroBlocks() const
number of allocated blocks

References _blockCols, and nonZeroBlocks().

◆ fillBlockStructure() [2/2]

◆ fillCCS() [1/2]

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 516 of file sparse_block_matrix.hpp.

517 {
518 assert(Cx && "Target destination is NULL");
519 double* CxStart = Cx;
520 for (size_t i = 0; i < _blockCols.size(); ++i) {
521 int cstart = i ? _colBlockIndices[i - 1] : 0;
522 int csize = colsOfBlock(i);
523 for (int c = 0; c < csize; ++c) {
525 it = _blockCols[i].begin();
526 it != _blockCols[i].end(); ++it) {
528 it->second;
529 int rstart = it->first ? _rowBlockIndices[it->first - 1] : 0;
530
531 int elemsToCopy = b->rows();
532 if (upperTriangle && rstart == cstart) elemsToCopy = c + 1;
533 memcpy(Cx, b->data() + c * b->rows(), elemsToCopy * sizeof(double));
534 Cx += elemsToCopy;
535 }
536 }
537 }
538 return Cx - CxStart;
539}
int rows() const
rows of the matrix

References _blockCols, _colBlockIndices, _rowBlockIndices, colsOfBlock(), rows(), and SparseBlockMatrix().

◆ fillCCS() [2/2]

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 542 of file sparse_block_matrix.hpp.

543 {
544 assert(Cp && Ci && Cx && "Target destination is NULL");
545 int nz = 0;
546 for (size_t i = 0; i < _blockCols.size(); ++i) {
547 int cstart = i ? _colBlockIndices[i - 1] : 0;
548 int csize = colsOfBlock(i);
549 for (int c = 0; c < csize; ++c) {
550 *Cp = nz;
552 it = _blockCols[i].begin();
553 it != _blockCols[i].end(); ++it) {
555 it->second;
556 int rstart = it->first ? _rowBlockIndices[it->first - 1] : 0;
557
558 int elemsToCopy = b->rows();
559 if (upperTriangle && rstart == cstart) elemsToCopy = c + 1;
560 for (int r = 0; r < elemsToCopy; ++r) {
561 *Cx++ = (*b)(r, c);
562 *Ci++ = rstart++;
563 ++nz;
564 }
565 }
566 ++Cp;
567 }
568 }
569 *Cp = nz;
570 return nz;
571}

References _blockCols, _colBlockIndices, _rowBlockIndices, colsOfBlock(), rows(), and SparseBlockMatrix().

Referenced by g2o::SparseOptimizerIncremental::computeCholeskyUpdate(), and g2o::LinearSolverCholmodOnline< MatrixType >::fillCholmodExt().

◆ fillSparseBlockMatrixCCS()

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 646 of file sparse_block_matrix.hpp.

647 {
648 blockCCS.blockCols().resize(blockCols().size());
649 int numblocks = 0;
650 for (size_t i = 0; i < blockCols().size(); ++i) {
651 const IntBlockMap& row = blockCols()[i];
654 dest.clear();
655 dest.reserve(row.size());
656 for (typename IntBlockMap::const_iterator it = row.begin(); it != row.end();
657 ++it) {
659 it->first, it->second));
660 ++numblocks;
661 }
662 }
663 return numblocks;
664}
const std::vector< IntBlockMap > & blockCols() const
the block matrices per block-column
std::map< int, SparseMatrixBlock * > IntBlockMap

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

Referenced by g2o::LinearSolverCCS< MatrixType >::initMatrixStructure().

◆ fillSparseBlockMatrixCCSTransposed()

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 667 of file sparse_block_matrix.hpp.

668 {
669 blockCCS.blockCols().clear();
670 blockCCS.blockCols().resize(_rowBlockIndices.size());
671 int numblocks = 0;
672 for (size_t i = 0; i < blockCols().size(); ++i) {
673 const IntBlockMap& row = blockCols()[i];
674 for (typename IntBlockMap::const_iterator it = row.begin(); it != row.end();
675 ++it) {
677 blockCCS.blockCols()[it->first];
678 dest.push_back(
680 ++numblocks;
681 }
682 }
683 return numblocks;
684}

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

◆ multiply() [1/2]

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

dest = (*this) * src

Definition at line 255 of file sparse_block_matrix.hpp.

256 {
257 if (!dest) {
258 dest = new double[_rowBlockIndices[_rowBlockIndices.size() - 1]];
259 memset(dest, 0,
260 _rowBlockIndices[_rowBlockIndices.size() - 1] * sizeof(double));
261 }
262
263 // map the memory by Eigen
266
267 for (size_t i = 0; i < _blockCols.size(); ++i) {
268 int srcOffset = i ? _colBlockIndices[i - 1] : 0;
269
271 it = _blockCols[i].begin();
272 it != _blockCols[i].end(); ++it) {
274 it->second;
275 int destOffset = it->first ? _rowBlockIndices[it->first - 1] : 0;
276 // destVec += *a * srcVec (according to the sub-vector parts)
280 }
281 }
282}
int cols() const
columns of the matrix

References _rowBlockIndices.

◆ multiply() [2/2]

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 212 of file sparse_block_matrix.hpp.

214 {
215 // sanity check
216 if (_colBlockIndices.size() != M->_rowBlockIndices.size()) return false;
217 for (size_t i = 0; i < _colBlockIndices.size(); ++i) {
218 if (_colBlockIndices[i] != M->_rowBlockIndices[i]) return false;
219 }
220 if (!dest) {
223 _rowBlockIndices.size(), M->_colBlockIndices.size());
224 }
225 if (!dest->_hasStorage) return false;
226 for (size_t i = 0; i < M->_blockCols.size(); ++i) {
227 for (typename SparseBlockMatrix<
229 M->_blockCols[i].begin();
230 it != M->_blockCols[i].end(); ++it) {
231 // look for a non-zero block in a row of column it
232 int colM = i;
234 it->second;
236 _blockCols[it->first].begin();
237 while (rbt != _blockCols[it->first].end()) {
238 // int colA=it->first;
239 int rowA = rbt->first;
241 rbt->second;
243 dest->block(rowA, colM, true);
244 assert(c->rows() == a->rows());
245 assert(c->cols() == b->cols());
246 ++rbt;
247 (*c) += (*a) * (*b);
248 }
249 }
250 }
251 return true;
252}

References _blockCols, _colBlockIndices, _hasStorage, _rowBlockIndices, and SparseBlockMatrix().

◆ multiplySymmetricUpperTriangle()

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 285 of file sparse_block_matrix.hpp.

286 {
287 if (!dest) {
288 dest = new double[_rowBlockIndices[_rowBlockIndices.size() - 1]];
289 memset(dest, 0,
290 _rowBlockIndices[_rowBlockIndices.size() - 1] * sizeof(double));
291 }
292
293 // map the memory by Eigen
296
297 for (size_t i = 0; i < _blockCols.size(); ++i) {
300 it = _blockCols[i].begin();
301 it != _blockCols[i].end(); ++it) {
303 it->second;
304 int destOffset = rowBaseOfBlock(it->first);
305 if (destOffset > srcOffset) // only upper triangle
306 break;
307 // destVec += *a * srcVec (according to the sub-vector parts)
311 if (destOffset < srcOffset)
315 }
316 }
317}
int colBaseOfBlock(int c) const
where does the col at block-col r starts?
int rowBaseOfBlock(int r) const
where does the row at block-row r starts?

References _blockCols, _rowBlockIndices, colBaseOfBlock(), cols(), rowBaseOfBlock(), rows(), and SparseBlockMatrix().

◆ nonZeroBlocks()

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

number of allocated blocks

Definition at line 401 of file sparse_block_matrix.hpp.

401 {
402 size_t count = 0;
403 for (size_t i = 0; i < _blockCols.size(); ++i) count += _blockCols[i].size();
404 return count;
405}

References _blockCols.

Referenced by g2o::LinearSolverEigen< MatrixType >::computeSymbolicDecomposition(), fillBlockStructure(), fillBlockStructure(), and nonZeros().

◆ nonZeros()

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

◆ rightMultiply()

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

dest = M * (*this)

Definition at line 320 of file sparse_block_matrix.hpp.

321 {
322 int destSize = cols();
323
324 if (!dest) {
325 dest = new double[destSize];
326 memset(dest, 0, destSize * sizeof(double));
327 }
328
329 // map the memory by Eigen
332
333#ifdef G2O_OPENMP
334#pragma omp parallel for default(shared) schedule(dynamic, 10)
335#endif
336 for (int i = 0; i < static_cast<int>(_blockCols.size()); ++i) {
339 it = _blockCols[i].begin();
340 it != _blockCols[i].end(); ++it) {
342 it->second;
343 int srcOffset = rowBaseOfBlock(it->first);
344 // destVec += *a.transpose() * srcVec (according to the sub-vector parts)
348 }
349 }
350}

References _blockCols, colBaseOfBlock(), cols(), rowBaseOfBlock(), rows(), and SparseBlockMatrix().

◆ rowBaseOfBlock()

template<class MatrixType = MatrixX>
int g2o::SparseBlockMatrix< MatrixType >::rowBaseOfBlock ( int r) const
inline

where does the row at block-row r starts?

Definition at line 124 of file sparse_block_matrix.h.

124 {
125 return r ? _rowBlockIndices[r - 1] : 0;
126 }

Referenced by g2o::MarginalCovarianceCholesky::computeCovariance(), multiplySymmetricUpperTriangle(), rightMultiply(), g2o::LinearSolverDense< MatrixType >::solve(), and writeOctave().

◆ rowBlockIndices() [1/2]

template<class MatrixType = MatrixX>
std::vector< int > & g2o::SparseBlockMatrix< MatrixType >::rowBlockIndices ( )
inline

Definition at line 216 of file sparse_block_matrix.h.

216{ return _rowBlockIndices; }

◆ rowBlockIndices() [2/2]

◆ rows()

◆ rowsOfBlock()

template<class MatrixType = MatrixX>
int g2o::SparseBlockMatrix< MatrixType >::rowsOfBlock ( int r) const
inline

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

Definition at line 112 of file sparse_block_matrix.h.

112 {
113 return r ? _rowBlockIndices[r] - _rowBlockIndices[r - 1]
114 : _rowBlockIndices[0];
115 }

Referenced by g2o::LinearSolver< PoseMatrixType >::allocateBlocks(), block(), slice(), and g2o::LinearSolverDense< MatrixType >::solve().

◆ scale()

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

*this *= a

Definition at line 353 of file sparse_block_matrix.hpp.

353 {
354 for (size_t i = 0; i < _blockCols.size(); ++i) {
356 it = _blockCols[i].begin();
357 it != _blockCols[i].end(); ++it) {
359 *a *= a_;
360 }
361 }
362}

References _blockCols, and SparseBlockMatrix().

◆ slice()

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
rminstarting block row
rmaxending block row
cminstarting block col
cmaxending block col
allocif true it makes a deep copy, if false it creates a view.

Definition at line 365 of file sparse_block_matrix.hpp.

366 {
367 int m = rmax - rmin;
368 int n = cmax - cmin;
369 int rowIdx[m];
370 rowIdx[0] = rowsOfBlock(rmin);
371 for (int i = 1; i < m; ++i) {
372 rowIdx[i] = rowIdx[i - 1] + rowsOfBlock(rmin + i);
373 }
374
375 int colIdx[n];
376 colIdx[0] = colsOfBlock(cmin);
377 for (int i = 1; i < n; ++i) {
378 colIdx[i] = colIdx[i - 1] + colsOfBlock(cmin + i);
379 }
381 for (int i = 0; i < n; ++i) {
382 int mc = cmin + i;
384 it = _blockCols[mc].begin();
385 it != _blockCols[mc].end(); ++it) {
386 if (it->first >= rmin && it->first < rmax) {
388 alloc ? new
390 *(it->second))
391 : it->second;
392 s->_blockCols[i].insert(std::make_pair(it->first - rmin, b));
393 }
394 }
395 }
397 return s;
398}

References _blockCols, _hasStorage, colsOfBlock(), rowsOfBlock(), and SparseBlockMatrix().

◆ symmPermutation()

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
pinvarray such that new_block[i] = old_block[pinv[i]]

Definition at line 453 of file sparse_block_matrix.hpp.

455 {
456 // compute the permuted version of the new row/column layout
457 size_t n = _rowBlockIndices.size();
458 // computed the block sizes
461 for (size_t i = 1; i < n; ++i) {
463 }
464 // permute them
466 for (size_t i = 0; i < n; ++i) {
468 }
469 for (size_t i = 1; i < n; ++i) {
471 }
472 // allocate C, or check the structure;
473 if (!dest) {
474 dest =
475 new SparseBlockMatrix(pBlockIndices.data(), pBlockIndices.data(), n, n);
476 } else {
477 if (dest->_rowBlockIndices.size() != n) return false;
478 if (dest->_colBlockIndices.size() != n) return false;
479 for (size_t i = 0; i < n; ++i) {
480 if (dest->_rowBlockIndices[i] != pBlockIndices[i]) return false;
481 if (dest->_colBlockIndices[i] != pBlockIndices[i]) return false;
482 }
483 dest->clear();
484 }
485 // now ready to permute the columns
486 for (size_t i = 0; i < n; ++i) {
487 int pi = pinv[i];
489 it = _blockCols[i].begin();
490 it != _blockCols[i].end(); ++it) {
491 int pj = pinv[it->first];
492
494 it->second;
495
497 if (!upperTriangle || pj <= pi) {
498 b = dest->block(pj, pi, true);
499 assert(b->cols() == s->cols());
500 assert(b->rows() == s->rows());
501 *b = *s;
502 } else {
503 b = dest->block(pi, pj, true);
504 assert(b);
505 assert(b->rows() == s->cols());
506 assert(b->cols() == s->rows());
507 *b = s->transpose();
508 }
509 }
510 // within each row,
511 }
512 return true;
513}
bool transpose(SparseBlockMatrix< MatrixTransposedType > &dest) const

References _blockCols, _colBlockIndices, _rowBlockIndices, block(), clear(), cols(), rows(), SparseBlockMatrix(), and transpose().

◆ takePatternFromHash()

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 687 of file sparse_block_matrix.hpp.

688 {
689 // sort the sparse columns and add them to the map structures by
690 // exploiting that we are inserting a sorted structure
694 for (size_t i = 0; i < hashMatrix.blockCols().size(); ++i) {
695 // prepare a temporary vector for sorting
697 if (column.size() == 0) continue;
698 std::vector<SparseColumnPair> sparseRowSorted; // temporary structure
699 sparseRowSorted.reserve(column.size());
700 for (typename HashSparseColumn::const_iterator it = column.begin();
701 it != column.end(); ++it)
702 sparseRowSorted.push_back(*it);
705 // try to free some memory early
708 // now insert sorted vector to the std::map structure
711 for (size_t j = 1; j < sparseRowSorted.size(); ++j) {
713 destColumnMap.end();
714 --hint; // cppreference says the element goes after the hint (until
715 // C++11)
717 }
718 }
719}

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

◆ transpose()

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 141 of file sparse_block_matrix.hpp.

142 {
143 if (!dest._hasStorage) return false;
144 if (_rowBlockIndices.size() != dest._colBlockIndices.size()) return false;
145 if (_colBlockIndices.size() != dest._rowBlockIndices.size()) return false;
146 for (size_t i = 0; i < _rowBlockIndices.size(); ++i) {
147 if (_rowBlockIndices[i] != dest._colBlockIndices[i]) return false;
148 }
149 for (size_t i = 0; i < _colBlockIndices.size(); ++i) {
150 if (_colBlockIndices[i] != dest._rowBlockIndices[i]) return false;
151 }
152
154 return true;
155}
void transpose_internal(SparseBlockMatrix< MatrixTransposedType > &dest) const

References _colBlockIndices, _hasStorage, _rowBlockIndices, and SparseBlockMatrix().

Referenced by g2o::LinearSolverDense< MatrixType >::solve(), and symmPermutation().

◆ transpose_internal()

template<class MatrixType>
template<class MatrixTransposedType>
void g2o::SparseBlockMatrix< MatrixType >::transpose_internal ( SparseBlockMatrix< MatrixTransposedType > & dest) const
private

Definition at line 125 of file sparse_block_matrix.hpp.

126 {
127 for (size_t i = 0; i < _blockCols.size(); ++i) {
129 it = _blockCols[i].begin();
130 it != _blockCols[i].end(); ++it) {
133 dest.block(i, it->first, true);
134 *d = s->transpose();
135 }
136 }
137}

References _blockCols, block(), and SparseBlockMatrix().

◆ transposed()

template<class MatrixType>
template<class MatrixTransposedType>
std::unique_ptr< SparseBlockMatrix< MatrixTransposedType > > g2o::SparseBlockMatrix< MatrixType >::transposed ( ) const

Definition at line 160 of file sparse_block_matrix.hpp.

◆ writeOctave()

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
upperTriangledoes this matrix store only the upper triangular blocks

Definition at line 600 of file sparse_block_matrix.hpp.

601 {
603 std::string::size_type lastDot = name.find_last_of('.');
604 if (lastDot != std::string::npos) name.resize(lastDot);
605
607 for (size_t i = 0; i < _blockCols.size(); ++i) {
608 const int& c = i;
610 it = _blockCols[i].begin();
611 it != _blockCols[i].end(); ++it) {
612 const int& r = it->first;
613 const MatrixType& m = *(it->second);
614 for (int cc = 0; cc < m.cols(); ++cc)
615 for (int rr = 0; rr < m.rows(); ++rr) {
616 int aux_r = rowBaseOfBlock(r) + rr;
617 int aux_c = colBaseOfBlock(c) + cc;
618 entries.push_back(TripletEntry(aux_r, aux_c, m(rr, cc)));
619 if (upperTriangle && r != c) {
620 entries.push_back(TripletEntry(aux_c, aux_r, m(rr, cc)));
621 }
622 }
623 }
624 }
625
626 int nz = entries.size();
627 std::sort(entries.begin(), entries.end(), TripletColSort());
628
630 fout << "# name: " << name << std::endl;
631 fout << "# type: sparse matrix" << std::endl;
632 fout << "# nnz: " << nz << std::endl;
633 fout << "# rows: " << rows() << std::endl;
634 fout << "# columns: " << cols() << std::endl;
636
638 it != entries.end(); ++it) {
639 const TripletEntry& entry = *it;
640 fout << entry.r + 1 << " " << entry.c + 1 << " " << entry.x << std::endl;
641 }
642 return fout.good();
643}

References _blockCols, g2o::TripletEntry::c, colBaseOfBlock(), cols(), g2o::TripletEntry::r, rowBaseOfBlock(), rows(), SparseBlockMatrix(), and g2o::TripletEntry::x.

Referenced by g2o::LinearSolverEigen< MatrixType >::computeCholesky().

Member Data Documentation

◆ _blockCols

template<class MatrixType = MatrixX>
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 257 of file sparse_block_matrix.h.

Referenced by add_internal(), block(), clear(), fillBlockStructure(), fillCCS(), fillCCS(), multiply(), multiplySymmetricUpperTriangle(), nonZeroBlocks(), nonZeros(), rightMultiply(), scale(), slice(), SparseBlockMatrix(), SparseBlockMatrix(), symmPermutation(), transpose_internal(), and writeOctave().

◆ _colBlockIndices

template<class MatrixType = MatrixX>
std::vector<int> g2o::SparseBlockMatrix< MatrixType >::_colBlockIndices
protected

vector of the indices of the blocks along the cols

Definition at line 253 of file sparse_block_matrix.h.

Referenced by add(), added(), fillBlockStructure(), fillCCS(), fillCCS(), multiply(), SparseBlockMatrix(), symmPermutation(), and transpose().

◆ _hasStorage

template<class MatrixType = MatrixX>
bool g2o::SparseBlockMatrix< MatrixType >::_hasStorage
protected

◆ _rowBlockIndices

template<class MatrixType = MatrixX>
std::vector<int> g2o::SparseBlockMatrix< MatrixType >::_rowBlockIndices
protected

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