csparse.h
Go to the documentation of this file.
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2009, Willow Garage, Inc.
5  * All rights reserved.
6  *
7  * Redistribution and use in source and binary forms, with or without
8  * modification, are permitted provided that the following conditions
9  * are met:
10  *
11  * * Redistributions of source code must retain the above copyright
12  * notice, this list of conditions and the following disclaimer.
13  * * Redistributions in binary form must reproduce the above
14  * copyright notice, this list of conditions and the following
15  * disclaimer in the documentation and/or other materials provided
16  * with the distribution.
17  * * Neither the name of the Willow Garage nor the names of its
18  * contributors may be used to endorse or promote products derived
19  * from this software without specific prior written permission.
20  *
21  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32  * POSSIBILITY OF SUCH DAMAGE.
33  *********************************************************************/
34 
35 //
36 // Interface to CSparse
37 //
38 
39 #ifndef _CSPARSE_H_
40 #define _CSPARSE_H_
41 
42 #ifndef EIGEN_USE_NEW_STDVECTOR
43 #define EIGEN_USE_NEW_STDVECTOR
44 #endif // EIGEN_USE_NEW_STDVECTOR
45 
46 #include <stdio.h>
47 #include <iostream>
48 #include <Eigen/Core>
49 #include <Eigen/Geometry>
50 #include <Eigen/LU>
51 #include <Eigen/StdVector>
52 
53 // CSparse header
54 extern "C" {
55 #include "suitesparse/cs.h"
56 }
57 #include <vector>
58 #include <map>
59 
60 // Cholmod header, other header files brought in
61 #ifdef SBA_CHOLMOD
62 #include "suitesparse/cholmod.h"
63 #endif
64 
65 // these are for SparseLib and IML, testing the Delayed State Filter
66 #ifdef SBA_DSIF
67 #include "SparseLib/compcol_double.h" // Compressed column matrix header
68 #include "SparseLib/mvblasd.h" // MV_Vector level 1 BLAS
69 #include "SparseLib/icpre_double.h" // Diagonal preconditioner
70 #include "SparseLib/cg.h" // IML++ CG template
71 #endif
72 
73 // block jacobian PCG
75 
76 using namespace Eigen;
77 using namespace std;
78 
79 namespace sba
80 {
81  class CSparse
82  {
83  public:
84  EIGEN_MAKE_ALIGNED_OPERATOR_NEW // needed for 16B alignment
85 
86  // constructor
87  CSparse();
88 
89  // destructor
90  ~CSparse();
91 
92  // storage of diagonal blocks
93  vector< Matrix<double,6,6>, aligned_allocator<Matrix<double,6,6> > > diag;
94  // compressed column storage of blocks
95  vector< map<int,Matrix<double,6,6>, less<int>,
96  aligned_allocator<Matrix<double,6,6> > > > cols;
97 
98  void setupBlockStructure(int n); // size of rows/cols of A (in blocks)
99 
100  // add in blocks
101  inline void addDiagBlock(Matrix<double,6,6> &m, int n)
102  { diag[n]+=m; };
103  void incDiagBlocks(double lam);
104  void addOffdiagBlock(Matrix<double,6,6> &m, int ii, int jj);
105 
106  // set up compressed column structure; <init> true if first time
107  // <diaginc> is the diagonal multiplier for LM
108  void setupCSstructure(double diaginc, bool init=false);
109 
110  // write cs structure into a dense Eigen matrix
111  void uncompress(MatrixXd &m);
112 
113  // parameters
114  int asize, csize; // matrix A is asize x asize (blocks), csize x csize (elements)
115  int nnz; // number of non-zeros in A
116 
117  // CSparse structures
118  cs *A; // linear problem matrix
119 
120  // RHS Eigen vector
121  VectorXd B;
122 
123  // which algorithm?
125 
126  // doing the Cholesky with CSparse or Cholmod
127  bool doChol(); // solve in place with RHS B
128 
129  // doing the BPCG
130  // max iterations <iter>, ending toleranace <tol>, current sba iteration <sba_iter>
131  int doBPCG(int iters, double tol, int sba_iter);
132  // CG structure for 6x6 matrices
134 
135 #ifdef SBA_CHOLMOD
136  // CHOLMOD structures
137  bool chInited;
138  cholmod_sparse *chA; // linear problem matrix
139  cholmod_common *chc;
140  cholmod_common Common;
141 #endif
142 
143  };
144 
145 
146  class CSparse2d
147  {
148  public:
149  EIGEN_MAKE_ALIGNED_OPERATOR_NEW // needed for 16B alignment
150 
151  // constructor
152  CSparse2d();
153 
154  // destructor
155  ~CSparse2d();
156 
157  // storage of diagonal blocks
158  vector< Matrix<double,3,3>, aligned_allocator<Matrix<double,3,3> > > diag;
159  // compressed column storage of blocks
160  vector< map<int,Matrix<double,3,3>, less<int>,
161  aligned_allocator<Matrix<double,3,3> > > > cols;
162 
163  void setupBlockStructure(int n, bool eraseit = true); // size of rows/cols of A (in blocks)
164 
165  // add in blocks
166  inline void addDiagBlock(Matrix<double,3,3> &m, int n)
167  { diag[n]+=m; };
168  void addOffdiagBlock(Matrix<double,3,3> &m, int ii, int jj);
169  void incDiagBlocks(double lam);
170 
171  // set up compressed column structure; <init> true if first time
172  // <diaginc> is the diagonal multiplier for LM
173  void setupCSstructure(double diaginc, bool init=false);
174 
175  // write cs structure into a dense Eigen matrix
176  void uncompress(MatrixXd &m);
177 
178  // parameters
179  int asize, csize; // matrix A is asize x asize (blocks), csize x csize (elements)
180  int nnz; // number of non-zeros in A
181 
182  // CSparse2d structures
183  cs *A, *AF; // linear problem matrices, A upper diagonal, AF symmetric
184 
185  // RHS Eigen vector
186  VectorXd B, Bprev;
187 
188  // which algorithm?
190 
191  // doing the Cholesky
192  bool doChol(); // solve in place with RHS B
193 
194  // doing PCG with incomplete Cholesky preconditioner
195  // returns 0 on success, 1 on not achieving tolerance, >1 on other errors
196  int doPCG(int iters);
197 
198  // doing the BPCG
199  // max iterations <iter>, ending toleranace <tol>
200  int doBPCG(int iters, double tol, int sba_iter);
201  // CG structure for 3x3 matrices
203 
204 #ifdef SBA_CHOLMOD
205  // CHOLMOD structures
206  bool chInited;
207  cholmod_sparse *chA; // linear problem matrix
208  cholmod_common *chc;
209  cholmod_common Common;
210 #endif
211 
212  };
213 
214 
215 } // end namespace sba
216 
217 #endif // _CSPARSE_H
sba::CSparse
Definition: csparse.h:81
sba::CSparse::B
VectorXd B
Definition: csparse.h:121
sba::CSparse2d::csize
int csize
Definition: csparse.h:179
sba::CSparse2d
Definition: csparse.h:146
Eigen
Definition: sba_file_io.h:18
bpcg.h
sba::CSparse2d::AF
cs * AF
Definition: csparse.h:183
sba::CSparse::A
cs * A
Definition: csparse.h:118
sba::CSparse::csize
int csize
Definition: csparse.h:114
sba::CSparse::addDiagBlock
void addDiagBlock(Matrix< double, 6, 6 > &m, int n)
Definition: csparse.h:101
sba::CSparse2d::useCholmod
bool useCholmod
Definition: csparse.h:189
sba::CSparse2d::nnz
int nnz
Definition: csparse.h:180
sba
Definition: bpcg.h:60
sba::CSparse::cols
vector< map< int, Matrix< double, 6, 6 >, less< int >, aligned_allocator< Matrix< double, 6, 6 > > > > cols
Definition: csparse.h:96
sba::CSparse::useCholmod
bool useCholmod
Definition: csparse.h:124
sba::CSparse::diag
vector< Matrix< double, 6, 6 >, aligned_allocator< Matrix< double, 6, 6 > > > diag
Definition: csparse.h:93
sba::CSparse2d::cols
vector< map< int, Matrix< double, 3, 3 >, less< int >, aligned_allocator< Matrix< double, 3, 3 > > > > cols
Definition: csparse.h:161
sba::CSparse2d::diag
vector< Matrix< double, 3, 3 >, aligned_allocator< Matrix< double, 3, 3 > > > diag
Definition: csparse.h:158
std
sba::CSparse2d::addDiagBlock
void addDiagBlock(Matrix< double, 3, 3 > &m, int n)
Definition: csparse.h:166
sba::CSparse::nnz
int nnz
Definition: csparse.h:115
sba::CSparse2d::Bprev
VectorXd Bprev
Definition: csparse.h:186
sba::CSparse::bpcg
jacobiBPCG< 6 > bpcg
Definition: csparse.h:133
sba::jacobiBPCG< 6 >
sba::CSparse2d::bpcg
jacobiBPCG< 3 > bpcg
Definition: csparse.h:202


sparse_bundle_adjustment
Author(s):
autogenerated on Wed Mar 2 2022 01:03:04