Main MRPT website > C++ reference for MRPT 1.5.7
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-2017, 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:
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
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".
Definition: os.cpp:358
EIGEN_STRONG_INLINE void multiply_Ab(const OTHERVECTOR1 &vIn, OTHERVECTOR2 &vOut, bool accumToOutput=false) const
void setColCount(const size_t nCols)
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
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
GLdouble s
Definition: glext.h:3602
void clear()
Clear the contents of this container.
Definition: ts_hash_map.h:113
A sparse matrix container (with cells of any type), with iterators.
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
std::vector< T1 > operator+(const std::vector< T1 > &a, const std::vector< T2 > &b)
a+b (element-wise sum)
Definition: ops_vectors.h:89
A numeric matrix of compile-time fixed size.
void multiply_AB(const CSparseMatrix &A, const CSparseMatrix &B)
this = A*B
CSparseMatrix(const CMatrixTemplateNumeric< T > &MAT)
Constructor from a dense matrix of any kind existing in MRPT, creating a "column-compressed" sparse m...
const GLubyte * c
Definition: glext.h:5590
std::vector< T1 > & operator+=(std::vector< T1 > &a, const std::vector< T2 > &b)
a+=b (element-wise sum)
Definition: ops_vectors.h:70
void add_AB(const CSparseMatrix &A, const CSparseMatrix &B)
this = A+B
int val
Definition: mrpt_jpeglib.h:953
GLubyte GLubyte b
Definition: glext.h:5575
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)
GLsizei const GLchar ** string
Definition: glext.h:3919
bool isTriplet() const
Returns true if this sparse matrix is in "triplet" form.
GLsizei GLboolean transpose
Definition: glext.h:3937
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...
GLdouble GLdouble GLdouble r
Definition: glext.h:3618
GLenum GLenum GLvoid * row
Definition: glext.h:3533
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
size_t getColCount() const
GLuint res
Definition: glext.h:6298
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)
GLsizei GLsizei GLenum GLenum const GLvoid * data
Definition: glext.h:3520
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.14 for MRPT 1.5.7 Git: 5902e14cc Wed Apr 24 15:04:01 2019 +0200 at lun oct 28 01:39:17 CET 2019