MRPT  1.9.9
CSparseMatrix.cpp
Go to the documentation of this file.
1 /* +------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | https://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2020, 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 
10 #include "math-precomp.h" // Precompiled headers
11 
13 #include <cstring>
14 #include <iostream>
15 #include <string>
16 
17 using std::cout;
18 using std::endl;
19 using std::string;
20 using namespace mrpt;
21 using namespace mrpt::math;
22 
23 /** Copy constructor */
25 {
27  copy(&other.sparse_matrix);
28 }
29 
30 /** Copy constructor from an existing "cs" CSparse data structure */
31 CSparseMatrix::CSparseMatrix(const cs* const sm)
32 {
34  copy(sm);
35 }
36 
37 /** Copy the data from an existing "cs" CSparse data structure */
38 void CSparseMatrix::copy(const cs* const sm)
39 {
40  ASSERTMSG_(
41  sm->nz == -1,
42  "I expected a column-compressed sparse matrix, not a triplet form.");
43 
44  sparse_matrix.m = sm->m;
45  sparse_matrix.n = sm->n;
46  sparse_matrix.nz = sm->nz;
47  sparse_matrix.nzmax = sm->nzmax;
48 
49  // Do it in a different way depending on it being triplet or compressed
50  // format:
51  std::memcpy(sparse_matrix.i, sm->i, sizeof(int) * sm->nzmax);
52  std::memcpy(sparse_matrix.p, sm->p, sizeof(int) * (sm->n + 1));
53  std::memcpy(sparse_matrix.x, sm->x, sizeof(double) * sm->nzmax);
54 }
55 
56 /** Fast copy the data from an existing "cs" CSparse data structure, copying the
57  * pointers and leaving NULLs in the source structure. */
58 void CSparseMatrix::copy_fast(cs* const sm)
59 {
60  // This method will work for either triplet or compressed format:
61 
62  // Free previous contents, if any.
64 
65  // Fast copy / Move:
66  sparse_matrix.m = sm->m;
67  sparse_matrix.n = sm->n;
68  sparse_matrix.nz = sm->nz;
69  sparse_matrix.nzmax = sm->nzmax;
70 
71  sparse_matrix.i = sm->i;
72  sparse_matrix.p = sm->p;
73  sparse_matrix.x = sm->x;
74 
75  // Mark source as empty:
76  sm->i = nullptr;
77  sm->p = nullptr;
78  sm->x = nullptr;
79 }
80 
81 /** Fast swap contents with another sparse matrix */
83 {
84  // Fast copy / Move:
85  std::swap(sparse_matrix.m, other.sparse_matrix.m);
86  std::swap(sparse_matrix.n, sparse_matrix.n);
87  std::swap(sparse_matrix.nz, other.sparse_matrix.nz);
88  std::swap(sparse_matrix.nzmax, other.sparse_matrix.nzmax);
89 
90  std::swap(sparse_matrix.i, other.sparse_matrix.i);
91  std::swap(sparse_matrix.p, other.sparse_matrix.p);
92  std::swap(sparse_matrix.x, other.sparse_matrix.x);
93 }
94 
95 // Dtor
97 /** Erase all previous contents and leave the matrix as a "triplet" 1x1 matrix
98  * without any data. */
99 void CSparseMatrix::clear(const size_t nRows, const size_t nCols)
100 {
101  // Free old data:
103 
104  // Init as 1x1 triplet:
105  sparse_matrix.nzmax = 1;
106  sparse_matrix.m = nRows;
107  sparse_matrix.n = nCols;
108  sparse_matrix.i = (int*)malloc(sizeof(int) * sparse_matrix.nzmax);
109  sparse_matrix.p = (int*)malloc(sizeof(int) * (sparse_matrix.n + 1));
110  sparse_matrix.x = (double*)malloc(sizeof(double) * sparse_matrix.nzmax);
111  sparse_matrix.nz = 0; // >=0 -> triplet format.
112 }
113 
114 /** free buffers (deallocate the memory of the i,p,x buffers) */
116 {
117  cs_free(sparse_matrix.i);
118  cs_free(sparse_matrix.p);
119  cs_free(sparse_matrix.x);
120 }
121 
122 /** Initialization from a triplet "cs", which is first compressed */
124 {
125  cs* sm = cs_compress(&triplet);
126  copy_fast(sm);
127  cs_spfree(sm); // This will release just the "cs" structure itself, not the
128  // internal buffers, by now set to NULL.
129 }
130 
131 // To be called by constructors only, assume previous pointers are trash and
132 // overwrite them:
133 // This ONLY allocate the memory, doesn't fill it.
135 {
136  ASSERTMSG_(
137  sm.nz == -1,
138  "I expected a column-compressed sparse matrix, not a triplet form.");
139  sparse_matrix.i = (int*)malloc(sizeof(int) * sm.nzmax);
140  sparse_matrix.p = (int*)malloc(sizeof(int) * (sm.n + 1));
141  sparse_matrix.x = (double*)malloc(sizeof(double) * sm.nzmax);
142 }
143 
144 /** Create an initially empty sparse matrix, in the "triplet" form.
145  * Notice that you must call "compressFromTriplet" after populating the matrix
146  * and before using the math operatons on this matrix.
147  * The initial size can be later on extended with insert_entry() or
148  * setRowCount() & setColCount().
149  */
150 CSparseMatrix::CSparseMatrix(const size_t nRows, const size_t nCols)
151 {
152  sparse_matrix.nzmax = 1;
153  sparse_matrix.m = nRows;
154  sparse_matrix.n = nCols;
155  sparse_matrix.i = (int*)malloc(sizeof(int) * sparse_matrix.nzmax);
156  sparse_matrix.p = (int*)malloc(sizeof(int) * (sparse_matrix.n + 1));
157  sparse_matrix.x = (double*)malloc(sizeof(double) * sparse_matrix.nzmax);
158  sparse_matrix.nz = 0;
159 }
160 
161 /** Insert an element into a "cs", return false on error. */
163  const size_t row, const size_t col, const double val)
164 {
165  if (!isTriplet())
167  "insert_entry() is only available for sparse matrix in 'triplet' "
168  "format.");
169  if (!cs_entry(&sparse_matrix, row, col, val))
171  "Error inserting element in sparse matrix (out of mem?)");
172 }
173 
174 /** Copy operator from another existing object */
176 {
177  if (&other == this) return *this;
178 
179  cs_free(sparse_matrix.i);
180  cs_free(sparse_matrix.p);
181  cs_free(sparse_matrix.x);
182 
183  sparse_matrix.i = (int*)malloc(sizeof(int) * other.sparse_matrix.nzmax);
184  sparse_matrix.p = (int*)malloc(sizeof(int) * (other.sparse_matrix.n + 1));
185  sparse_matrix.x =
186  (double*)malloc(sizeof(double) * other.sparse_matrix.nzmax);
187  copy(&other.sparse_matrix);
188  return *this;
189 }
190 
192 {
193  ASSERT_(A.cols() == B.cols() && A.rows() == B.rows());
194 
195  cs* sm = cs_add(&(A.sparse_matrix), &(B.sparse_matrix), 1, 1);
196  ASSERT_(sm);
197  this->copy_fast(sm);
198  cs_spfree(sm);
199 }
200 
202 {
203  ASSERT_(A.cols() == B.rows());
204 
205  cs* sm = cs_multiply(&(A.sparse_matrix), &(B.sparse_matrix));
206  ASSERT_(sm);
207  this->copy_fast(sm);
208  cs_spfree(sm);
209 }
210 
212  const mrpt::math::CVectorDouble& b,
213  mrpt::math::CVectorDouble& out_res) const
214 {
215  ASSERT_EQUAL_(int(b.size()), int(cols()));
216  out_res.resize(rows());
217  const double* y = &(b[0]);
218  double* x = &(out_res[0]);
219  cs_gaxpy(&sparse_matrix, y, x);
220 }
221 
223 {
224  cs* sm = cs_transpose(&sparse_matrix, 1);
225  ASSERT_(sm);
226  CSparseMatrix SM(sm);
227  cs_spfree(sm);
228  return SM;
229 }
230 
231 /** Static method to convert a "cs" structure into a dense representation of the
232  * sparse matrix.
233  */
234 void CSparseMatrix::cs2dense(const cs& SM, CMatrixDouble& d_M)
235 {
236  d_M.setZero(SM.m, SM.n);
237  if (SM.nz >= 0) // isTriplet ??
238  { // It's in triplet form.
239  for (int idx = 0; idx < SM.nz; ++idx)
240  d_M(SM.i[idx], SM.p[idx]) +=
241  SM.x[idx]; // += since the convention is that duplicate (i,j)
242  // entries add to each other.
243  }
244  else
245  { // Column compressed format:
246  ASSERT_(SM.x); // JL: Could it be nullptr and be OK???
247 
248  for (int j = 0; j < SM.n; j++)
249  {
250  const int p0 = SM.p[j];
251  const int p1 = SM.p[j + 1];
252  for (int p = p0; p < p1; p++)
253  d_M(SM.i[p], j) += SM.x[p]; // += since the convention is that
254  // duplicate (i,j) entries add to
255  // each other.
256  }
257  }
258 }
259 
261 {
262  cs2dense(sparse_matrix, d_M);
263 }
264 
266 {
267  if (!isTriplet())
269  "compressFromTriplet(): Matrix is already in column-compressed "
270  "format.");
271 
272  cs* sm = cs_compress(&this->sparse_matrix);
273  copy_fast(sm);
274  cs_spfree(sm); // This will release just the "cs" structure itself, not the
275  // internal buffers, now set to NULL.
276 }
277 
278 /** save as a dense matrix to a text file \return False on any error.
279  */
280 bool CSparseMatrix::saveToTextFile_dense(const std::string& filName)
281 {
282  CMatrixDouble dense;
283  this->get_dense(dense);
284  try
285  {
286  dense.saveToTextFile(filName);
287  return true;
288  }
289  catch (...)
290  {
291  return false;
292  }
293 }
294 
295 // Format:
296 // NUM_ROWS NUM_COLS NUM_NON_ZERO_MAX
297 // row_1 col_1 value_1
298 // row_2 col_2 value_2
299 bool CSparseMatrix::saveToTextFile_sparse(const std::string& filName)
300 {
301  FILE* f = fopen(filName.c_str(), "wt");
302  if (!f) return false;
303 
304  // Help notes:
305  fprintf(
306  f,
307  "\
308 # This sparse matrix can be loaded in Octave/Matlab as follows:\n\
309 # D=load('file.txt');\n\
310 # SM=spconvert(D(2:end,:));\n\
311 # or...\n\
312 # m=D(1,1); n=D(1,2); nzmax=D(1,3);\n\
313 # Di=D(2:end,1); Dj=D(2:end,2); Ds=D(2:end,3);\n\
314 # SM=sparse(Di,Dj,Ds, m,n, nzmax);\n\n");
315 
316  // First line:
317  fprintf(
318  f, "%i %i %i\n", sparse_matrix.m, sparse_matrix.n,
319  sparse_matrix.nzmax); // Rows, cols, nzmax
320 
321  // Data lines:
322  if (sparse_matrix.nz >= 0) // isTriplet ??
323  { // It's in triplet form.
324  for (int i = 0; i < sparse_matrix.nzmax; i++)
325  if (sparse_matrix.x[i] != 0)
326  fprintf(
327  f, "%4i %4i %e\n", 1 + sparse_matrix.i[i],
328  1 + sparse_matrix.p[i], sparse_matrix.x[i]);
329  }
330  else
331  { // Column compressed format:
332  ASSERT_(sparse_matrix.x); // JL: Could it be nullptr and be OK???
333 
334  for (int j = 0; j < sparse_matrix.n; j++)
335  {
336  const int p0 = sparse_matrix.p[j];
337  const int p1 = sparse_matrix.p[j + 1];
338  for (int p = p0; p < p1; p++)
339  fprintf(
340  f, "%4i %4i %e\n", 1 + sparse_matrix.i[p], 1 + j,
341  sparse_matrix.x[p]);
342  }
343  }
344 
345  fclose(f);
346  return true;
347 }
348 
349 // =============== START OF: CSparseMatrix::CholeskyDecomp inner class
350 // ==============================
351 
352 /** Constructor from a square semidefinite-positive sparse matrix.
353  * The actual Cholesky decomposition takes places in this constructor.
354  * \exception std::runtime_error On non-square input matrix.
355  * \exception mrpt::math::CExceptionNotDefPos On non-semidefinite-positive
356  * matrix as input.
357  */
359  : m_symbolic_structure(nullptr),
360  m_numeric_structure(nullptr),
361  m_originalSM(&SM)
362 {
363  ASSERT_(SM.cols() == SM.rows());
365 
366  // symbolic decomposition:
368  cs_schol(1 /* order */, &m_originalSM->sparse_matrix);
369 
370  // numeric decomposition:
373 
374  if (!m_numeric_structure)
376  "CSparseMatrix::CholeskyDecomp: Not positive definite matrix.");
377 }
378 
379 // Destructor:
381 {
382  cs_nfree(m_numeric_structure);
383  cs_sfree(m_symbolic_structure);
384 }
385 
386 /** Return the L matrix (L*L' = M), as a dense matrix. */
388 {
389  CSparseMatrix::cs2dense(*m_numeric_structure->L, L);
390 }
391 
392 /** Return the vector from a back-substitution step that solves: Ux=b */
394  const CVectorDouble& b, CVectorDouble& sol) const
395 {
396  ASSERT_(b.size() > 0);
397  sol.resize(b.size());
398  this->backsub(&b[0], &sol[0], b.size());
399 }
400 
401 /** Return the vector from a back-substitution step that solves: Ux=b */
403  const double* b, double* sol, const size_t N) const
404 {
405  ASSERT_(N > 0);
406  std::vector<double> tmp(N);
407 
408  cs_ipvec(
409  m_symbolic_structure->pinv, &b[0], &tmp[0], N); /* tmp = PERMUT*b */
410  // permute con. pivoting
411  cs_lsolve(m_numeric_structure->L, &tmp[0]); /* tmp = L\tmp */
412  cs_ltsolve(m_numeric_structure->L, &tmp[0]); /* tmp = L'\tmp */
413  cs_pvec(
414  m_symbolic_structure->pinv, &tmp[0], &sol[0],
415  N); /* sol = PERMUT'*tmp */
416  // unpermute con. pivoting
417 
418  // Result is now in "sol".
419 }
420 
421 /** Update the Cholesky factorization from an updated vesion of the original
422  * input, square definite-positive sparse matrix.
423  * NOTE: This new matrix MUST HAVE exactly the same sparse structure than the
424  * original one.
425  */
427 {
428  ASSERTMSG_(
429  m_originalSM->sparse_matrix.nzmax == new_SM.sparse_matrix.nzmax,
430  "New matrix doesn't have the same sparse structure!");
431  ASSERTMSG_(
432  m_originalSM->sparse_matrix.n == new_SM.sparse_matrix.n,
433  "New matrix doesn't have the same sparse structure!");
434 
435  m_originalSM = &new_SM; // Just copy the reference.
436 
437  // Release old data:
438  cs_nfree(m_numeric_structure);
439  m_numeric_structure = nullptr;
440 
441  // numeric decomposition:
442  m_numeric_structure =
443  cs_chol(&m_originalSM->sparse_matrix, m_symbolic_structure);
444  if (!m_numeric_structure)
446  "CholeskyDecomp::update: Not positive definite matrix.");
447 }
448 
449 // =============== END OF: CSparseMatrix::CholeskyDecomp inner class
450 // ==============================
#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
int void fclose(FILE *f)
An OS-independent version of fclose.
Definition: os.cpp:275
void construct_from_triplet(const cs &triplet)
Initialization from a triplet "cs", which is first compressed.
size_type size() const
Get a 2-vector with [NROWS NCOLS] (as in MATLAB command size(x))
A sparse matrix structure, wrapping T.
Definition: CSparseMatrix.h:98
void compressFromTriplet()
ONLY for TRIPLET matrices: convert the matrix in a column-compressed form.
void swap(CSparseMatrix &other)
Fast swap contents with another sparse matrix.
void saveToTextFile(const std::string &file, mrpt::math::TMatrixTextFileFormat fileFormat=mrpt::math::MATRIX_FORMAT_ENG, bool appendMRPTHeader=false, const std::string &userHeader=std::string()) const
Saves the vector/matrix to a file compatible with MATLAB/Octave text format.
void insert_entry(const size_t row, const size_t col, const double val)
@ Access the matrix, get, set elements, etc.
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
This base provides a set of functions for maths stuff.
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...
#define ASSERT_EQUAL_(__A, __B)
Assert comparing two values, reporting their actual values upon failure.
Definition: exceptions.h:137
CSparseMatrix(const size_t nRows=0, const size_t nCols=0)
Create an initially empty sparse matrix, in the "triplet" form.
void add_AB(const CSparseMatrix &A, const CSparseMatrix &B)
this = A+B
int val
Definition: mrpt_jpeglib.h:957
#define ASSERTMSG_(f, __ERROR_MSG)
Defines an assertion mechanism.
Definition: exceptions.h:108
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
int fprintf(FILE *fil, const char *format,...) noexcept MRPT_printf_format_check(2
An OS-independent version of fprintf.
Definition: os.cpp:408
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 & operator=(const CSparseMatrix &other)
Copy operator from another existing object.
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...
virtual ~CSparseMatrix()
Destructor.
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...
FILE * fopen(const char *fileName, const char *mode) noexcept
An OS-independent version of fopen.
Definition: os.cpp:257
VECTOR backsub(const VECTOR &b) const
Return the vector from a back-substitution step that solves: Ux=b.
void resize(std::size_t N, bool zeroNewElements=false)
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.
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".



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: c7a3bec24 Sun Mar 29 18:33:13 2020 +0200 at dom mar 29 18:50:38 CEST 2020