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



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: 9b18308f3 Mon Nov 18 23:39:25 2019 +0100 at lun nov 18 23:45:12 CET 2019