sparse_block_matrix.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_
28 #define G2O_SPARSE_BLOCK_MATRIX_
29 
30 #include <map>
31 #include <vector>
32 #include <fstream>
33 #include <iostream>
34 #include <iomanip>
35 #include <cassert>
36 #include <Eigen/Core>
37 
39 #include "matrix_structure.h"
40 #include "matrix_operations.h"
41 #include "../../config.h"
42 
43 namespace g2o {
44  using namespace Eigen;
61 template <class MatrixType = MatrixXd >
63 
64  public:
66  typedef MatrixType SparseMatrixBlock;
67 
69  inline int cols() const {return _colBlockIndices.size() ? _colBlockIndices.back() : 0;}
71  inline int rows() const {return _rowBlockIndices.size() ? _rowBlockIndices.back() : 0;}
72 
73  typedef std::map<int, SparseMatrixBlock*> IntBlockMap;
74 
86  SparseBlockMatrix( const int * rbi, const int* cbi, int rb, int cb, bool hasStorage=true);
87 
89 
91 
92 
94  void clear(bool dealloc=false) ;
95 
97  SparseMatrixBlock* block(int r, int c, bool alloc=false);
99  const SparseMatrixBlock* block(int r, int c) const;
100 
102  inline int rowsOfBlock(int r) const { return r ? _rowBlockIndices[r] - _rowBlockIndices[r-1] : _rowBlockIndices[0] ; }
103 
105  inline int colsOfBlock(int c) const { return c ? _colBlockIndices[c] - _colBlockIndices[c-1] : _colBlockIndices[0]; }
106 
108  inline int rowBaseOfBlock(int r) const { return r ? _rowBlockIndices[r-1] : 0 ; }
109 
111  inline int colBaseOfBlock(int c) const { return c ? _colBlockIndices[c-1] : 0 ; }
112 
114  size_t nonZeros() const;
116  size_t nonZeroBlocks() const;
117 
119  SparseBlockMatrix* clone() const ;
120 
129  SparseBlockMatrix* slice(int rmin, int rmax, int cmin, int cmax, bool alloc=true) const;
130 
132  template <class MatrixTransposedType>
133  bool transpose(SparseBlockMatrix<MatrixTransposedType>*& dest) const;
134 
136  bool add(SparseBlockMatrix<MatrixType>*& dest) const ;
137 
139  template <class MatrixResultType, class MatrixFactorType>
140  bool multiply(SparseBlockMatrix<MatrixResultType> *& dest, const SparseBlockMatrix<MatrixFactorType>* M) const;
141 
143  void multiply(double*& dest, const double* src) const;
144 
149  void multiplySymmetricUpperTriangle(double*& dest, const double* src) const;
150 
152  void rightMultiply(double*& dest, const double* src) const;
153 
155  void scale( double a);
156 
161  bool symmPermutation(SparseBlockMatrix<MatrixType>*& dest, const int* pinv, bool onlyUpper=false) const;
162 
166  int fillCCS(int* Cp, int* Ci, double* Cx, bool upperTriangle = false) const;
167 
172  int fillCCS(double* Cx, bool upperTriangle = false) const;
173 
175  void fillBlockStructure(MatrixStructure& ms) const;
176 
178  const std::vector<IntBlockMap>& blockCols() const { return _blockCols;}
179  std::vector<IntBlockMap>& blockCols() { return _blockCols;}
180 
182  const std::vector<int>& rowBlockIndices() const { return _rowBlockIndices;}
183  std::vector<int>& rowBlockIndices() { return _rowBlockIndices;}
184 
186  const std::vector<int>& colBlockIndices() const { return _colBlockIndices;}
187  std::vector<int>& colBlockIndices() { return _colBlockIndices;}
188 
193  bool writeOctave(const char* filename, bool upperTriangle = true) const;
194 
199  int fillSparseBlockMatrixCCS(SparseBlockMatrixCCS<MatrixType>& blockCCS) const;
200 
205  int fillSparseBlockMatrixCCSTransposed(SparseBlockMatrixCCS<MatrixType>& blockCCS) const;
206 
211  void takePatternFromHash(SparseBlockMatrixHashMap<MatrixType>& hashMatrix);
212 
213  protected:
214  std::vector<int> _rowBlockIndices;
215  std::vector<int> _colBlockIndices;
216  std::vector <IntBlockMap> _blockCols;
220 };
221 
222 template < class MatrixType >
223 std::ostream& operator << (std::ostream&, const SparseBlockMatrix<MatrixType>& m);
224 
226 
227 } //end namespace
228 
229 #include "sparse_block_matrix.hpp"
230 
231 #endif
const std::vector< int > & rowBlockIndices() const
indices of the row blocks
std::vector< int > _rowBlockIndices
vector of the indices of the blocks along the rows.
std::vector< int > _colBlockIndices
filename
int rowsOfBlock(int r) const
how many rows does the block at block-row r has?
int cols() const
columns of the matrix
int rows() const
rows of the matrix
representing the structure of a matrix in column compressed structure (only the upper triangular part...
int colsOfBlock(int c) const
how many cols does the block at block-col c has?
bool add(const actionlib::TwoIntsGoal &req, actionlib::TwoIntsResult &res)
MatrixType SparseMatrixBlock
this is the type of the elementary block, it is an Eigen::Matrix.
const std::vector< int > & colBlockIndices() const
indices of the column blocks
std::vector< IntBlockMap > & blockCols()
std::vector< int > & colBlockIndices()
Sparse matrix which uses blocks.
std::vector< int > & rowBlockIndices()
std::map< int, SparseMatrixBlock * > IntBlockMap
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
Sparse matrix which uses blocks based on hash structures.
SparseBlockMatrix< MatrixXd > SparseBlockMatrixXd
int rowBaseOfBlock(int r) const
where does the row at block-row r starts?
Sparse matrix which uses blocks.


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