Main MRPT website > C++ reference for MRPT 1.4.0
CSparseMatrix.h
Go to the documentation of this file.
1 /* +---------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | http://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2016, Individual contributors, see AUTHORS file |
6  | See: http://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See details in http://www.mrpt.org/License |
8  +---------------------------------------------------------------------------+ */
9 #ifndef CSparseMatrix_H
10 #define CSparseMatrix_H
11 
12 #include <mrpt/utils/utils_defs.h>
13 #include <mrpt/utils/exceptions.h>
14 #include <mrpt/utils/CUncopiable.h>
15 
16 #include <mrpt/math/math_frwds.h>
17 #include <mrpt/utils/types_math.h>
21 #include <cstring> // memcpy
22 
23 // Include CSparse lib headers, either from the system or embedded:
24 extern "C"{
25 #if MRPT_HAS_SUITESPARSE
26 # define NCOMPLEX // In MRPT we don't need complex numbers, so avoid the annoying warning: 'cs_ci_house' has C-linkage specified, but returns UDT 'std::complex<double>' which is incompatible with C
27 # include "cs.h"
28 #else
29 # include <mrpt/otherlibs/CSparse/cs.h>
30 #endif
31 }
32 
33 namespace mrpt
34 {
35  namespace math
36  {
37  /** Used in mrpt::math::CSparseMatrix */
39  {
40  public:
41  CExceptionNotDefPos(const std::string &s) : CMRPTException(s) { }
42  };
43 
44 
45  /** A sparse matrix structure, wrapping T. Davis' CSparse library (part of suitesparse)
46  * The type of the matrix entries is fixed to "double".
47  *
48  * There are two formats for the non-zero entries in this matrix:
49  * - A "triplet" matrix: a set of (r,c)=val triplet entries.
50  * - A column-compressed sparse (CCS) matrix.
51  *
52  * The latter is the "normal" format, which is expected by all mathematical operations defined
53  * in this class. There're three ways of initializing and populating a sparse matrix:
54  *
55  * <ol>
56  * <li> <b>As a triplet (empty), then add entries, then compress:</b>
57  * \code
58  * CSparseMatrix SM(100,100);
59  * SM.insert_entry(i,j, val); // or
60  * SM.insert_submatrix(i,j, MAT); // ...
61  * SM.compressFromTriplet();
62  * \endcode
63  * </li>
64  * <li> <b>As a triplet from a CSparseMatrixTemplate<double>:</b>
65  * \code
66  * CSparseMatrixTemplate<double> data;
67  * data(row,col) = val;
68  * ...
69  * CSparseMatrix SM(data);
70  * \endcode
71  * </li>
72  * <li> <b>From an existing dense matrix:</b>
73  * \code
74  * CMatrixDouble data(100,100); // or
75  * CMatrixFloat data(100,100); // or
76  * CMatrixFixedNumeric<double,4,6> data; // etc...
77  * CSparseMatrix SM(data);
78  * \endcode
79  * </li>
80  *
81  * </ol>
82  *
83  * Due to its practical utility, there is a special inner class CSparseMatrix::CholeskyDecomp to handle Cholesky-related methods and data.
84  *
85  * \note This class was initially adapted from "robotvision", by Hauke Strasdat, Steven Lovegrove and Andrew J. Davison. See http://www.openslam.org/robotvision.html
86  * \note CSparse is maintained by Timothy Davis: http://people.sc.fsu.edu/~jburkardt/c_src/csparse/csparse.html .
87  * \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
88  *
89  * \sa mrpt::math::MatrixBlockSparseCols, mrpt::math::CMatrixFixedNumeric, mrpt::math::CMatrixTemplateNumeric, etc.
90  * \ingroup mrpt_base_grp
91  */
93  {
94  private:
96 
97  /** Initialization from a dense matrix of any kind existing in MRPT. */
98  template <class MATRIX>
99  void construct_from_mrpt_mat(const MATRIX & C)
100  {
101  std::vector<int> row_list, col_list; // Use "int" for convenience so we can do a memcpy below...
102  std::vector<double> content_list;
103  const int nCol = C.getColCount();
104  const int nRow = C.getRowCount();
105  for (int c=0; c<nCol; ++c)
106  {
107  col_list.push_back(row_list.size());
108  for (int r=0; r<nRow; ++r)
109  if (C.get_unsafe(r,c)!=0)
110  {
111  row_list.push_back(r);
112  content_list.push_back(C(r,c));
113  }
114  }
115  col_list.push_back(row_list.size());
116 
117  sparse_matrix.m = nRow;
118  sparse_matrix.n = nCol;
119  sparse_matrix.nzmax = content_list.size();
120  sparse_matrix.i = (int*)malloc(sizeof(int)*row_list.size());
121  sparse_matrix.p = (int*)malloc(sizeof(int)*col_list.size());
122  sparse_matrix.x = (double*)malloc(sizeof(double)*content_list.size());
123 
124  ::memcpy(sparse_matrix.i, &row_list[0], sizeof(row_list[0])*row_list.size() );
125  ::memcpy(sparse_matrix.p, &col_list[0], sizeof(col_list[0])*col_list.size() );
126  ::memcpy(sparse_matrix.x, &content_list[0], sizeof(content_list[0])*content_list.size() );
127 
128  sparse_matrix.nz = -1; // <0 means "column compressed", ">=0" means triplet.
129  }
130 
131  /** Initialization from a triplet "cs", which is first compressed */
132  void construct_from_triplet(const cs & triplet);
133 
134  /** To be called by constructors only, assume previous pointers are trash and overwrite them */
135  void construct_from_existing_cs(const cs &sm);
136 
137  /** free buffers (deallocate the memory of the i,p,x buffers) */
138  void internal_free_mem();
139 
140  /** Copy the data from an existing "cs" CSparse data structure */
141  void copy(const cs * const sm);
142 
143  /** Fast copy the data from an existing "cs" CSparse data structure, copying the pointers and leaving NULLs in the source structure. */
144  void copy_fast(cs * const sm);
145 
146  public:
147 
148  /** @name Constructors, destructor & copy operations
149  @{ */
150 
151  /** Create an initially empty sparse matrix, in the "triplet" form.
152  * Notice that you must call "compressFromTriplet" after populating the matrix and before using the math operatons on this matrix.
153  * The initial size can be later on extended with insert_entry() or setRowCount() & setColCount().
154  * \sa insert_entry, setRowCount, setColCount
155  */
156  CSparseMatrix(const size_t nRows=0, const size_t nCols=0);
157 
158  /** A good way to initialize a sparse matrix from a list of non NULL elements.
159  * This constructor takes all the non-zero entries in "data" and builds a column-compressed sparse representation.
160  */
161  template <typename T>
163  {
164  ASSERTMSG_(!data.empty(), "Input data must contain at least one non-zero element.")
165  sparse_matrix.i = NULL; // This is to know they shouldn't be tried to free()
166  sparse_matrix.p = NULL;
167  sparse_matrix.x = NULL;
168 
169  // 1) Create triplet matrix
170  CSparseMatrix triplet(data.getRowCount(),data.getColCount());
171  // 2) Put data in:
172  for (typename CSparseMatrixTemplate<T>::const_iterator it=data.begin();it!=data.end();++it)
173  triplet.insert_entry_fast(it->first.first,it->first.second, it->second);
174 
175  // 3) Compress:
176  construct_from_triplet(triplet.sparse_matrix);
177  }
178 
179 
180  // We can't do a simple "template <class ANYMATRIX>" since it would be tried to match against "cs*"...
181 
182  /** Constructor from a dense matrix of any kind existing in MRPT, creating a "column-compressed" sparse matrix. */
183  template <typename T,size_t N,size_t M> inline explicit CSparseMatrix(const CMatrixFixedNumeric<T,N,M> &MAT) { construct_from_mrpt_mat(MAT); }
184 
185  /** Constructor from a dense matrix of any kind existing in MRPT, creating a "column-compressed" sparse matrix. */
186  template <typename T> inline explicit CSparseMatrix(const CMatrixTemplateNumeric<T> &MAT) { construct_from_mrpt_mat(MAT); }
187 
188  /** Copy constructor */
189  CSparseMatrix(const CSparseMatrix & other);
190 
191  /** Copy constructor from an existing "cs" CSparse data structure */
192  explicit CSparseMatrix(const cs * const sm);
193 
194  /** Destructor */
195  virtual ~CSparseMatrix();
196 
197  /** Copy operator from another existing object */
198  void operator = (const CSparseMatrix & other);
199 
200  /** Fast swap contents with another sparse matrix */
201  void swap(CSparseMatrix & other);
202 
203  /** Erase all previous contents and leave the matrix as a "triplet" ROWS x COLS matrix without any nonzero entry. */
204  void clear(const size_t nRows=1, const size_t nCols=1);
205 
206  /** @} */
207 
208  /** @name Math operations (the interesting stuff...)
209  @{ */
210 
211  void add_AB(const CSparseMatrix & A,const CSparseMatrix & B); //!< this = A+B
212  void multiply_AB(const CSparseMatrix & A,const CSparseMatrix & B); //!< this = A*B
213  void multiply_Ab(const mrpt::math::CVectorDouble &b, mrpt::math::CVectorDouble &out_res) const; //!< out_res = this * b
214 
215  inline CSparseMatrix operator + (const CSparseMatrix & other) const
216  {
217  CSparseMatrix RES;
218  RES.add_AB(*this,other);
219  return RES;
220  }
221  inline CSparseMatrix operator * (const CSparseMatrix & other) const
222  {
223  CSparseMatrix RES;
224  RES.multiply_AB(*this,other);
225  return RES;
226  }
229  multiply_Ab(other,res);
230  return res;
231  }
232  inline void operator += (const CSparseMatrix & other) {
233  this->add_AB(*this,other);
234  }
235  inline void operator *= (const CSparseMatrix & other) {
236  this->multiply_AB(*this,other);
237  }
238  CSparseMatrix transpose() const;
239 
240  /** @} */
241 
242 
243  /** @ Access the matrix, get, set elements, etc.
244  @{ */
245 
246  /** ONLY for TRIPLET matrices: insert a new non-zero entry in the matrix.
247  * This method cannot be used once the matrix is in column-compressed form.
248  * The size of the matrix will be automatically extended if the indices are out of the current limits.
249  * \sa isTriplet, compressFromTriplet
250  */
251  void insert_entry(const size_t row, const size_t col, const double val );
252 
253  /** This was an optimized version, but is now equivalent to insert_entry() due to the need to be compatible with unmodified CSparse system libraries. */
254  inline void insert_entry_fast(const size_t row, const size_t col, const double val ) { insert_entry(row,col,val); }
255 
256  /** ONLY for TRIPLET matrices: insert a given matrix (in any of the MRPT formats) at a given location of the sparse matrix.
257  * This method cannot be used once the matrix is in column-compressed form.
258  * The size of the matrix will be automatically extended if the indices are out of the current limits.
259  * Since this is inline, it can be very efficient for fixed-size MRPT matrices.
260  * \sa isTriplet, compressFromTriplet, insert_entry
261  */
262  template <class MATRIX>
263  inline void insert_submatrix(const size_t row, const size_t col, const MATRIX &M )
264  {
265  if (!isTriplet()) THROW_EXCEPTION("insert_entry() is only available for sparse matrix in 'triplet' format.")
266  const size_t nR = M.getRowCount();
267  const size_t nC = M.getColCount();
268  for (size_t r=0;r<nR;r++)
269  for (size_t c=0;c<nC;c++)
270  insert_entry_fast(row+r,col+c, M.get_unsafe(r,c));
271  // If needed, extend the size of the matrix:
272  sparse_matrix.m = std::max(sparse_matrix.m, int(row+nR));
273  sparse_matrix.n = std::max(sparse_matrix.n, int(col+nC));
274  }
275 
276 
277  /** ONLY for TRIPLET matrices: convert the matrix in a column-compressed form.
278  * \sa insert_entry
279  */
280  void compressFromTriplet();
281 
282  /** Return a dense representation of the sparse matrix.
283  * \sa saveToTextFile_dense
284  */
285  void get_dense(CMatrixDouble &outMat) const;
286 
287  /** Static method to convert a "cs" structure into a dense representation of the sparse matrix.
288  */
289  static void cs2dense(const cs& SM, CMatrixDouble &outMat);
290 
291  /** save as a dense matrix to a text file \return False on any error.
292  */
293  bool saveToTextFile_dense(const std::string &filName);
294 
295  /** Save sparse structure to a text file loadable from MATLAB (can be called on triplet or CCS matrices).
296  *
297  * The format of the text file is:
298  * \code
299  * NUM_ROWS NUM_COLS NUM_NON_ZERO_MAX
300  * row_1 col_1 value_1
301  * row_2 col_2 value_2
302  * ...
303  * \endcode
304  *
305  * Instructions for loading from MATLAB in triplet form will be automatically writen to the
306  * output file as comments in th first lines:
307  *
308  * \code
309  * D=load('file.txt');
310  * SM=spconvert(D(2:end,:));
311  * or, to always preserve the actual matrix size m x n:
312  * m=D(1,1); n=D(1,2); nzmax=D(1,3);
313  * Di=D(2:end,1); Dj=D(2:end,2); Ds=D(2:end,3);
314  * M=sparse(Di,Dj,Ds, m,n, nzmax);
315  * \endcode
316  * \return False on any error.
317  */
318  bool saveToTextFile_sparse(const std::string &filName);
319 
320  // Very basic, standard methods that MRPT methods expect for any matrix:
321  inline size_t getRowCount() const { return sparse_matrix.m; }
322  inline size_t getColCount() const { return sparse_matrix.n; }
323 
324  /** Change the number of rows in the matrix (can't be lower than current size) */
325  inline void setRowCount(const size_t nRows) { ASSERT_(nRows>=(size_t)sparse_matrix.m); sparse_matrix.m = nRows; }
326  inline void setColCount(const size_t nCols) { ASSERT_(nCols>=(size_t)sparse_matrix.n); sparse_matrix.n = nCols; }
327 
328  /** Returns true if this sparse matrix is in "triplet" form. \sa isColumnCompressed */
329  inline bool isTriplet() const { return sparse_matrix.nz>=0; } // <0 means "column compressed", ">=0" means triplet.
330 
331  /** Returns true if this sparse matrix is in "column compressed" form. \sa isTriplet */
332  inline bool isColumnCompressed() const { return sparse_matrix.nz<0; } // <0 means "column compressed", ">=0" means triplet.
333 
334  /** @} */
335 
336 
337  /** @name Cholesky factorization
338  @{ */
339 
340  /** Auxiliary class to hold the results of a Cholesky factorization of a sparse matrix.
341  * This implementation does not allow updating/downdating.
342  *
343  * Usage example:
344  * \code
345  * CSparseMatrix SM(100,100);
346  * SM.insert_entry(i,j, val); ...
347  * SM.compressFromTriplet();
348  *
349  * // Do Cholesky decomposition:
350  * CSparseMatrix::CholeskyDecomp CD(SM);
351  * CD.get_inverse();
352  * ...
353  * \endcode
354  *
355  * \note Only the upper triangular part of the input matrix is accessed.
356  * \note This class was initially adapted from "robotvision", by Hauke Strasdat, Steven Lovegrove and Andrew J. Davison. See http://www.openslam.org/robotvision.html
357  * \note This class designed to be "uncopiable".
358  * \sa The main class: CSparseMatrix
359  */
361  {
362  private:
365  const CSparseMatrix *m_originalSM; //!< A const reference to the original matrix used to build this decomposition.
366 
367  public:
368  /** Constructor from a square definite-positive sparse matrix A, which can be use to solve Ax=b
369  * The actual Cholesky decomposition takes places in this constructor.
370  * \note Only the upper triangular part of the matrix is accessed.
371  * \exception std::runtime_error On non-square input matrix.
372  * \exception mrpt::math::CExceptionNotDefPos On non-definite-positive matrix as input.
373  */
374  CholeskyDecomp(const CSparseMatrix &A);
375 
376  /** Destructor */
377  virtual ~CholeskyDecomp();
378 
379  /** Return the L matrix (L*L' = M), as a dense matrix. */
380  inline CMatrixDouble get_L() const { CMatrixDouble L; get_L(L); return L; }
381 
382  /** Return the L matrix (L*L' = M), as a dense matrix. */
383  void get_L(CMatrixDouble &out_L) const;
384 
385  /** Return the vector from a back-substitution step that solves: Ux=b */
386  template <class VECTOR>
387  inline VECTOR backsub(const VECTOR &b) const { VECTOR res; backsub(b,res); return res; }
388 
389  /** Return the vector from a back-substitution step that solves: Ux=b. Vectors can be Eigen::VectorXd or mrpt::math::CVectorDouble */
390  void backsub(const Eigen::VectorXd &b, Eigen::VectorXd &result_x) const;
391 
392  /** \overload for double pointers which assume the user has reserved the output memory for \a result */
393  void backsub(const double *b, double *result, const size_t N) const;
394 
395  /** Update the Cholesky factorization from an updated vesion of the original input, square definite-positive sparse matrix.
396  * NOTE: This new matrix MUST HAVE exactly the same sparse structure than the original one.
397  */
398  void update(const CSparseMatrix &new_SM);
399  };
400 
401 
402  /** @} */
403 
404  }; // end class CSparseMatrix
405 
406  }
407 }
408 #endif
const_iterator begin() const
Returns an iterator which points to the starting point of the matrix.
EIGEN_STRONG_INLINE void multiply_Ab(const OTHERVECTOR1 &vIn, OTHERVECTOR2 &vOut, bool accumToOutput=false) const
void setColCount(const size_t nCols)
void BASE_IMPEXP memcpy(void *dest, size_t destSize, const void *src, size_t copyCount) MRPT_NO_THROWS
An OS and compiler independent version of "memcpy".
size_t getRowCount() const
const CSparseMatrix * m_originalSM
A const reference to the original matrix used to build this decomposition.
void insert_entry_fast(const size_t row, const size_t col, const double val)
This was an optimized version, but is now equivalent to insert_entry() due to the need to be compatib...
#define THROW_EXCEPTION(msg)
Column vector, like Eigen::MatrixX*, but automatically initialized to zeros since construction...
Definition: eigen_frwds.h:35
size_t getColCount() const
Returns the amount of columns in this matrix.
Auxiliary class to hold the results of a Cholesky factorization of a sparse matrix.
A sparse matrix structure, wrapping T.
Definition: CSparseMatrix.h:92
void construct_from_mrpt_mat(const MATRIX &C)
Initialization from a dense matrix of any kind existing in MRPT.
Definition: CSparseMatrix.h:99
SparseMatrixMap::const_iterator const_iterator
Const iterator to move through the matrix.
CExceptionNotDefPos(const std::string &s)
Definition: CSparseMatrix.h:41
A sparse matrix container (with cells of any type), with iterators.
bool empty() const
Are there no elements set to !=0 ?
CMatrixDouble get_L() const
Return the L matrix (L*L&#39; = M), as a dense matrix.
Used in mrpt::math::CSparseMatrix.
Definition: CSparseMatrix.h:38
size_t getRowCount() const
Returns the amount of rows in this matrix.
std::vector< T1 > operator+(const std::vector< T1 > &a, const std::vector< T2 > &b)
a+b (element-wise sum)
Definition: ops_vectors.h:89
void multiply_AB(const CSparseMatrix &A, const CSparseMatrix &B)
this = A*B
A numeric matrix of compile-time fixed size.
CSparseMatrix(const CMatrixTemplateNumeric< T > &MAT)
Constructor from a dense matrix of any kind existing in MRPT, creating a "column-compressed" sparse m...
std::vector< T1 > & operator+=(std::vector< T1 > &a, const std::vector< T2 > &b)
a+=b (element-wise sum)
Definition: ops_vectors.h:70
EIGEN_STRONG_INLINE void multiply_AB(const MATRIX1 &A, const MATRIX2 &B)
The base class of classes that cannot be copied: compile-time errors will be issued on any copy opera...
Definition: CUncopiable.h:30
The base for MRPT-especific exceptions.
Definition: exceptions.h:24
void setRowCount(const size_t nRows)
Change the number of rows in the matrix (can&#39;t be lower than current size)
bool isTriplet() const
Returns true if this sparse matrix is in "triplet" form.
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
CSparseMatrix(const CMatrixFixedNumeric< T, N, M > &MAT)
Constructor from a dense matrix of any kind existing in MRPT, creating a "column-compressed" sparse m...
const_iterator end() const
Returns an iterator which points to the end of the matrix.
void insert_submatrix(const size_t row, const size_t col, const MATRIX &M)
ONLY for TRIPLET matrices: insert a given matrix (in any of the MRPT formats) at a given location of ...
#define ASSERT_(f)
CMRPTException(const std::string &s)
Definition: exceptions.h:27
void add_AB(const CSparseMatrix &A, const CSparseMatrix &B)
this = A+B
size_t getColCount() const
VECTOR backsub(const VECTOR &b) const
Return the vector from a back-substitution step that solves: Ux=b.
CSparseMatrix(const CSparseMatrixTemplate< T > &data)
A good way to initialize a sparse matrix from a list of non NULL elements.
bool isColumnCompressed() const
Returns true if this sparse matrix is in "column compressed" form.
#define ASSERTMSG_(f, __ERROR_MSG)
std::vector< T1 > & operator*=(std::vector< T1 > &a, const std::vector< T2 > &b)
a*=b (element-wise multiplication)
Definition: ops_vectors.h:40
std::vector< T1 > operator*(const std::vector< T1 > &a, const std::vector< T2 > &b)
a*b (element-wise multiplication)
Definition: ops_vectors.h:59



Page generated by Doxygen 1.8.13 for MRPT 1.4.0 SVN: at Wed Mar 15 00:43:31 UTC 2017