g2o
sparse_block_matrix_diagonal.h
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 #ifndef G2O_SPARSE_BLOCK_MATRIX_DIAGONAL_H
28 #define G2O_SPARSE_BLOCK_MATRIX_DIAGONAL_H
29 
30 #include <vector>
31 #include <Eigen/Core>
32 #include <Eigen/StdVector>
33 
34 #include "g2o/config.h"
35 #include "matrix_operations.h"
36 
37 namespace g2o {
38 
46  template <class MatrixType>
48  {
49  public:
51  typedef MatrixType SparseMatrixBlock;
52 
54  int cols() const {return _blockIndices.size() ? _blockIndices.back() : 0;}
56  int rows() const {return _blockIndices.size() ? _blockIndices.back() : 0;}
57 
58  typedef std::vector<MatrixType, Eigen::aligned_allocator<MatrixType> > DiagonalVector;
59 
60  SparseBlockMatrixDiagonal(const std::vector<int>& blockIndices) :
61  _blockIndices(blockIndices)
62  {}
63 
65  inline int dimOfBlock(int r) const { return r ? _blockIndices[r] - _blockIndices[r-1] : _blockIndices[0] ; }
66 
68  inline int baseOfBlock(int r) const { return r ? _blockIndices[r-1] : 0 ; }
69 
71  const DiagonalVector& diagonal() const { return _diagonal;}
72  DiagonalVector& diagonal() { return _diagonal;}
73 
75  const std::vector<int>& blockIndices() const { return _blockIndices;}
76 
77  void multiply(double*& dest, const double* src) const
78  {
79  int destSize=cols();
80  if (! dest) {
81  dest=new double[destSize];
82  memset(dest,0, destSize*sizeof(double));
83  }
84 
85  // map the memory by Eigen
86  Eigen::Map<Eigen::VectorXd> destVec(dest, destSize);
87  Eigen::Map<const Eigen::VectorXd> srcVec(src, rows());
88 
89 # ifdef G2O_OPENMP
90 # pragma omp parallel for default (shared) schedule(dynamic, 10)
91 # endif
92  for (int i=0; i < static_cast<int>(_diagonal.size()); ++i){
93  int destOffset = baseOfBlock(i);
94  int srcOffset = destOffset;
95  const SparseMatrixBlock& A = _diagonal[i];
96  // destVec += *A.transpose() * srcVec (according to the sub-vector parts)
97  internal::template axpy<SparseMatrixBlock>(A, srcVec, srcOffset, destVec, destOffset);
98  }
99  }
100 
101  protected:
102  const std::vector<int>& _blockIndices;
103  DiagonalVector _diagonal;
104  };
105 
106 } //end namespace
107 
108 #endif
const std::vector< int > & blockIndices() const
indices of the row blocks
int baseOfBlock(int r) const
where does the row /col at block-row / block-column r starts?
const DiagonalVector & diagonal() const
the block matrices per block-column
int cols() const
columns of the matrix
void multiply(double *&dest, const double *src) const
int rows() const
rows of the matrix
SparseBlockMatrixDiagonal(const std::vector< int > &blockIndices)
MatrixType SparseMatrixBlock
this is the type of the elementary block, it is an Eigen::Matrix.
int dimOfBlock(int r) const
how many rows/cols does the block at block-row / block-column r has?
const std::vector< int > & _blockIndices
vector of the indices of the blocks along the diagonal
std::vector< MatrixType, Eigen::aligned_allocator< MatrixType > > DiagonalVector
Sparse matrix which uses blocks on the diagonal.