00001 /* +---------------------------------------------------------------------------+ 00002 | The Mobile Robot Programming Toolkit (MRPT) C++ library | 00003 | | 00004 | http://mrpt.sourceforge.net/ | 00005 | | 00006 | Copyright (C) 2005-2011 University of Malaga | 00007 | | 00008 | This software was written by the Machine Perception and Intelligent | 00009 | Robotics Lab, University of Malaga (Spain). | 00010 | Contact: Jose-Luis Blanco <jlblanco@ctima.uma.es> | 00011 | | 00012 | This file is part of the MRPT project. | 00013 | | 00014 | MRPT is free software: you can redistribute it and/or modify | 00015 | it under the terms of the GNU General Public License as published by | 00016 | the Free Software Foundation, either version 3 of the License, or | 00017 | (at your option) any later version. | 00018 | | 00019 | MRPT is distributed in the hope that it will be useful, | 00020 | but WITHOUT ANY WARRANTY; without even the implied warranty of | 00021 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the | 00022 | GNU General Public License for more details. | 00023 | | 00024 | You should have received a copy of the GNU General Public License | 00025 | along with MRPT. If not, see <http://www.gnu.org/licenses/>. | 00026 | | 00027 +---------------------------------------------------------------------------+ */ 00028 #ifndef CSparseMatrix_H 00029 #define CSparseMatrix_H 00030 00031 #include <mrpt/utils/utils_defs.h> 00032 #include <mrpt/utils/CUncopiable.h> 00033 00034 #include <mrpt/math/math_frwds.h> 00035 #include <mrpt/math/CSparseMatrixTemplate.h> 00036 00037 extern "C"{ 00038 #include <mrpt/otherlibs/CSparse/cs.h> 00039 } 00040 00041 namespace mrpt 00042 { 00043 namespace math 00044 { 00045 /** Used in mrpt::math::CSparseMatrix */ 00046 class CExceptionNotDefPos : public mrpt::utils::CMRPTException 00047 { 00048 public: 00049 CExceptionNotDefPos(const std::string &s) : CMRPTException(s) { } 00050 }; 00051 00052 00053 /** A sparse matrix capable of efficient math operations (a wrapper around the CSparse library) 00054 * The type of cells is fixed to "double". 00055 * 00056 * There are two main formats for the non-zero entries in this matrix: 00057 * - A "triplet" matrix: a set of (r,c)=val triplet entries. 00058 * - A column-compressed sparse matrix. 00059 * 00060 * The latter is the "normal" format, which is expected by most mathematical operations defined 00061 * in this class. There're two three ways of initializing and populating a sparse matrix: 00062 * 00063 * <ol> 00064 * <li> <b>As a triplet (empty), then add entries, then compress:</b> 00065 * \code 00066 * CSparseMatrix SM(100,100); 00067 * SM.insert_entry(i,j, val); // or 00068 * SM.insert_submatrix(i,j, MAT); // ... 00069 * SM.compressFromTriplet(); 00070 * \endcode 00071 * </li> 00072 * <li> <b>As a triplet from a CSparseMatrixTemplate<double>:</b> 00073 * \code 00074 * CSparseMatrixTemplate<double> data; 00075 * data(row,col) = val; 00076 * ... 00077 * CSparseMatrix SM(data); 00078 * \endcode 00079 * </li> 00080 * <li> <b>From an existing dense matrix:</b> 00081 * \code 00082 * CMatrixDouble data(100,100); // or 00083 * CMatrixFloat data(100,100); // or 00084 * CMatrixFixedNumeric<double,4,6> data; // etc... 00085 * CSparseMatrix SM(data); 00086 * \endcode 00087 * </li> 00088 * 00089 * </ol> 00090 * 00091 * Due to its practical utility, there is a special inner class CSparseMatrix::CholeskyDecomp to handle Cholesky-related methods and data. 00092 * 00093 * 00094 * \note This class was initially adapted from "robotvision", by Hauke Strasdat, Steven Lovegrove and Andrew J. Davison. See http://www.openslam.org/robotvision.html 00095 * \note CSparse is maintained by Timothy Davis: http://people.sc.fsu.edu/~jburkardt/c_src/csparse/csparse.html . 00096 * \note See also his book "Direct methods for sparse linear systems". http://books.google.es/books?id=TvwiyF8vy3EC&pg=PA12&lpg=PA12&dq=cs_compress&source=bl&ots=od9uGJ793j&sig=Wa-fBk4sZkZv3Y0Op8FNH8PvCUs&hl=es&ei=UjA0TJf-EoSmsQay3aXPAw&sa=X&oi=book_result&ct=result&resnum=8&ved=0CEQQ6AEwBw#v=onepage&q&f=false 00097 * 00098 * \sa mrpt::math::CMatrixFixedNumeric, mrpt::math::CMatrixTemplateNumeric, etc. 00099 */ 00100 class BASE_IMPEXP CSparseMatrix 00101 { 00102 private: 00103 cs sparse_matrix; 00104 00105 /** Initialization from a dense matrix of any kind existing in MRPT. */ 00106 template <class MATRIX> 00107 void construct_from_mrpt_mat(const MATRIX & C) 00108 { 00109 std::vector<int> row_list, col_list; // Use "int" for convenience so we can do a memcpy below... 00110 std::vector<double> content_list; 00111 const int nCol = C.getColCount(); 00112 const int nRow = C.getRowCount(); 00113 for (int c=0; c<nCol; ++c) 00114 { 00115 col_list.push_back(row_list.size()); 00116 for (int r=0; r<nRow; ++r) 00117 if (C.get_unsafe(r,c)!=0) 00118 { 00119 row_list.push_back(r); 00120 content_list.push_back(C(r,c)); 00121 } 00122 } 00123 col_list.push_back(row_list.size()); 00124 00125 sparse_matrix.m = nRow; 00126 sparse_matrix.n = nCol; 00127 sparse_matrix.nzmax = content_list.size(); 00128 sparse_matrix.i = (int*)malloc(sizeof(int)*row_list.size()); 00129 sparse_matrix.p = (int*)malloc(sizeof(int)*col_list.size()); 00130 sparse_matrix.x = (double*)malloc(sizeof(double)*content_list.size()); 00131 00132 ::memcpy(sparse_matrix.i, &row_list[0], sizeof(row_list[0])*row_list.size() ); 00133 ::memcpy(sparse_matrix.p, &col_list[0], sizeof(col_list[0])*col_list.size() ); 00134 ::memcpy(sparse_matrix.x, &content_list[0], sizeof(content_list[0])*content_list.size() ); 00135 00136 sparse_matrix.nz = -1; // <0 means "column compressed", ">=0" means triplet. 00137 } 00138 00139 /** Initialization from a triplet "cs", which is first compressed */ 00140 void construct_from_triplet(const cs & triplet); 00141 00142 /** To be called by constructors only, assume previous pointers are trash and overwrite them */ 00143 void construct_from_existing_cs(const cs &sm); 00144 00145 /** free buffers (deallocate the memory of the i,p,x buffers) */ 00146 void internal_free_mem(); 00147 00148 /** Copy the data from an existing "cs" CSparse data structure */ 00149 void copy(const cs * const sm); 00150 00151 /** Fast copy the data from an existing "cs" CSparse data structure, copying the pointers and leaving NULLs in the source structure. */ 00152 void copy_fast(cs * const sm); 00153 00154 public: 00155 00156 /** @name Constructors, destructor & copy operations 00157 @{ */ 00158 00159 /** Create an initially empty sparse matrix, in the "triplet" form. 00160 * Notice that you must call "compressFromTriplet" after populating the matrix and before using the math operatons on this matrix. 00161 * The initial size can be later on extended with insert_entry() or setRowCount() & setColCount(). 00162 * \sa insert_entry, setRowCount, setColCount 00163 */ 00164 CSparseMatrix(const size_t nRows=0, const size_t nCols=0); 00165 00166 /** A good way to initialize a sparse matrix from a list of non NULL elements. 00167 * This constructor takes all the non-zero entries in "data" and builds a column-compressed sparse representation. 00168 */ 00169 template <typename T> 00170 inline CSparseMatrix(const CSparseMatrixTemplate<T> & data) 00171 { 00172 ASSERTMSG_(!data.empty(), "Input data must contain at least one non-zero element.") 00173 sparse_matrix.i = NULL; // This is to know they shouldn't be tried to free() 00174 sparse_matrix.p = NULL; 00175 sparse_matrix.x = NULL; 00176 00177 // 1) Create triplet matrix 00178 CSparseMatrix triplet(data.getRowCount(),data.getColCount()); 00179 // 2) Put data in: 00180 for (typename CSparseMatrixTemplate<T>::const_iterator it=data.begin();it!=data.end();++it) 00181 triplet.insert_entry_fast(it->first.first,it->first.second, it->second); 00182 00183 // 3) Compress: 00184 construct_from_triplet(triplet.sparse_matrix); 00185 } 00186 00187 00188 // We can't do a simple "template <class ANYMATRIX>" since it would be tried to match against "cs*"... 00189 00190 /** Constructor from a dense matrix of any kind existing in MRPT, creating a "column-compressed" sparse matrix. */ 00191 template <typename T,size_t N,size_t M> inline explicit CSparseMatrix(const CMatrixFixedNumeric<T,N,M> &MAT) { construct_from_mrpt_mat(MAT); } 00192 00193 /** Constructor from a dense matrix of any kind existing in MRPT, creating a "column-compressed" sparse matrix. */ 00194 template <typename T> inline explicit CSparseMatrix(const CMatrixTemplateNumeric<T> &MAT) { construct_from_mrpt_mat(MAT); } 00195 00196 /** Copy constructor */ 00197 CSparseMatrix(const CSparseMatrix & other); 00198 00199 /** Copy constructor from an existing "cs" CSparse data structure */ 00200 explicit CSparseMatrix(const cs * const sm); 00201 00202 /** Destructor */ 00203 virtual ~CSparseMatrix(); 00204 00205 /** Copy operator from another existing object */ 00206 void operator = (const CSparseMatrix & other); 00207 00208 /** Erase all previous contents and leave the matrix as a "triplet" 1x1 matrix without any data. */ 00209 void clear(); 00210 00211 /** @} */ 00212 00213 /** @name Math operations (the interesting stuff...) 00214 @{ */ 00215 00216 void add_AB(const CSparseMatrix & A,const CSparseMatrix & B); //!< this = A+B 00217 void multiply_AB(const CSparseMatrix & A,const CSparseMatrix & B); //!< this = A*B 00218 void multiply_Ab(const mrpt::vector_double &b, mrpt::vector_double &out_res) const; //!< out_res = this * b 00219 00220 inline CSparseMatrix operator + (const CSparseMatrix & other) const 00221 { 00222 CSparseMatrix RES; 00223 RES.add_AB(*this,other); 00224 return RES; 00225 } 00226 inline CSparseMatrix operator * (const CSparseMatrix & other) const 00227 { 00228 CSparseMatrix RES; 00229 RES.multiply_AB(*this,other); 00230 return RES; 00231 } 00232 inline mrpt::vector_double operator * (const mrpt::vector_double & other) const { 00233 mrpt::vector_double res; 00234 multiply_Ab(other,res); 00235 return res; 00236 } 00237 inline void operator += (const CSparseMatrix & other) { 00238 this->add_AB(*this,other); 00239 } 00240 inline void operator *= (const CSparseMatrix & other) { 00241 this->multiply_AB(*this,other); 00242 } 00243 CSparseMatrix transpose() const; 00244 00245 /** @} */ 00246 00247 00248 /** @ Access the matrix, get, set elements, etc. 00249 @{ */ 00250 00251 /** ONLY for TRIPLET matrices: insert a new non-zero entry in the matrix. 00252 * This method cannot be used once the matrix is in column-compressed form. 00253 * The size of the matrix will be automatically extended if the indices are out of the current limits. 00254 * \sa isTriplet, compressFromTriplet 00255 */ 00256 void insert_entry(const size_t row, const size_t col, const double val ); 00257 00258 /** ONLY for TRIPLET matrices: Insert an element into a "cs", without checking if the matrix is in Triplet format and without extending the matrix extension/limits if (row,col) is out of the current size. */ 00259 void insert_entry_fast(const size_t row, const size_t col, const double val ); 00260 00261 /** ONLY for TRIPLET matrices: insert a given matrix (in any of the MRPT formats) at a given location of the sparse matrix. 00262 * This method cannot be used once the matrix is in column-compressed form. 00263 * The size of the matrix will be automatically extended if the indices are out of the current limits. 00264 * Since this is inline, it can be very efficient for fixed-size MRPT matrices. 00265 * \sa isTriplet, compressFromTriplet, insert_entry 00266 */ 00267 template <class MATRIX> 00268 inline void insert_submatrix(const size_t row, const size_t col, const MATRIX &M ) 00269 { 00270 if (!isTriplet()) THROW_EXCEPTION("insert_entry() is only available for sparse matrix in 'triplet' format.") 00271 const size_t nR = M.getRowCount(); 00272 const size_t nC = M.getColCount(); 00273 for (size_t r=0;r<nR;r++) 00274 for (size_t c=0;c<nC;c++) 00275 insert_entry_fast(row+r,col+c, M.get_unsafe(r,c)); 00276 // If needed, extend the size of the matrix: 00277 sparse_matrix.m = std::max(sparse_matrix.m, int(row+nR)); 00278 sparse_matrix.n = std::max(sparse_matrix.n, int(col+nC)); 00279 } 00280 00281 00282 /** ONLY for TRIPLET matrices: convert the matrix in a column-compressed form. 00283 * \sa insert_entry 00284 */ 00285 void compressFromTriplet(); 00286 00287 /** Return a dense representation of the sparse matrix. 00288 * \sa saveToTextFile_dense 00289 */ 00290 void get_dense(CMatrixDouble &outMat) const; 00291 00292 /** Static method to convert a "cs" structure into a dense representation of the sparse matrix. 00293 */ 00294 static void cs2dense(const cs& SM, CMatrixDouble &outMat); 00295 00296 /** save as a dense matrix to a text file \return False on any error. 00297 */ 00298 bool saveToTextFile_dense(const std::string &filName); 00299 00300 // Very basic, standard methods that MRPT methods expect for any matrix: 00301 inline size_t getRowCount() const { return sparse_matrix.m; } 00302 inline size_t getColCount() const { return sparse_matrix.n; } 00303 00304 /** Change the number of rows in the matrix (can't be lower than current size) */ 00305 inline void setRowCount(const size_t nRows) { ASSERT_(nRows>=(size_t)sparse_matrix.m); sparse_matrix.m = nRows; } 00306 inline void setColCount(const size_t nCols) { ASSERT_(nCols>=(size_t)sparse_matrix.n); sparse_matrix.n = nCols; } 00307 00308 /** Returns true if this sparse matrix is in "triplet" form. \sa isColumnCompressed */ 00309 inline bool isTriplet() const { return sparse_matrix.nz>=0; } // <0 means "column compressed", ">=0" means triplet. 00310 00311 /** Returns true if this sparse matrix is in "column compressed" form. \sa isTriplet */ 00312 inline bool isColumnCompressed() const { return sparse_matrix.nz<0; } // <0 means "column compressed", ">=0" means triplet. 00313 00314 /** @} */ 00315 00316 00317 /** @name Cholesky factorization 00318 @{ */ 00319 00320 /** Auxiliary class to hold the results of a Cholesky factorization of a sparse matrix. 00321 * Usage example: 00322 * \code 00323 * CSparseMatrix SM(100,100); 00324 * SM.insert_entry(i,j, val); ... 00325 * SM.compressFromTriplet(); 00326 * 00327 * // Do Cholesky decomposition: 00328 * CSparseMatrix::CholeskyDecomp CD(SM); 00329 * CD.get_inverse(); 00330 * ... 00331 * \endcode 00332 * 00333 * \note This class was initially adapted from "robotvision", by Hauke Strasdat, Steven Lovegrove and Andrew J. Davison. See http://www.openslam.org/robotvision.html 00334 * \note This class designed to be "uncopiable". 00335 * \sa The main class: CSparseMatrix 00336 */ 00337 class BASE_IMPEXP CholeskyDecomp : public mrpt::utils::CUncopiable 00338 { 00339 private: 00340 css * m_symbolic_structure; 00341 csn * m_numeric_structure; 00342 const CSparseMatrix *m_originalSM; //!< A const reference to the original matrix used to build this decomposition. 00343 00344 public: 00345 /** Constructor from a square definite-positive sparse matrix A, which can be use to solve Ax=b 00346 * The actual Cholesky decomposition takes places in this constructor. 00347 * \exception std::runtime_error On non-square input matrix. 00348 * \exception mrpt::math::CExceptionNotDefPos On non-definite-positive matrix as input. 00349 */ 00350 CholeskyDecomp(const CSparseMatrix &A); 00351 00352 /** Destructor */ 00353 virtual ~CholeskyDecomp(); 00354 00355 /** Return the L matrix (L*L' = M), as a dense matrix. */ 00356 inline CMatrixDouble get_L() const { CMatrixDouble L; get_L(L); return L; } 00357 00358 /** Return the L matrix (L*L' = M), as a dense matrix. */ 00359 void get_L(CMatrixDouble &out_L) const; 00360 00361 /** Return the vector from a back-substitution step that solves: Ux=b */ 00362 inline mrpt::vector_double backsub(const mrpt::vector_double &b) const { mrpt::vector_double res; backsub(b,res); return res; } 00363 00364 /** Return the vector from a back-substitution step that solves: Ux=b */ 00365 void backsub(const mrpt::vector_double &b, mrpt::vector_double &result_x) const; 00366 00367 /** Update the Cholesky factorization from an updated vesion of the original input, square definite-positive sparse matrix. 00368 * NOTE: This new matrix MUST HAVE exactly the same sparse structure than the original one. 00369 */ 00370 void update(const CSparseMatrix &new_SM); 00371 }; 00372 00373 00374 /** @} */ 00375 00376 }; // end class CSparseMatrix 00377 00378 } 00379 } 00380 #endif
Page generated by Doxygen 1.7.3 for MRPT 0.9.4 SVN: at Sat Mar 26 06:40:17 UTC 2011 |