sparse_block_matrix.hpp
Go to the documentation of this file.
1 // g2o - General Graph Optimization
2 // Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard
3 // All rights reserved.
4 //
5 // Redistribution and use in source and binary forms, with or without
6 // modification, are permitted provided that the following conditions are
7 // met:
8 //
9 // * Redistributions of source code must retain the above copyright notice,
10 // this list of conditions and the following disclaimer.
11 // * Redistributions in binary form must reproduce the above copyright
12 // notice, this list of conditions and the following disclaimer in the
13 // documentation and/or other materials provided with the distribution.
14 //
15 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS
16 // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
17 // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
18 // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
19 // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
20 // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
21 // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
22 // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
23 // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
24 // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
25 // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
26 
27 namespace g2o {
28  using namespace Eigen;
29 
30  namespace {
31  struct TripletEntry
32  {
33  int r, c;
34  double x;
35  TripletEntry(int r_, int c_, double x_) : r(r_), c(c_), x(x_) {}
36  };
37  struct TripletColSort
38  {
39  bool operator()(const TripletEntry& e1, const TripletEntry& e2) const
40  {
41  return e1.c < e2.c || (e1.c == e2.c && e1.r < e2.r);
42  }
43  };
45  template<class T1, class T2, class Pred = std::less<T1> >
46  struct CmpPairFirst {
47  bool operator()(const std::pair<T1,T2>& left, const std::pair<T1,T2>& right) {
48  return Pred()(left.first, right.first);
49  }
50  };
51  }
52 
53  template <class MatrixType>
54  SparseBlockMatrix<MatrixType>::SparseBlockMatrix( const int * rbi, const int* cbi, int rb, int cb, bool hasStorage):
55  _rowBlockIndices(rbi,rbi+rb),
56  _colBlockIndices(cbi,cbi+cb),
57  _blockCols(cb), _hasStorage(hasStorage)
58  {
59  }
60 
61  template <class MatrixType>
63  _blockCols(0), _hasStorage(true)
64  {
65  }
66 
67  template <class MatrixType>
69 # ifdef G2O_OPENMP
70 # pragma omp parallel for default (shared) if (_blockCols.size() > 100)
71 # endif
72  for (int i=0; i < static_cast<int>(_blockCols.size()); ++i) {
73  for (typename SparseBlockMatrix<MatrixType>::IntBlockMap::const_iterator it=_blockCols[i].begin(); it!=_blockCols[i].end(); ++it){
75  if (_hasStorage && dealloc)
76  delete b;
77  else
78  b->setZero();
79  }
80  if (_hasStorage && dealloc)
81  _blockCols[i].clear();
82  }
83  }
84 
85  template <class MatrixType>
87  if (_hasStorage)
88  clear(true);
89  }
90 
91  template <class MatrixType>
95  if (it==_blockCols[c].end()){
96  if (!_hasStorage && ! alloc )
97  return 0;
98  else {
99  int rb=rowsOfBlock(r);
100  int cb=colsOfBlock(c);
101  _block=new typename SparseBlockMatrix<MatrixType>::SparseMatrixBlock(rb,cb);
102  _block->setZero();
103  std::pair < typename SparseBlockMatrix<MatrixType>::IntBlockMap::iterator, bool> result
104  =_blockCols[c].insert(std::make_pair(r,_block)); (void) result;
105  assert (result.second);
106  }
107  } else {
108  _block=it->second;
109  }
110  return _block;
111  }
112 
113  template <class MatrixType>
116  if (it==_blockCols[c].end())
117  return 0;
118  return it->second;
119  }
120 
121 
122  template <class MatrixType>
125  for (size_t i=0; i<_blockCols.size(); ++i){
126  for (typename SparseBlockMatrix<MatrixType>::IntBlockMap::const_iterator it=_blockCols[i].begin(); it!=_blockCols[i].end(); ++it){
128  ret->_blockCols[i].insert(std::make_pair(it->first, b));
129  }
130  }
131  ret->_hasStorage=true;
132  return ret;
133  }
134 
135 
136  template <class MatrixType>
137  template <class MatrixTransposedType>
139  if (! dest){
140  dest=new SparseBlockMatrix<MatrixTransposedType>(&_colBlockIndices[0], &_rowBlockIndices[0], _colBlockIndices.size(), _rowBlockIndices.size());
141  } else {
142  if (! dest->_hasStorage)
143  return false;
144  if(_rowBlockIndices.size()!=dest->_colBlockIndices.size())
145  return false;
146  if (_colBlockIndices.size()!=dest->_rowBlockIndices.size())
147  return false;
148  for (size_t i=0; i<_rowBlockIndices.size(); ++i){
149  if(_rowBlockIndices[i]!=dest->_colBlockIndices[i])
150  return false;
151  }
152  for (size_t i=0; i<_colBlockIndices.size(); ++i){
153  if(_colBlockIndices[i]!=dest->_rowBlockIndices[i])
154  return false;
155  }
156  }
157 
158  for (size_t i=0; i<_blockCols.size(); ++i){
159  for (typename SparseBlockMatrix<MatrixType>::IntBlockMap::const_iterator it=_blockCols[i].begin(); it!=_blockCols[i].end(); ++it){
161  typename SparseBlockMatrix<MatrixTransposedType>::SparseMatrixBlock* d=dest->block(i,it->first,true);
162  *d = s->transpose();
163  }
164  }
165  return true;
166  }
167 
168  template <class MatrixType>
170  if (! dest){
172  } else {
173  if (! dest->_hasStorage)
174  return false;
175  if(_rowBlockIndices.size()!=dest->_rowBlockIndices.size())
176  return false;
177  if (_colBlockIndices.size()!=dest->_colBlockIndices.size())
178  return false;
179  for (size_t i=0; i<_rowBlockIndices.size(); ++i){
180  if(_rowBlockIndices[i]!=dest->_rowBlockIndices[i])
181  return false;
182  }
183  for (size_t i=0; i<_colBlockIndices.size(); ++i){
184  if(_colBlockIndices[i]!=dest->_colBlockIndices[i])
185  return false;
186  }
187  }
188  for (size_t i=0; i<_blockCols.size(); ++i){
189  for (typename SparseBlockMatrix<MatrixType>::IntBlockMap::const_iterator it=_blockCols[i].begin(); it!=_blockCols[i].end(); ++it){
191  typename SparseBlockMatrix<MatrixType>::SparseMatrixBlock* d=dest->block(it->first,i,true);
192  (*d)+=*s;
193  }
194  }
195  return true;
196  }
197 
198  template <class MatrixType>
199  template < class MatrixResultType, class MatrixFactorType >
201  // sanity check
202  if (_colBlockIndices.size()!=M->_rowBlockIndices.size())
203  return false;
204  for (size_t i=0; i<_colBlockIndices.size(); ++i){
205  if (_colBlockIndices[i]!=M->_rowBlockIndices[i])
206  return false;
207  }
208  if (! dest) {
209  dest=new SparseBlockMatrix<MatrixResultType>(&_rowBlockIndices[0],&(M->_colBlockIndices[0]), _rowBlockIndices.size(), M->_colBlockIndices.size() );
210  }
211  if (! dest->_hasStorage)
212  return false;
213  for (size_t i=0; i<M->_blockCols.size(); ++i){
214  for (typename SparseBlockMatrix<MatrixFactorType>::IntBlockMap::const_iterator it=M->_blockCols[i].begin(); it!=M->_blockCols[i].end(); ++it){
215  // look for a non-zero block in a row of column it
216  int colM=i;
217  const typename SparseBlockMatrix<MatrixFactorType>::SparseMatrixBlock *b=it->second;
219  while(rbt!=_blockCols[it->first].end()){
220  //int colA=it->first;
221  int rowA=rbt->first;
222  typename SparseBlockMatrix<MatrixType>::SparseMatrixBlock *a=rbt->second;
223  typename SparseBlockMatrix<MatrixResultType>::SparseMatrixBlock *c=dest->block(rowA,colM,true);
224  assert (c->rows()==a->rows());
225  assert (c->cols()==b->cols());
226  ++rbt;
227  (*c)+=(*a)*(*b);
228  }
229  }
230  }
231  return false;
232  }
233 
234  template <class MatrixType>
235  void SparseBlockMatrix<MatrixType>::multiply(double*& dest, const double* src) const {
236  if (! dest){
237  dest=new double [_rowBlockIndices[_rowBlockIndices.size()-1] ];
238  memset(dest,0, _rowBlockIndices[_rowBlockIndices.size()-1]*sizeof(double));
239  }
240 
241  // map the memory by Eigen
242  Eigen::Map<VectorXd> destVec(dest, rows());
243  const Eigen::Map<const VectorXd> srcVec(src, cols());
244 
245  for (size_t i=0; i<_blockCols.size(); ++i){
246  int srcOffset = i ? _colBlockIndices[i-1] : 0;
247 
248  for (typename SparseBlockMatrix<MatrixType>::IntBlockMap::const_iterator it=_blockCols[i].begin(); it!=_blockCols[i].end(); ++it){
249  const typename SparseBlockMatrix<MatrixType>::SparseMatrixBlock* a=it->second;
250  int destOffset = it->first ? _rowBlockIndices[it->first - 1] : 0;
251  // destVec += *a * srcVec (according to the sub-vector parts)
252  internal::axpy(*a, srcVec, srcOffset, destVec, destOffset);
253  }
254  }
255  }
256 
257  template <class MatrixType>
258  void SparseBlockMatrix<MatrixType>::multiplySymmetricUpperTriangle(double*& dest, const double* src) const
259  {
260  if (! dest){
261  dest=new double [_rowBlockIndices[_rowBlockIndices.size()-1] ];
262  memset(dest,0, _rowBlockIndices[_rowBlockIndices.size()-1]*sizeof(double));
263  }
264 
265  // map the memory by Eigen
266  Eigen::Map<VectorXd> destVec(dest, rows());
267  const Eigen::Map<const VectorXd> srcVec(src, cols());
268 
269  for (size_t i=0; i<_blockCols.size(); ++i){
270  int srcOffset = colBaseOfBlock(i);
271  for (typename SparseBlockMatrix<MatrixType>::IntBlockMap::const_iterator it=_blockCols[i].begin(); it!=_blockCols[i].end(); ++it){
272  const typename SparseBlockMatrix<MatrixType>::SparseMatrixBlock* a=it->second;
273  int destOffset = rowBaseOfBlock(it->first);
274  if (destOffset > srcOffset) // only upper triangle
275  break;
276  // destVec += *a * srcVec (according to the sub-vector parts)
277  internal::axpy(*a, srcVec, srcOffset, destVec, destOffset);
278  if (destOffset < srcOffset)
279  internal::atxpy(*a, srcVec, destOffset, destVec, srcOffset);
280  }
281  }
282  }
283 
284  template <class MatrixType>
285  void SparseBlockMatrix<MatrixType>::rightMultiply(double*& dest, const double* src) const {
286  int destSize=cols();
287 
288  if (! dest){
289  dest=new double [ destSize ];
290  memset(dest,0, destSize*sizeof(double));
291  }
292 
293  // map the memory by Eigen
294  Eigen::Map<VectorXd> destVec(dest, destSize);
295  Eigen::Map<const VectorXd> srcVec(src, rows());
296 
297 # ifdef G2O_OPENMP
298 # pragma omp parallel for default (shared) schedule(dynamic, 10)
299 # endif
300  for (int i=0; i < static_cast<int>(_blockCols.size()); ++i){
301  int destOffset = colBaseOfBlock(i);
303  it!=_blockCols[i].end();
304  ++it){
305  const typename SparseBlockMatrix<MatrixType>::SparseMatrixBlock* a=it->second;
306  int srcOffset = rowBaseOfBlock(it->first);
307  // destVec += *a.transpose() * srcVec (according to the sub-vector parts)
308  internal::atxpy(*a, srcVec, srcOffset, destVec, destOffset);
309  }
310  }
311 
312  }
313 
314  template <class MatrixType>
316  for (size_t i=0; i<_blockCols.size(); ++i){
317  for (typename SparseBlockMatrix<MatrixType>::IntBlockMap::const_iterator it=_blockCols[i].begin(); it!=_blockCols[i].end(); ++it){
319  *a *= a_;
320  }
321  }
322  }
323 
324  template <class MatrixType>
325  SparseBlockMatrix<MatrixType>* SparseBlockMatrix<MatrixType>::slice(int rmin, int rmax, int cmin, int cmax, bool alloc) const {
326  int m=rmax-rmin;
327  int n=cmax-cmin;
328  int rowIdx [m];
329  rowIdx[0] = rowsOfBlock(rmin);
330  for (int i=1; i<m; ++i){
331  rowIdx[i]=rowIdx[i-1]+rowsOfBlock(rmin+i);
332  }
333 
334  int colIdx [n];
335  colIdx[0] = colsOfBlock(cmin);
336  for (int i=1; i<n; ++i){
337  colIdx[i]=colIdx[i-1]+colsOfBlock(cmin+i);
338  }
339  typename SparseBlockMatrix<MatrixType>::SparseBlockMatrix* s=new SparseBlockMatrix(rowIdx, colIdx, m, n, true);
340  for (int i=0; i<n; ++i){
341  int mc=cmin+i;
342  for (typename SparseBlockMatrix<MatrixType>::IntBlockMap::const_iterator it=_blockCols[mc].begin(); it!=_blockCols[mc].end(); ++it){
343  if (it->first >= rmin && it->first < rmax){
344  typename SparseBlockMatrix<MatrixType>::SparseMatrixBlock* b = alloc ? new typename SparseBlockMatrix<MatrixType>::SparseMatrixBlock (* (it->second) ) : it->second;
345  s->_blockCols[i].insert(std::make_pair(it->first-rmin, b));
346  }
347  }
348  }
349  s->_hasStorage=alloc;
350  return s;
351  }
352 
353  template <class MatrixType>
355  size_t count=0;
356  for (size_t i=0; i<_blockCols.size(); ++i)
357  count+=_blockCols[i].size();
358  return count;
359  }
360 
361  template <class MatrixType>
363  if (MatrixType::SizeAtCompileTime != Eigen::Dynamic) {
364  size_t nnz = nonZeroBlocks() * MatrixType::SizeAtCompileTime;
365  return nnz;
366  } else {
367  size_t count=0;
368  for (size_t i=0; i<_blockCols.size(); ++i){
369  for (typename SparseBlockMatrix<MatrixType>::IntBlockMap::const_iterator it=_blockCols[i].begin(); it!=_blockCols[i].end(); ++it){
370  const typename SparseBlockMatrix<MatrixType>::SparseMatrixBlock* a=it->second;
371  count += a->cols()*a->rows();
372  }
373  }
374  return count;
375  }
376  }
377 
378  template <class MatrixType>
379  std::ostream& operator << (std::ostream& os, const SparseBlockMatrix<MatrixType>& m){
380  os << "RBI: " << m.rowBlockIndices().size();
381  for (size_t i=0; i<m.rowBlockIndices().size(); ++i)
382  os << " " << m.rowBlockIndices()[i];
383  os << std::endl;
384  os << "CBI: " << m.colBlockIndices().size();
385  for (size_t i=0; i<m.colBlockIndices().size(); ++i)
386  os << " " << m.colBlockIndices()[i];
387  os << std::endl;
388 
389  for (size_t i=0; i<m.blockCols().size(); ++i){
390  for (typename SparseBlockMatrix<MatrixType>::IntBlockMap::const_iterator it=m.blockCols()[i].begin(); it!=m.blockCols()[i].end(); ++it){
391  const typename SparseBlockMatrix<MatrixType>::SparseMatrixBlock* b=it->second;
392  os << "BLOCK: " << it->first << " " << i << std::endl;
393  os << *b << std::endl;
394  }
395  }
396  return os;
397  }
398 
399  template <class MatrixType>
400  bool SparseBlockMatrix<MatrixType>::symmPermutation(SparseBlockMatrix<MatrixType>*& dest, const int* pinv, bool upperTriangle) const{
401  // compute the permuted version of the new row/column layout
402  size_t n=_rowBlockIndices.size();
403  // computed the block sizes
404  int blockSizes[_rowBlockIndices.size()];
405  blockSizes[0]=_rowBlockIndices[0];
406  for (size_t i=1; i<n; ++i){
407  blockSizes[i]=_rowBlockIndices[i]-_rowBlockIndices[i-1];
408  }
409  // permute them
410  int pBlockIndices[_rowBlockIndices.size()];
411  for (size_t i=0; i<n; ++i){
412  pBlockIndices[pinv[i]]=blockSizes[i];
413  }
414  for (size_t i=1; i<n; ++i){
415  pBlockIndices[i]+=pBlockIndices[i-1];
416  }
417  // allocate C, or check the structure;
418  if (! dest){
419  dest=new SparseBlockMatrix(pBlockIndices, pBlockIndices, n, n);
420  } else {
421  if (dest->_rowBlockIndices.size()!=n)
422  return false;
423  if (dest->_colBlockIndices.size()!=n)
424  return false;
425  for (size_t i=0; i<n; ++i){
426  if (dest->_rowBlockIndices[i]!=pBlockIndices[i])
427  return false;
428  if (dest->_colBlockIndices[i]!=pBlockIndices[i])
429  return false;
430  }
431  dest->clear();
432  }
433  // now ready to permute the columns
434  for (size_t i=0; i<n; ++i){
435  //cerr << PVAR(i) << " ";
436  int pi=pinv[i];
438  it!=_blockCols[i].end(); ++it){
439  int pj=pinv[it->first];
440 
441  const typename SparseBlockMatrix<MatrixType>::SparseMatrixBlock* s=it->second;
442 
444  if (! upperTriangle || pj<=pi) {
445  b=dest->block(pj,pi,true);
446  assert(b->cols()==s->cols());
447  assert(b->rows()==s->rows());
448  *b=*s;
449  } else {
450  b=dest->block(pi,pj,true);
451  assert(b);
452  assert(b->rows()==s->cols());
453  assert(b->cols()==s->rows());
454  *b=s->transpose();
455  }
456  }
457  //cerr << endl;
458  // within each row,
459  }
460  return true;
461 
462  }
463 
464  template <class MatrixType>
465  int SparseBlockMatrix<MatrixType>::fillCCS(double* Cx, bool upperTriangle) const
466  {
467  assert(Cx && "Target destination is NULL");
468  double* CxStart = Cx;
469  for (size_t i=0; i<_blockCols.size(); ++i){
470  int cstart=i ? _colBlockIndices[i-1] : 0;
471  int csize=colsOfBlock(i);
472  for (int c=0; c<csize; ++c) {
473  for (typename SparseBlockMatrix<MatrixType>::IntBlockMap::const_iterator it=_blockCols[i].begin(); it!=_blockCols[i].end(); ++it){
474  const typename SparseBlockMatrix<MatrixType>::SparseMatrixBlock* b=it->second;
475  int rstart=it->first ? _rowBlockIndices[it->first-1] : 0;
476 
477  int elemsToCopy = b->rows();
478  if (upperTriangle && rstart == cstart)
479  elemsToCopy = c + 1;
480  memcpy(Cx, b->data() + c*b->rows(), elemsToCopy * sizeof(double));
481  Cx += elemsToCopy;
482 
483  }
484  }
485  }
486  return Cx - CxStart;
487  }
488 
489  template <class MatrixType>
490  int SparseBlockMatrix<MatrixType>::fillCCS(int* Cp, int* Ci, double* Cx, bool upperTriangle) const
491  {
492  assert(Cp && Ci && Cx && "Target destination is NULL");
493  int nz=0;
494  for (size_t i=0; i<_blockCols.size(); ++i){
495  int cstart=i ? _colBlockIndices[i-1] : 0;
496  int csize=colsOfBlock(i);
497  for (int c=0; c<csize; ++c) {
498  *Cp=nz;
499  for (typename SparseBlockMatrix<MatrixType>::IntBlockMap::const_iterator it=_blockCols[i].begin(); it!=_blockCols[i].end(); ++it){
500  const typename SparseBlockMatrix<MatrixType>::SparseMatrixBlock* b=it->second;
501  int rstart=it->first ? _rowBlockIndices[it->first-1] : 0;
502 
503  int elemsToCopy = b->rows();
504  if (upperTriangle && rstart == cstart)
505  elemsToCopy = c + 1;
506  for (int r=0; r<elemsToCopy; ++r){
507  *Cx++ = (*b)(r,c);
508  *Ci++ = rstart++;
509  ++nz;
510  }
511  }
512  ++Cp;
513  }
514  }
515  *Cp=nz;
516  return nz;
517  }
518 
519  template <class MatrixType>
521  {
522  int n = _colBlockIndices.size();
523  int nzMax = (int)nonZeroBlocks();
524 
525  ms.alloc(n, nzMax);
526  ms.m = _rowBlockIndices.size();
527 
528  int nz = 0;
529  int* Cp = ms.Ap;
530  int* Ci = ms.Aii;
531  for (int i = 0; i < static_cast<int>(_blockCols.size()); ++i){
532  *Cp = nz;
533  const int& c = i;
534  for (typename SparseBlockMatrix<MatrixType>::IntBlockMap::const_iterator it=_blockCols[i].begin(); it!=_blockCols[i].end(); ++it) {
535  const int& r = it->first;
536  if (r <= c) {
537  *Ci++ = r;
538  ++nz;
539  }
540  }
541  Cp++;
542  }
543  *Cp=nz;
544  assert(nz <= nzMax);
545  }
546 
547  template <class MatrixType>
548  bool SparseBlockMatrix<MatrixType>::writeOctave(const char* filename, bool upperTriangle) const
549  {
550  std::string name = filename;
551  std::string::size_type lastDot = name.find_last_of('.');
552  if (lastDot != std::string::npos)
553  name = name.substr(0, lastDot);
554 
555  std::vector<TripletEntry> entries;
556  for (size_t i = 0; i<_blockCols.size(); ++i){
557  const int& c = i;
558  for (typename SparseBlockMatrix<MatrixType>::IntBlockMap::const_iterator it=_blockCols[i].begin(); it!=_blockCols[i].end(); ++it) {
559  const int& r = it->first;
560  const MatrixType& m = *(it->second);
561  for (int cc = 0; cc < m.cols(); ++cc)
562  for (int rr = 0; rr < m.rows(); ++rr) {
563  int aux_r = rowBaseOfBlock(r) + rr;
564  int aux_c = colBaseOfBlock(c) + cc;
565  entries.push_back(TripletEntry(aux_r, aux_c, m(rr, cc)));
566  if (upperTriangle && r != c) {
567  entries.push_back(TripletEntry(aux_c, aux_r, m(rr, cc)));
568  }
569  }
570  }
571  }
572 
573  int nz = entries.size();
574  std::sort(entries.begin(), entries.end(), TripletColSort());
575 
576  std::ofstream fout(filename);
577  fout << "# name: " << name << std::endl;
578  fout << "# type: sparse matrix" << std::endl;
579  fout << "# nnz: " << nz << std::endl;
580  fout << "# rows: " << rows() << std::endl;
581  fout << "# columns: " << cols() << std::endl;
582  fout << std::setprecision(9) << std::fixed << std::endl;
583 
584  for (std::vector<TripletEntry>::const_iterator it = entries.begin(); it != entries.end(); ++it) {
585  const TripletEntry& entry = *it;
586  fout << entry.r+1 << " " << entry.c+1 << " " << entry.x << std::endl;
587  }
588  return fout.good();
589  }
590 
591  template <class MatrixType>
593  {
594  blockCCS.blockCols().resize(blockCols().size());
595  int numblocks = 0;
596  for (size_t i = 0; i < blockCols().size(); ++i) {
597  const IntBlockMap& row = blockCols()[i];
598  typename SparseBlockMatrixCCS<MatrixType>::SparseColumn& dest = blockCCS.blockCols()[i];
599  dest.clear();
600  dest.reserve(row.size());
601  for (typename IntBlockMap::const_iterator it = row.begin(); it != row.end(); ++it) {
602  dest.push_back(typename SparseBlockMatrixCCS<MatrixType>::RowBlock(it->first, it->second));
603  ++numblocks;
604  }
605  }
606  return numblocks;
607  }
608 
609  template <class MatrixType>
611  {
612  blockCCS.blockCols().clear();
613  blockCCS.blockCols().resize(_rowBlockIndices.size());
614  int numblocks = 0;
615  for (size_t i = 0; i < blockCols().size(); ++i) {
616  const IntBlockMap& row = blockCols()[i];
617  for (typename IntBlockMap::const_iterator it = row.begin(); it != row.end(); ++it) {
618  typename SparseBlockMatrixCCS<MatrixType>::SparseColumn& dest = blockCCS.blockCols()[it->first];
619  dest.push_back(typename SparseBlockMatrixCCS<MatrixType>::RowBlock(i, it->second));
620  ++numblocks;
621  }
622  }
623  return numblocks;
624  }
625 
626  template <class MatrixType>
628  {
629  // sort the sparse columns and add them to the map structures by
630  // exploiting that we are inserting a sorted structure
631  typedef std::pair<int, MatrixType*> SparseColumnPair;
632  typedef typename SparseBlockMatrixHashMap<MatrixType>::SparseColumn HashSparseColumn;
633  for (size_t i = 0; i < hashMatrix.blockCols().size(); ++i) {
634  // prepare a temporary vector for sorting
635  HashSparseColumn& column = hashMatrix.blockCols()[i];
636  if (column.size() == 0)
637  continue;
638  std::vector<SparseColumnPair> sparseRowSorted; // temporary structure
639  sparseRowSorted.reserve(column.size());
640  for (typename HashSparseColumn::const_iterator it = column.begin(); it != column.end(); ++it)
641  sparseRowSorted.push_back(*it);
642  std::sort(sparseRowSorted.begin(), sparseRowSorted.end(), CmpPairFirst<int, MatrixType*>());
643  // try to free some memory early
644  HashSparseColumn aux;
645  swap(aux, column);
646  // now insert sorted vector to the std::map structure
647  IntBlockMap& destColumnMap = blockCols()[i];
648  destColumnMap.insert(sparseRowSorted[0]);
649  for (size_t j = 1; j < sparseRowSorted.size(); ++j) {
650  typename SparseBlockMatrix<MatrixType>::IntBlockMap::iterator hint = destColumnMap.end();
651  --hint; // cppreference says the element goes after the hint (until C++11)
652  destColumnMap.insert(hint, sparseRowSorted[j]);
653  }
654  }
655  }
656 
657 }// end namespace
d
std::vector< int > _rowBlockIndices
vector of the indices of the blocks along the rows.
std::vector< int > _colBlockIndices
int fillSparseBlockMatrixCCS(SparseBlockMatrixCCS< MatrixType > &blockCCS) const
bool transpose(SparseBlockMatrix< MatrixTransposedType > *&dest) const
transposes a block matrix, The transposed type should match the argument false on failure ...
int rowsOfBlock(int r) const
how many rows does the block at block-row r has?
int cols() const
columns of the matrix
int * Ap
column pointers for A, of size n+1
bool writeOctave(const char *filename, bool upperTriangle=true) const
XmlRpcServer s
void axpy(const MatrixType &A, const Eigen::Map< const Eigen::VectorXd > &x, int xoff, Eigen::Map< Eigen::VectorXd > &y, int yoff)
int rows() const
rows of the matrix
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 ...
representing the structure of a matrix in column compressed structure (only the upper triangular part...
size_t nonZeroBlocks() const
number of allocated blocks
int colsOfBlock(int c) const
how many cols does the block at block-col c has?
void atxpy(const MatrixType &A, const Eigen::Map< const Eigen::VectorXd > &x, int xoff, Eigen::Map< Eigen::VectorXd > &y, int yoff)
void clear(bool dealloc=false)
this zeroes all the blocks. If dealloc=true the blocks are removed from memory
MatrixType SparseMatrixBlock
this is the type of the elementary block, it is an Eigen::Matrix.
size_t nonZeros() const
number of non-zero elements
std::vector< RowBlock > SparseColumn
void fillBlockStructure(MatrixStructure &ms) const
exports the non zero blocks in the structure matrix ms
std::vector< IntBlockMap > _blockCols
SparseBlockMatrix * slice(int rmin, int rmax, int cmin, int cmax, bool alloc=true) const
void takePatternFromHash(SparseBlockMatrixHashMap< MatrixType > &hashMatrix)
int m
A is m-by-n. m must be >= 0.
void multiplySymmetricUpperTriangle(double *&dest, const double *src) const
const std::vector< SparseColumn > & blockCols() const
the block matrices per block-column
Sparse matrix which uses blocks.
int fillSparseBlockMatrixCCSTransposed(SparseBlockMatrixCCS< MatrixType > &blockCCS) const
void rightMultiply(double *&dest, const double *src) const
dest = M * (*this)
TFSIMD_FORCE_INLINE const tfScalar & x() const
bool symmPermutation(SparseBlockMatrix< MatrixType > *&dest, const int *pinv, bool onlyUpper=false) const
std::map< int, SparseMatrixBlock * > IntBlockMap
void alloc(int n_, int nz)
int colBaseOfBlock(int c) const
where does the col at block-col r starts?
const std::vector< IntBlockMap > & blockCols() const
the block matrices per block-column
std::tr1::unordered_map< int, MatrixType * > SparseColumn
bool multiply(SparseBlockMatrix< MatrixResultType > *&dest, const SparseBlockMatrix< MatrixFactorType > *M) const
dest = (*this) * M
int * Aii
row indices of A, of size nz = Ap [n]
void scale(double a)
*this *= a
SparseBlockMatrix * clone() const
deep copy of a sparse-block-matrix;
Sparse matrix which uses blocks based on hash structures.
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 starts?
Sparse matrix which uses blocks.
int fillCCS(int *Cp, int *Ci, double *Cx, bool upperTriangle=false) const


orb_slam2_with_maps_odom
Author(s): teng zhang
autogenerated on Fri Sep 25 2020 03:24:47