g2o
sparse_block_matrix_test.cpp
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 #include "sparse_block_matrix.h"
28 #include <iostream>
29 
30 using namespace std;
31 using namespace g2o;
32 using namespace Eigen;
33 
36 
37 std::ostream& operator << (std::ostream& os, const SparseBlockMatrixX::SparseMatrixBlock& m) {
38  for (int i=0; i<m.rows(); ++i){
39  for (int j=0; j<m.cols(); ++j)
40  cerr << m(i,j) << " ";
41  cerr << endl;
42  }
43  return os;
44 }
45 
46 int main (int argc, char** argv){
47  int rcol[] = {3,6,8,12};
48  int ccol[] = {2,4,13};
49  cerr << "creation" << endl;
50  SparseBlockMatrixX* M=new SparseBlockMatrixX(rcol, ccol, 4,3);
51 
52  cerr << "block access" << endl;
53 
55  cerr << b->rows() << " " << b->cols() << endl;
56  for (int i=0; i<b->rows(); ++i)
57  for (int j=0; j<b->cols(); ++j){
58  (*b)(i,j)=i*b->cols()+j;
59  }
60 
61 
62  cerr << "block access 2" << endl;
63  b=M->block(0,2, true);
64  cerr << b->rows() << " " << b->cols() << endl;
65  for (int i=0; i<b->rows(); ++i)
66  for (int j=0; j<b->cols(); ++j){
67  (*b)(i,j)=i*b->cols()+j;
68  }
69 
70  b=M->block(3,2, true);
71  cerr << b->rows() << " " << b->cols() << endl;
72  for (int i=0; i<b->rows(); ++i)
73  for (int j=0; j<b->cols(); ++j){
74  (*b)(i,j)=i*b->cols()+j;
75  }
76 
77  cerr << *M << endl;
78 
79  cerr << "SUM" << endl;
80 
81  SparseBlockMatrixX* Ms=0;
82  M->add(Ms);
83  M->add(Ms);
84  cerr << *Ms;
85 
86  SparseBlockMatrixX* Mt=0;
87  M->transpose(Mt);
88  cerr << *Mt << endl;
89 
90  SparseBlockMatrixX* Mp=0;
91  M->multiply(Mp, Mt);
92  cerr << *Mp << endl;
93 
94  int iperm[]={3,2,1,0};
95  SparseBlockMatrixX* PMp=0;
96 
97  Mp->symmPermutation(PMp,iperm, false);
98  cerr << *PMp << endl;
99 
100  PMp->clear(true);
101  Mp->block(3,0)->fill(0.);
102  Mp->symmPermutation(PMp,iperm, true);
103  cerr << *PMp << endl;
104 
105 
106 
107 }
bool transpose(SparseBlockMatrix< MatrixTransposedType > *&dest) const
transposes a block matrix, The transposed type should match the argument false on failure ...
std::ostream & operator<<(std::ostream &os, const SparseBlockMatrixX::SparseMatrixBlock &m)
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 ...
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.
SparseBlockMatrix< MatrixXd > SparseBlockMatrixX
bool symmPermutation(SparseBlockMatrix< MatrixType > *&dest, const int *pinv, bool onlyUpper=false) const
bool multiply(SparseBlockMatrix< MatrixResultType > *&dest, const SparseBlockMatrix< MatrixFactorType > *M) const
dest = (*this) * M
bool add(SparseBlockMatrix< MatrixType > *&dest) const
adds the current matrix to the destination
int main(int argc, char **argv)
Sparse matrix which uses blocks.