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 "suitesparse/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 "sparse_bundle_adjustment/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


sparse_bundle_adjustment
Author(s):
autogenerated on Wed Aug 24 2016 03:37:37