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
15
21#include <cstring> // memcpy
22
23// Include CSparse lib headers, either from the system or embedded:
24extern "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
33namespace 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) */
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 */
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 }
227 inline mrpt::math::CVectorDouble operator * (const mrpt::math::CVectorDouble & other) const {
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 }
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 */
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 */
375
376 /** Destructor */
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
Used in mrpt::math::CSparseMatrix.
CExceptionNotDefPos(const std::string &s)
A numeric matrix of compile-time fixed size.
Auxiliary class to hold the results of a Cholesky factorization of a sparse matrix.
void update(const CSparseMatrix &new_SM)
Update the Cholesky factorization from an updated vesion of the original input, square definite-posit...
void backsub(const double *b, double *result, const size_t N) const
CMatrixDouble get_L() const
Return the L matrix (L*L' = M), as a dense matrix.
CholeskyDecomp(const CSparseMatrix &A)
Constructor from a square definite-positive sparse matrix A, which can be use to solve Ax=b The actua...
void backsub(const Eigen::VectorXd &b, Eigen::VectorXd &result_x) const
Return the vector from a back-substitution step that solves: Ux=b.
VECTOR backsub(const VECTOR &b) const
Return the vector from a back-substitution step that solves: Ux=b
void get_L(CMatrixDouble &out_L) const
Return the L matrix (L*L' = M), as a dense matrix.
const CSparseMatrix * m_originalSM
A const reference to the original matrix used to build this decomposition.
A sparse matrix structure, wrapping T.
void add_AB(const CSparseMatrix &A, const CSparseMatrix &B)
this = A+B
void compressFromTriplet()
ONLY for TRIPLET matrices: convert the matrix in a column-compressed form.
void setRowCount(const size_t nRows)
Change the number of rows in the matrix (can't be lower than current size)
void construct_from_triplet(const cs &triplet)
Initialization from a triplet "cs", which is first compressed.
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 isColumnCompressed() const
Returns true if this sparse matrix is in "column compressed" form.
void construct_from_existing_cs(const cs &sm)
To be called by constructors only, assume previous pointers are trash and overwrite them.
void internal_free_mem()
free buffers (deallocate the memory of the i,p,x buffers)
CSparseMatrix transpose() const
CSparseMatrix(const size_t nRows=0, const size_t nCols=0)
Create an initially empty sparse matrix, in the "triplet" form.
void get_dense(CMatrixDouble &outMat) const
Return a dense representation of the sparse matrix.
CSparseMatrix(const CSparseMatrixTemplate< T > &data)
A good way to initialize a sparse matrix from a list of non NULL elements.
void construct_from_mrpt_mat(const MATRIX &C)
Initialization from a dense matrix of any kind existing in MRPT.
void setColCount(const size_t nCols)
virtual ~CSparseMatrix()
Destructor.
void copy_fast(cs *const sm)
Fast copy the data from an existing "cs" CSparse data structure, copying the pointers and leaving NUL...
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(const CSparseMatrix &other)
Copy constructor.
CSparseMatrix(const cs *const sm)
Copy constructor from an existing "cs" CSparse data structure.
void insert_entry(const size_t row, const size_t col, const double val)
@ Access the matrix, get, set elements, etc.
bool saveToTextFile_dense(const std::string &filName)
save as a dense matrix to a text file
CSparseMatrix(const CMatrixFixedNumeric< T, N, M > &MAT)
Constructor from a dense matrix of any kind existing in MRPT, creating a "column-compressed" sparse m...
void copy(const cs *const sm)
Copy the data from an existing "cs" CSparse data structure.
void multiply_AB(const CSparseMatrix &A, const CSparseMatrix &B)
this = A*B
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).
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...
CSparseMatrix(const CMatrixTemplateNumeric< T > &MAT)
Constructor from a dense matrix of any kind existing in MRPT, creating a "column-compressed" sparse m...
void swap(CSparseMatrix &other)
Fast swap contents with another sparse matrix.
static void cs2dense(const cs &SM, CMatrixDouble &outMat)
Static method to convert a "cs" structure into a dense representation of the sparse matrix.
void multiply_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.
A sparse matrix container (with cells of any type), with iterators.
const_iterator begin() const
Returns an iterator which points to the starting point of the matrix.
size_t getRowCount() const
Returns the amount of rows in this matrix.
const_iterator end() const
Returns an iterator which points to the end of the matrix.
size_t getColCount() const
Returns the amount of columns in this matrix.
bool empty() const
Are there no elements set to !=0 ?
SparseMatrixMap::const_iterator const_iterator
Const iterator to move through the matrix.
Column vector, like Eigen::MatrixX*, but automatically initialized to zeros since construction.
Definition types_math.h:65
The base for MRPT-especific exceptions.
Definition exceptions.h:25
CMRPTException(const std::string &s)
Definition exceptions.h:27
The base class of classes that cannot be copied: compile-time errors will be issued on any copy opera...
Definition CUncopiable.h:31
EIGEN_STRONG_INLINE void multiply_AB(const MATRIX1 &A, const MATRIX2 &B)
EIGEN_STRONG_INLINE void multiply_Ab(const OTHERVECTOR1 &vIn, OTHERVECTOR2 &vOut, bool accumToOutput=false) const
#define ASSERT_(f)
#define THROW_EXCEPTION(msg)
#define ASSERTMSG_(f, __ERROR_MSG)
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.



Page generated by Doxygen 1.9.8 for MRPT 1.4.0 SVN: at Thu Dec 14 16:41:50 UTC 2023