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



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: 7d5e6d718 Fri Aug 24 01:51:28 2018 +0200 at lun nov 2 08:35:50 CET 2020