Main MRPT website > C++ reference for MRPT 1.9.9
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-2018, 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 #pragma once
10 
11 #include <mrpt/math/math_frwds.h>
15 #include <cstring> // memcpy
16 #include <stdexcept>
17 
18 // Include CSparse lib headers, either from the system or embedded:
19 extern "C" {
20 #if MRPT_HAS_SUITESPARSE
21 #define NCOMPLEX // In MRPT we don't need complex numbers, so avoid the
22 // annoying warning: 'cs_ci_house' has C-linkage specified,
23 // but returns UDT 'std::complex<double>' which is
24 // incompatible with C
25 #include "cs.h"
26 #else
27 #include <mrpt/otherlibs/CSparse/cs.h>
28 #endif
29 }
30 
31 namespace mrpt
32 {
33 namespace math
34 {
35 /** Used in mrpt::math::CSparseMatrix */
36 struct CExceptionNotDefPos : public std::runtime_error
37 {
38  CExceptionNotDefPos(const char* s) : std::runtime_error(s) {}
39 };
40 
41 /** A sparse matrix structure, wrapping T. Davis' CSparse library (part of
42  *suitesparse)
43  * The type of the matrix entries is fixed to "double".
44  *
45  * There are two formats for the non-zero entries in this matrix:
46  * - A "triplet" matrix: a set of (r,c)=val triplet entries.
47  * - A column-compressed sparse (CCS) matrix.
48  *
49  * The latter is the "normal" format, which is expected by all mathematical
50  *operations defined
51  * in this class. There're three ways of initializing and populating a sparse
52  *matrix:
53  *
54  * <ol>
55  * <li> <b>As a triplet (empty), then add entries, then compress:</b>
56  * \code
57  * CSparseMatrix SM(100,100);
58  * SM.insert_entry(i,j, val); // or
59  * SM.insert_submatrix(i,j, MAT); // ...
60  * SM.compressFromTriplet();
61  * \endcode
62  * </li>
63  * <li> <b>As a triplet from a CSparseMatrixTemplate<double>:</b>
64  * \code
65  * CSparseMatrixTemplate<double> data;
66  * data(row,col) = val;
67  * ...
68  * CSparseMatrix SM(data);
69  * \endcode
70  * </li>
71  * <li> <b>From an existing dense matrix:</b>
72  * \code
73  * CMatrixDouble data(100,100); // or
74  * CMatrixFloat data(100,100); // or
75  * CMatrixFixedNumeric<double,4,6> data; // etc...
76  * CSparseMatrix SM(data);
77  * \endcode
78  * </li>
79  *
80  * </ol>
81  *
82  * Due to its practical utility, there is a special inner class
83  *CSparseMatrix::CholeskyDecomp to handle Cholesky-related methods and data.
84  *
85  * \note This class was initially adapted from "robotvision", by Hauke
86  *Strasdat, Steven Lovegrove and Andrew J. Davison. See
87  *http://www.openslam.org/robotvision.html
88  * \note CSparse is maintained by Timothy Davis:
89  *http://people.sc.fsu.edu/~jburkardt/c_src/csparse/csparse.html .
90  * \note See also his book "Direct methods for sparse linear systems".
91  *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
92  *
93  * \sa mrpt::math::MatrixBlockSparseCols, mrpt::math::CMatrixFixedNumeric,
94  *mrpt::math::CMatrixTemplateNumeric, etc.
95  * \ingroup mrpt_math_grp
96  */
98 {
99  private:
101 
102  /** Initialization from a dense matrix of any kind existing in MRPT. */
103  template <class MATRIX>
104  void construct_from_mrpt_mat(const MATRIX& C)
105  {
106  std::vector<int> row_list, col_list; // Use "int" for convenience so we
107  // can do a memcpy below...
108  std::vector<double> content_list;
109  const int nCol = C.cols();
110  const int nRow = C.rows();
111  for (int c = 0; c < nCol; ++c)
112  {
113  col_list.push_back(row_list.size());
114  for (int r = 0; r < nRow; ++r)
115  if (C.get_unsafe(r, c) != 0)
116  {
117  row_list.push_back(r);
118  content_list.push_back(C(r, c));
119  }
120  }
121  col_list.push_back(row_list.size());
122 
123  sparse_matrix.m = nRow;
124  sparse_matrix.n = nCol;
125  sparse_matrix.nzmax = content_list.size();
126  sparse_matrix.i = (int*)malloc(sizeof(int) * row_list.size());
127  sparse_matrix.p = (int*)malloc(sizeof(int) * col_list.size());
128  sparse_matrix.x = (double*)malloc(sizeof(double) * content_list.size());
129 
130  ::memcpy(
131  sparse_matrix.i, &row_list[0],
132  sizeof(row_list[0]) * row_list.size());
133  ::memcpy(
134  sparse_matrix.p, &col_list[0],
135  sizeof(col_list[0]) * col_list.size());
136  ::memcpy(
137  sparse_matrix.x, &content_list[0],
138  sizeof(content_list[0]) * content_list.size());
139 
140  sparse_matrix.nz =
141  -1; // <0 means "column compressed", ">=0" means triplet.
142  }
143 
144  /** Initialization from a triplet "cs", which is first compressed */
145  void construct_from_triplet(const cs& triplet);
146 
147  /** To be called by constructors only, assume previous pointers are trash
148  * and overwrite them */
149  void construct_from_existing_cs(const cs& sm);
150 
151  /** free buffers (deallocate the memory of the i,p,x buffers) */
152  void internal_free_mem();
153 
154  /** Copy the data from an existing "cs" CSparse data structure */
155  void copy(const cs* const sm);
156 
157  /** Fast copy the data from an existing "cs" CSparse data structure, copying
158  * the pointers and leaving NULLs in the source structure. */
159  void copy_fast(cs* const sm);
160 
161  public:
162  /** @name Constructors, destructor & copy operations
163  @{ */
164 
165  /** Create an initially empty sparse matrix, in the "triplet" form.
166  * Notice that you must call "compressFromTriplet" after populating the
167  * matrix and before using the math operatons on this matrix.
168  * The initial size can be later on extended with insert_entry() or
169  * setRowCount() & setColCount().
170  * \sa insert_entry, setRowCount, setColCount
171  */
172  CSparseMatrix(const size_t nRows = 0, const size_t nCols = 0);
173 
174  /** A good way to initialize a sparse matrix from a list of non nullptr
175  * elements.
176  * This constructor takes all the non-zero entries in "data" and builds a
177  * column-compressed sparse representation.
178  */
179  template <typename T>
181  {
182  ASSERTMSG_(
183  !data.empty(),
184  "Input data must contain at least one non-zero element.");
185  sparse_matrix.i =
186  nullptr; // This is to know they shouldn't be tried to free()
187  sparse_matrix.p = nullptr;
188  sparse_matrix.x = nullptr;
189 
190  // 1) Create triplet matrix
191  CSparseMatrix triplet(data.rows(), data.cols());
192  // 2) Put data in:
194  data.begin();
195  it != data.end(); ++it)
196  triplet.insert_entry_fast(
197  it->first.first, it->first.second, it->second);
198 
199  // 3) Compress:
200  construct_from_triplet(triplet.sparse_matrix);
201  }
202 
203  // We can't do a simple "template <class ANYMATRIX>" since it would be tried
204  // to match against "cs*"...
205 
206  /** Constructor from a dense matrix of any kind existing in MRPT, creating a
207  * "column-compressed" sparse matrix. */
208  template <typename T, size_t N, size_t M>
209  inline explicit CSparseMatrix(const CMatrixFixedNumeric<T, N, M>& MAT)
210  {
212  }
213 
214  /** Constructor from a dense matrix of any kind existing in MRPT, creating a
215  * "column-compressed" sparse matrix. */
216  template <typename T>
217  inline explicit CSparseMatrix(const CMatrixTemplateNumeric<T>& MAT)
218  {
220  }
221 
222  /** Copy constructor */
223  CSparseMatrix(const CSparseMatrix& other);
224 
225  /** Copy constructor from an existing "cs" CSparse data structure */
226  explicit CSparseMatrix(const cs* const sm);
227 
228  /** Destructor */
229  virtual ~CSparseMatrix();
230 
231  /** Copy operator from another existing object */
232  void operator=(const CSparseMatrix& other);
233 
234  /** Fast swap contents with another sparse matrix */
235  void swap(CSparseMatrix& other);
236 
237  /** Erase all previous contents and leave the matrix as a "triplet" ROWS x
238  * COLS matrix without any nonzero entry. */
239  void clear(const size_t nRows = 1, const size_t nCols = 1);
240 
241  /** @} */
242 
243  /** @name Math operations (the interesting stuff...)
244  @{ */
245 
246  /** this = A+B */
247  void add_AB(const CSparseMatrix& A, const CSparseMatrix& B);
248  /** this = A*B */
249  void multiply_AB(const CSparseMatrix& A, const CSparseMatrix& B);
250  /** out_res = this * b */
251  void multiply_Ab(
253  mrpt::math::CVectorDouble& out_res) const;
254 
255  inline CSparseMatrix operator+(const CSparseMatrix& other) const
256  {
257  CSparseMatrix RES;
258  RES.add_AB(*this, other);
259  return RES;
260  }
261  inline CSparseMatrix operator*(const CSparseMatrix& other) const
262  {
263  CSparseMatrix RES;
264  RES.multiply_AB(*this, other);
265  return RES;
266  }
268  const mrpt::math::CVectorDouble& other) const
269  {
271  multiply_Ab(other, res);
272  return res;
273  }
274  inline void operator+=(const CSparseMatrix& other)
275  {
276  this->add_AB(*this, other);
277  }
278  inline void operator*=(const CSparseMatrix& other)
279  {
280  this->multiply_AB(*this, other);
281  }
282  CSparseMatrix transpose() const;
283 
284  /** @} */
285 
286  /** @ Access the matrix, get, set elements, etc.
287  @{ */
288 
289  /** ONLY for TRIPLET matrices: insert a new non-zero entry in the matrix.
290  * This method cannot be used once the matrix is in column-compressed
291  * form.
292  * The size of the matrix will be automatically extended if the indices
293  * are out of the current limits.
294  * \sa isTriplet, compressFromTriplet
295  */
296  void insert_entry(const size_t row, const size_t col, const double val);
297 
298  /** This was an optimized version, but is now equivalent to insert_entry()
299  * due to the need to be compatible with unmodified CSparse system
300  * libraries. */
301  inline void insert_entry_fast(
302  const size_t row, const size_t col, const double val)
303  {
304  insert_entry(row, col, val);
305  }
306 
307  /** ONLY for TRIPLET matrices: insert a given matrix (in any of the MRPT
308  * formats) at a given location of the sparse matrix.
309  * This method cannot be used once the matrix is in column-compressed
310  * form.
311  * The size of the matrix will be automatically extended if the indices
312  * are out of the current limits.
313  * Since this is inline, it can be very efficient for fixed-size MRPT
314  * matrices.
315  * \sa isTriplet, compressFromTriplet, insert_entry
316  */
317  template <class MATRIX>
318  inline void insert_submatrix(
319  const size_t row, const size_t col, const MATRIX& M)
320  {
321  if (!isTriplet())
323  "insert_entry() is only available for sparse matrix in "
324  "'triplet' format.")
325  const size_t nR = M.rows();
326  const size_t nC = M.cols();
327  for (size_t r = 0; r < nR; r++)
328  for (size_t c = 0; c < nC; c++)
329  insert_entry_fast(row + r, col + c, M.get_unsafe(r, c));
330  // If needed, extend the size of the matrix:
331  sparse_matrix.m = std::max(sparse_matrix.m, int(row + nR));
332  sparse_matrix.n = std::max(sparse_matrix.n, int(col + nC));
333  }
334 
335  /** ONLY for TRIPLET matrices: convert the matrix in a column-compressed
336  * form.
337  * \sa insert_entry
338  */
339  void compressFromTriplet();
340 
341  /** Return a dense representation of the sparse matrix.
342  * \sa saveToTextFile_dense
343  */
344  void get_dense(CMatrixDouble& outMat) const;
345 
346  /** Static method to convert a "cs" structure into a dense representation of
347  * the sparse matrix.
348  */
349  static void cs2dense(const cs& SM, CMatrixDouble& outMat);
350 
351  /** save as a dense matrix to a text file \return False on any error.
352  */
353  bool saveToTextFile_dense(const std::string& filName);
354 
355  /** Save sparse structure to a text file loadable from MATLAB (can be called
356  * on triplet or CCS matrices).
357  *
358  * The format of the text file is:
359  * \code
360  * NUM_ROWS NUM_COLS NUM_NON_ZERO_MAX
361  * row_1 col_1 value_1
362  * row_2 col_2 value_2
363  * ...
364  * \endcode
365  *
366  * Instructions for loading from MATLAB in triplet form will be
367  * automatically writen to the
368  * output file as comments in th first lines:
369  *
370  * \code
371  * D=load('file.txt');
372  * SM=spconvert(D(2:end,:));
373  * or, to always preserve the actual matrix size m x n:
374  * m=D(1,1); n=D(1,2); nzmax=D(1,3);
375  * Di=D(2:end,1); Dj=D(2:end,2); Ds=D(2:end,3);
376  * M=sparse(Di,Dj,Ds, m,n, nzmax);
377  * \endcode
378  * \return False on any error.
379  */
380  bool saveToTextFile_sparse(const std::string& filName);
381 
382  // Very basic, standard methods that MRPT methods expect for any matrix:
383  inline size_t rows() const { return sparse_matrix.m; }
384  inline size_t cols() const { return sparse_matrix.n; }
385  /** Change the number of rows in the matrix (can't be lower than current
386  * size) */
387  inline void setRowCount(const size_t nRows)
388  {
389  ASSERT_(nRows >= (size_t)sparse_matrix.m);
390  sparse_matrix.m = nRows;
391  }
392  inline void setColCount(const size_t nCols)
393  {
394  ASSERT_(nCols >= (size_t)sparse_matrix.n);
395  sparse_matrix.n = nCols;
396  }
397 
398  /** Returns true if this sparse matrix is in "triplet" form. \sa
399  * isColumnCompressed */
400  inline bool isTriplet() const
401  {
402  return sparse_matrix.nz >= 0;
403  } // <0 means "column compressed", ">=0" means triplet.
404 
405  /** Returns true if this sparse matrix is in "column compressed" form. \sa
406  * isTriplet */
407  inline bool isColumnCompressed() const
408  {
409  return sparse_matrix.nz < 0;
410  } // <0 means "column compressed", ">=0" means triplet.
411 
412  /** @} */
413 
414  /** @name Cholesky factorization
415  @{ */
416 
417  /** Auxiliary class to hold the results of a Cholesky factorization of a
418  *sparse matrix.
419  * This implementation does not allow updating/downdating.
420  *
421  * Usage example:
422  * \code
423  * CSparseMatrix SM(100,100);
424  * SM.insert_entry(i,j, val); ...
425  * SM.compressFromTriplet();
426  *
427  * // Do Cholesky decomposition:
428  * CSparseMatrix::CholeskyDecomp CD(SM);
429  * CD.get_inverse();
430  * ...
431  * \endcode
432  *
433  * \note Only the upper triangular part of the input matrix is accessed.
434  * \note This class was initially adapted from "robotvision", by Hauke
435  *Strasdat, Steven Lovegrove and Andrew J. Davison. See
436  *http://www.openslam.org/robotvision.html
437  * \note This class designed to be "uncopiable".
438  * \sa The main class: CSparseMatrix
439  */
441  {
442  private:
445  /** A const reference to the original matrix used to build this
446  * decomposition. */
448 
449  public:
450  /** Constructor from a square definite-positive sparse matrix A, which
451  * can be use to solve Ax=b
452  * The actual Cholesky decomposition takes places in this
453  * constructor.
454  * \note Only the upper triangular part of the matrix is accessed.
455  * \exception std::runtime_error On non-square input matrix.
456  * \exception mrpt::math::CExceptionNotDefPos On non-definite-positive
457  * matrix as input.
458  */
459  CholeskyDecomp(const CSparseMatrix& A);
460  CholeskyDecomp(const CholeskyDecomp& A) = delete;
461 
462  CholeskyDecomp& operator=(const CholeskyDecomp&) = delete;
463 
464  /** Destructor */
465  virtual ~CholeskyDecomp();
466 
467  /** Return the L matrix (L*L' = M), as a dense matrix. */
468  inline CMatrixDouble get_L() const
469  {
470  CMatrixDouble L;
471  get_L(L);
472  return L;
473  }
474 
475  /** Return the L matrix (L*L' = M), as a dense matrix. */
476  void get_L(CMatrixDouble& out_L) const;
477 
478  /** Return the vector from a back-substitution step that solves: Ux=b */
479  template <class VECTOR>
480  inline VECTOR backsub(const VECTOR& b) const
481  {
482  VECTOR res;
483  backsub(b, res);
484  return res;
485  }
486 
487  /** Return the vector from a back-substitution step that solves: Ux=b.
488  * Vectors can be Eigen::VectorXd or mrpt::math::CVectorDouble */
489  void backsub(const Eigen::VectorXd& b, Eigen::VectorXd& result_x) const;
490 
491  /** overload for double pointers which assume the user has reserved the
492  * output memory for \a result */
493  void backsub(const double* b, double* result, const size_t N) const;
494 
495  /** Update the Cholesky factorization from an updated vesion of the
496  * original input, square definite-positive sparse matrix.
497  * NOTE: This new matrix MUST HAVE exactly the same sparse structure
498  * than the original one.
499  */
500  void update(const CSparseMatrix& new_SM);
501  };
502  static_assert(
505  "Copy Check");
506 
507  /** @} */
508 
509 }; // end class CSparseMatrix
510 } // namespace math
511 } // namespace mrpt
void setColCount(const size_t nCols)
#define THROW_EXCEPTION(msg)
Definition: exceptions.h:41
const CSparseMatrix * m_originalSM
A const reference to the original matrix used to build this decomposition.
Used in mrpt::math::CSparseMatrix.
Definition: CSparseMatrix.h:36
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...
void construct_from_triplet(const cs &triplet)
Initialization from a triplet "cs", which is first compressed.
Column vector, like Eigen::MatrixX*, but automatically initialized to zeros since construction...
Definition: eigen_frwds.h:44
Auxiliary class to hold the results of a Cholesky factorization of a sparse matrix.
A sparse matrix structure, wrapping T.
Definition: CSparseMatrix.h:97
STL namespace.
void compressFromTriplet()
ONLY for TRIPLET matrices: convert the matrix in a column-compressed form.
void construct_from_mrpt_mat(const MATRIX &C)
Initialization from a dense matrix of any kind existing in MRPT.
GLdouble s
Definition: glext.h:3676
void swap(CSparseMatrix &other)
Fast swap contents with another sparse matrix.
void insert_entry(const size_t row, const size_t col, const double val)
@ Access the matrix, get, set elements, etc.
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.
void get_dense(CMatrixDouble &outMat) const
Return a dense representation of the sparse matrix.
#define ASSERT_(f)
Defines an assertion mechanism.
Definition: exceptions.h:113
A numeric matrix of compile-time fixed size.
CSparseMatrix operator*(const CSparseMatrix &other) const
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...
void construct_from_existing_cs(const cs &sm)
To be called by constructors only, assume previous pointers are trash and overwrite them...
CSparseMatrix(const size_t nRows=0, const size_t nCols=0)
Create an initially empty sparse matrix, in the "triplet" form.
const GLubyte * c
Definition: glext.h:6313
void add_AB(const CSparseMatrix &A, const CSparseMatrix &B)
this = A+B
int val
Definition: mrpt_jpeglib.h:955
void operator=(const CSparseMatrix &other)
Copy operator from another existing object.
GLubyte GLubyte b
Definition: glext.h:6279
#define ASSERTMSG_(f, __ERROR_MSG)
Defines an assertion mechanism.
Definition: exceptions.h:101
void multiply_Ab(const mrpt::math::CVectorDouble &b, mrpt::math::CVectorDouble &out_res) const
out_res = this * b
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:4101
CSparseMatrix operator+(const CSparseMatrix &other) const
void clear(const size_t nRows=1, const size_t nCols=1)
Erase all previous contents and leave the matrix as a "triplet" ROWS x COLS matrix without any nonzer...
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.
CholeskyDecomp & operator=(const CholeskyDecomp &)=delete
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:3705
mrpt::math::CVectorDouble operator*(const mrpt::math::CVectorDouble &other) const
void operator*=(const CSparseMatrix &other)
CholeskyDecomp(const CSparseMatrix &A)
Constructor from a square definite-positive sparse matrix A, which can be use to solve Ax=b The actua...
bool saveToTextFile_dense(const std::string &filName)
save as a dense matrix to a text file
void update(const CSparseMatrix &new_SM)
Update the Cholesky factorization from an updated vesion of the original input, square definite-posit...
GLenum GLenum GLvoid * row
Definition: glext.h:3576
void operator+=(const CSparseMatrix &other)
virtual ~CSparseMatrix()
Destructor.
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 ...
CSparseMatrix transpose() const
bool saveToTextFile_sparse(const std::string &filName)
Save sparse structure to a text file loadable from MATLAB (can be called on triplet or CCS matrices)...
static void cs2dense(const cs &SM, CMatrixDouble &outMat)
Static method to convert a "cs" structure into a dense representation of the sparse matrix...
GLsizei const GLfloat * value
Definition: glext.h:4117
GLuint res
Definition: glext.h:7268
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 nullptr elements.
bool isColumnCompressed() const
Returns true if this sparse matrix is in "column compressed" form.
void copy(const cs *const sm)
Copy the data from an existing "cs" CSparse data structure.
GLsizei GLsizei GLenum GLenum const GLvoid * data
Definition: glext.h:3546
void copy_fast(cs *const sm)
Fast copy the data from an existing "cs" CSparse data structure, copying the pointers and leaving NUL...
typename SparseMatrixMap::const_iterator const_iterator
Const iterator to move through the matrix.
void internal_free_mem()
free buffers (deallocate the memory of the i,p,x buffers)
void memcpy(void *dest, size_t destSize, const void *src, size_t copyCount) noexcept
An OS and compiler independent version of "memcpy".
Definition: os.cpp:356



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: ad3a9d8ae Tue May 1 23:10:22 2018 -0700 at lun oct 28 00:14:14 CET 2019