Main MRPT website > C++ reference
MRPT logo

CSparseMatrix.h

Go to the documentation of this file.
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:exported at Tue Jan 25 21:56:31 UTC 2011