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 "cs.h" 00056 } 00057 #include <vector> 00058 #include <map> 00059 00060 // Cholmod header, other header files brought in 00061 #ifdef SBA_CHOLMOD 00062 #include "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 "SPA/bpcg.h" 00075 00076 using namespace Eigen; 00077 using namespace std; 00078 00079 00080 class CSparse2d 00081 { 00082 public: 00083 EIGEN_MAKE_ALIGNED_OPERATOR_NEW // needed for 16B alignment 00084 00085 // constructor 00086 CSparse2d(); 00087 00088 // destructor 00089 ~CSparse2d(); 00090 00091 // storage of diagonal blocks 00092 vector< Matrix<double,3,3>, aligned_allocator<Matrix<double,3,3> > > diag; 00093 // compressed column storage of blocks 00094 vector< map<int,Matrix<double,3,3>, less<int>, 00095 aligned_allocator<Matrix<double,3,3> > > > cols; 00096 00097 void setupBlockStructure(int n, bool eraseit = true); // size of rows/cols of A (in blocks) 00098 00099 // add in blocks 00100 inline void addDiagBlock(Matrix<double,3,3> &m, int n) 00101 { diag[n]+=m; }; 00102 void addOffdiagBlock(Matrix<double,3,3> &m, int ii, int jj); 00103 void incDiagBlocks(double lam); 00104 00105 // set up compressed column structure; <init> true if first time 00106 // <diaginc> is the diagonal multiplier for LM 00107 void setupCSstructure(double diaginc, bool init=false); 00108 00109 // write cs structure into a dense Eigen matrix 00110 void uncompress(MatrixXd &m); 00111 00112 // parameters 00113 int asize, csize; // matrix A is asize x asize (blocks), csize x csize (elements) 00114 int nnz; // number of non-zeros in A 00115 00116 // CSparse2d structures 00117 cs *A, *AF; // linear problem matrices, A upper diagonal, AF symmetric 00118 00119 // RHS Eigen vector 00120 VectorXd B, Bprev; 00121 00122 // which algorithm? 00123 bool useCholmod; 00124 00125 // doing the Cholesky 00126 bool doChol(); // solve in place with RHS B 00127 00128 // doing PCG with incomplete Cholesky preconditioner 00129 // returns 0 on success, 1 on not achieving tolerance, >1 on other errors 00130 int doPCG(int iters); 00131 00132 // doing the BPCG 00133 // max iterations <iter>, ending toleranace <tol> 00134 // int doBPCG(int iters, double tol, int sba_iter); 00135 // CG structure for 3x3 matrices 00136 // jacobiBPCG<3> bpcg; 00137 00138 #ifdef SBA_CHOLMOD 00139 // CHOLMOD structures 00140 bool chInited; 00141 cholmod_sparse *chA; // linear problem matrix 00142 cholmod_common *chc; 00143 cholmod_common Common; 00144 #endif 00145 00146 }; 00147 00148 #endif // _CSPARSE_H