csparse.h
Go to the documentation of this file.00001 /********************************************************************* 00002 * Software License Agreement (BSD License) 00003 * 00004 * Copyright (c) 2009, Willow Garage, Inc. 00005 * All rights reserved. 00006 * 00007 * Redistribution and use in source and binary forms, with or without 00008 * modification, are permitted provided that the following conditions 00009 * are met: 00010 * 00011 * * Redistributions of source code must retain the above copyright 00012 * notice, this list of conditions and the following disclaimer. 00013 * * Redistributions in binary form must reproduce the above 00014 * copyright notice, this list of conditions and the following 00015 * disclaimer in the documentation and/or other materials provided 00016 * with the distribution. 00017 * * Neither the name of the Willow Garage nor the names of its 00018 * contributors may be used to endorse or promote products derived 00019 * from this software without specific prior written permission. 00020 * 00021 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 00022 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 00023 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 00024 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 00025 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 00026 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 00027 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 00028 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 00029 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 00030 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 00031 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 00032 * POSSIBILITY OF SUCH DAMAGE. 00033 *********************************************************************/ 00034 00035 // 00036 // Interface to CSparse 00037 // 00038 00039 #ifndef _CSPARSE_H_ 00040 #define _CSPARSE_H_ 00041 00042 #ifndef EIGEN_USE_NEW_STDVECTOR 00043 #define EIGEN_USE_NEW_STDVECTOR 00044 #endif // EIGEN_USE_NEW_STDVECTOR 00045 00046 #include <stdio.h> 00047 #include <iostream> 00048 #include <Eigen/Core> 00049 #include <Eigen/Geometry> 00050 #include <Eigen/LU> 00051 #include <Eigen/StdVector> 00052 00053 // CSparse header 00054 extern "C" { 00055 #include "CSparse/cs.h" 00056 } 00057 #include <vector> 00058 #include <map> 00059 00060 // Cholmod header, other header files brought in 00061 #ifdef SBA_CHOLMOD 00062 #include "SuiteSparse/cholmod.h" 00063 #endif 00064 00065 // these are for SparseLib and IML, testing the Delayed State Filter 00066 #ifdef SBA_DSIF 00067 #include "SparseLib/compcol_double.h" // Compressed column matrix header 00068 #include "SparseLib/mvblasd.h" // MV_Vector level 1 BLAS 00069 #include "SparseLib/icpre_double.h" // Diagonal preconditioner 00070 #include "SparseLib/cg.h" // IML++ CG template 00071 #endif 00072 00073 // block jacobian PCG 00074 #include "bpcg/bpcg.h" 00075 00076 using namespace Eigen; 00077 using namespace std; 00078 00079 namespace sba 00080 { 00081 class CSparse 00082 { 00083 public: 00084 EIGEN_MAKE_ALIGNED_OPERATOR_NEW // needed for 16B alignment 00085 00086 // constructor 00087 CSparse(); 00088 00089 // destructor 00090 ~CSparse(); 00091 00092 // storage of diagonal blocks 00093 vector< Matrix<double,6,6>, aligned_allocator<Matrix<double,6,6> > > diag; 00094 // compressed column storage of blocks 00095 vector< map<int,Matrix<double,6,6>, less<int>, 00096 aligned_allocator<Matrix<double,6,6> > > > cols; 00097 00098 void setupBlockStructure(int n); // size of rows/cols of A (in blocks) 00099 00100 // add in blocks 00101 inline void addDiagBlock(Matrix<double,6,6> &m, int n) 00102 { diag[n]+=m; }; 00103 void incDiagBlocks(double lam); 00104 void addOffdiagBlock(Matrix<double,6,6> &m, int ii, int jj); 00105 00106 // set up compressed column structure; <init> true if first time 00107 // <diaginc> is the diagonal multiplier for LM 00108 void setupCSstructure(double diaginc, bool init=false); 00109 00110 // write cs structure into a dense Eigen matrix 00111 void uncompress(MatrixXd &m); 00112 00113 // parameters 00114 int asize, csize; // matrix A is asize x asize (blocks), csize x csize (elements) 00115 int nnz; // number of non-zeros in A 00116 00117 // CSparse structures 00118 cs *A; // linear problem matrix 00119 00120 // RHS Eigen vector 00121 VectorXd B; 00122 00123 // which algorithm? 00124 bool useCholmod; 00125 00126 // doing the Cholesky with CSparse or Cholmod 00127 bool doChol(); // solve in place with RHS B 00128 00129 // doing the BPCG 00130 // max iterations <iter>, ending toleranace <tol>, current sba iteration <sba_iter> 00131 int doBPCG(int iters, double tol, int sba_iter); 00132 // CG structure for 6x6 matrices 00133 jacobiBPCG<6> bpcg; 00134 00135 #ifdef SBA_CHOLMOD 00136 // CHOLMOD structures 00137 bool chInited; 00138 cholmod_sparse *chA; // linear problem matrix 00139 cholmod_common *chc; 00140 cholmod_common Common; 00141 #endif 00142 00143 }; 00144 00145 00146 class CSparse2d 00147 { 00148 public: 00149 EIGEN_MAKE_ALIGNED_OPERATOR_NEW // needed for 16B alignment 00150 00151 // constructor 00152 CSparse2d(); 00153 00154 // destructor 00155 ~CSparse2d(); 00156 00157 // storage of diagonal blocks 00158 vector< Matrix<double,3,3>, aligned_allocator<Matrix<double,3,3> > > diag; 00159 // compressed column storage of blocks 00160 vector< map<int,Matrix<double,3,3>, less<int>, 00161 aligned_allocator<Matrix<double,3,3> > > > cols; 00162 00163 void setupBlockStructure(int n, bool eraseit = true); // size of rows/cols of A (in blocks) 00164 00165 // add in blocks 00166 inline void addDiagBlock(Matrix<double,3,3> &m, int n) 00167 { diag[n]+=m; }; 00168 void addOffdiagBlock(Matrix<double,3,3> &m, int ii, int jj); 00169 void incDiagBlocks(double lam); 00170 00171 // set up compressed column structure; <init> true if first time 00172 // <diaginc> is the diagonal multiplier for LM 00173 void setupCSstructure(double diaginc, bool init=false); 00174 00175 // write cs structure into a dense Eigen matrix 00176 void uncompress(MatrixXd &m); 00177 00178 // parameters 00179 int asize, csize; // matrix A is asize x asize (blocks), csize x csize (elements) 00180 int nnz; // number of non-zeros in A 00181 00182 // CSparse2d structures 00183 cs *A, *AF; // linear problem matrices, A upper diagonal, AF symmetric 00184 00185 // RHS Eigen vector 00186 VectorXd B, Bprev; 00187 00188 // which algorithm? 00189 bool useCholmod; 00190 00191 // doing the Cholesky 00192 bool doChol(); // solve in place with RHS B 00193 00194 // doing PCG with incomplete Cholesky preconditioner 00195 // returns 0 on success, 1 on not achieving tolerance, >1 on other errors 00196 int doPCG(int iters); 00197 00198 // doing the BPCG 00199 // max iterations <iter>, ending toleranace <tol> 00200 int doBPCG(int iters, double tol, int sba_iter); 00201 // CG structure for 3x3 matrices 00202 jacobiBPCG<3> bpcg; 00203 00204 #ifdef SBA_CHOLMOD 00205 // CHOLMOD structures 00206 bool chInited; 00207 cholmod_sparse *chA; // linear problem matrix 00208 cholmod_common *chc; 00209 cholmod_common Common; 00210 #endif 00211 00212 }; 00213 00214 00215 } // end namespace sba 00216 00217 #endif // _CSPARSE_H