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 "cs.h"
56 }
57 #include <vector>
58 #include <map>
59 
60 // Cholmod header, other header files brought in
61 #ifdef SBA_CHOLMOD
62 #include "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
74 //#include "SPA/bpcg.h"
75 
76 using namespace Eigen;
77 using namespace std;
78 
79 
80  class CSparse2d
81  {
82  public:
83  EIGEN_MAKE_ALIGNED_OPERATOR_NEW // needed for 16B alignment
84 
85  // constructor
86  CSparse2d();
87 
88  // destructor
89  ~CSparse2d();
90 
91  // storage of diagonal blocks
92  vector< Matrix<double,3,3>, aligned_allocator<Matrix<double,3,3> > > diag;
93  // compressed column storage of blocks
94  vector< map<int,Matrix<double,3,3>, less<int>,
95  aligned_allocator<Matrix<double,3,3> > > > cols;
96 
97  void setupBlockStructure(int n, bool eraseit = true); // size of rows/cols of A (in blocks)
98 
99  // add in blocks
100  inline void addDiagBlock(Matrix<double,3,3> &m, int n)
101  { diag[n]+=m; };
102  void addOffdiagBlock(Matrix<double,3,3> &m, int ii, int jj);
103  void incDiagBlocks(double lam);
104 
105  // set up compressed column structure; <init> true if first time
106  // <diaginc> is the diagonal multiplier for LM
107  void setupCSstructure(double diaginc, bool init=false);
108 
109  // write cs structure into a dense Eigen matrix
110  void uncompress(MatrixXd &m);
111 
112  // parameters
113  int asize, csize; // matrix A is asize x asize (blocks), csize x csize (elements)
114  int nnz; // number of non-zeros in A
115 
116  // CSparse2d structures
117  cs *A, *AF; // linear problem matrices, A upper diagonal, AF symmetric
118 
119  // RHS Eigen vector
120  VectorXd B, Bprev;
121 
122  // which algorithm?
124 
125  // doing the Cholesky
126  bool doChol(); // solve in place with RHS B
127 
128  // doing PCG with incomplete Cholesky preconditioner
129  // returns 0 on success, 1 on not achieving tolerance, >1 on other errors
130  int doPCG(int iters);
131 
132  // doing the BPCG
133  // max iterations <iter>, ending toleranace <tol>
134  // int doBPCG(int iters, double tol, int sba_iter);
135  // CG structure for 3x3 matrices
136  // jacobiBPCG<3> bpcg;
137 
138 #ifdef SBA_CHOLMOD
139  // CHOLMOD structures
140  bool chInited;
141  cholmod_sparse *chA; // linear problem matrix
142  cholmod_common *chc;
143  cholmod_common Common;
144 #endif
145 
146  };
147 
148 #endif // _CSPARSE_H
int csize
Definition: csparse.h:113
vector< Matrix< double, 3, 3 >, aligned_allocator< Matrix< double, 3, 3 > > > diag
Definition: csparse.h:92
bool useCholmod
Definition: csparse.h:123
vector< map< int, Matrix< double, 3, 3 >, less< int >, aligned_allocator< Matrix< double, 3, 3 > > > > cols
Definition: csparse.h:95
void addDiagBlock(Matrix< double, 3, 3 > &m, int n)
Definition: csparse.h:100
cs * AF
Definition: csparse.h:117
VectorXd Bprev
Definition: csparse.h:120
int nnz
Definition: csparse.h:114


nav2d_karto
Author(s): Sebastian Kasperski
autogenerated on Thu Jun 6 2019 19:20:24