Main MRPT website > C++ reference for MRPT 1.5.7
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-2017, 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 "base-precomp.h" // Precompiled headers
11 
13 
14 using std::string;
15 using std::cout;
16 using std::endl;
17 using namespace mrpt;
18 using namespace mrpt::math;
19 using namespace mrpt::utils;
20 
21 
22 /** Copy constructor */
24 {
25  construct_from_existing_cs(other.sparse_matrix);
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 {
32  construct_from_existing_cs(*sm);
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_(sm->nz==-1, "I expected a column-compressed sparse matrix, not a triplet form.")
40 
41  sparse_matrix.m = sm->m;
42  sparse_matrix.n = sm->n;
43  sparse_matrix.nz = sm->nz;
44  sparse_matrix.nzmax = sm->nzmax;
45 
46  // Do it in a different way depending on it being triplet or compressed format:
47  ::memcpy(sparse_matrix.i,sm->i,sizeof(int)*sm->nzmax);
48  ::memcpy(sparse_matrix.p,sm->p,sizeof(int)*(sm->n+1));
49  ::memcpy(sparse_matrix.x,sm->x,sizeof(double)*sm->nzmax);
50 }
51 
52 /** Fast copy the data from an existing "cs" CSparse data structure, copying the pointers and leaving NULLs in the source structure. */
53 void CSparseMatrix::copy_fast(cs * const sm)
54 {
55  // This method will work for either triplet or compressed format:
56 
57  // Free previous contents, if any.
58  internal_free_mem();
59 
60  // Fast copy / Move:
61  sparse_matrix.m = sm->m;
62  sparse_matrix.n = sm->n;
63  sparse_matrix.nz = sm->nz;
64  sparse_matrix.nzmax = sm->nzmax;
65 
66  sparse_matrix.i = sm->i;
67  sparse_matrix.p = sm->p;
68  sparse_matrix.x = sm->x;
69 
70  // Mark source as empty:
71  sm->i=NULL;
72  sm->p=NULL;
73  sm->x=NULL;
74 }
75 
76 /** Fast swap contents with another sparse matrix */
78 {
79  // Fast copy / Move:
80  std::swap( sparse_matrix.m, other.sparse_matrix.m );
81  std::swap( sparse_matrix.n, sparse_matrix.n);
82  std::swap( sparse_matrix.nz, other.sparse_matrix.nz);
83  std::swap( sparse_matrix.nzmax, other.sparse_matrix.nzmax);
84 
85  std::swap( sparse_matrix.i, other.sparse_matrix.i);
86  std::swap( sparse_matrix.p, other.sparse_matrix.p);
87  std::swap( sparse_matrix.x, other.sparse_matrix.x);
88 }
89 
90 
91 
92 // Dtor
94 {
95  internal_free_mem();
96 }
97 
98 /** Erase all previous contents and leave the matrix as a "triplet" 1x1 matrix without any data. */
99 void CSparseMatrix::clear(const size_t nRows, const size_t nCols)
100 {
101  // Free old data:
102  internal_free_mem();
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 internal buffers, by now set to NULL.
128 }
129 
130 // To be called by constructors only, assume previous pointers are trash and overwrite them:
131 // This ONLY allocate the memory, doesn't fill it.
133 {
134  ASSERTMSG_(sm.nz==-1, "I expected a column-compressed sparse matrix, not a triplet form.")
135  sparse_matrix.i = (int*)malloc(sizeof(int)*sm.nzmax);
136  sparse_matrix.p = (int*)malloc(sizeof(int)*(sm.n+1));
137  sparse_matrix.x = (double*)malloc(sizeof(double)*sm.nzmax);
138 }
139 
140 /** Create an initially empty sparse matrix, in the "triplet" form.
141 * Notice that you must call "compressFromTriplet" after populating the matrix and before using the math operatons on this matrix.
142 * The initial size can be later on extended with insert_entry() or setRowCount() & setColCount().
143 */
144 CSparseMatrix::CSparseMatrix(const size_t nRows, const size_t nCols)
145 {
146  sparse_matrix.nzmax = 1;
147  sparse_matrix.m = nRows;
148  sparse_matrix.n = nCols;
149  sparse_matrix.i = (int*)malloc(sizeof(int)*sparse_matrix.nzmax);
150  sparse_matrix.p = (int*)malloc(sizeof(int)*(sparse_matrix.n+1));
151  sparse_matrix.x = (double*)malloc(sizeof(double)*sparse_matrix.nzmax);
152  sparse_matrix.nz = 0;
153 }
154 
155 
156 /** Insert an element into a "cs", return false on error. */
157 void CSparseMatrix::insert_entry(const size_t row, const size_t col, const double val )
158 {
159  if (!isTriplet())
160  THROW_EXCEPTION("insert_entry() is only available for sparse matrix in 'triplet' format.")
161  if (!cs_entry(&sparse_matrix,row,col,val))
162  THROW_EXCEPTION("Error inserting element in sparse matrix (out of mem?)")
163 }
164 
165 /** Copy operator from another existing object */
167 {
168  if (&other==this) return;
169 
170  cs_free(sparse_matrix.i);
171  cs_free(sparse_matrix.p);
172  cs_free(sparse_matrix.x);
173 
174  sparse_matrix.i = (int*)malloc(sizeof(int)*other.sparse_matrix.nzmax);
175  sparse_matrix.p = (int*)malloc(sizeof(int)*(other.sparse_matrix.n+1));
176  sparse_matrix.x = (double*)malloc(sizeof(double)*other.sparse_matrix.nzmax);
177  copy(&other.sparse_matrix);
178 }
179 
180 
182 {
184 
185  cs * sm = cs_add(&(A.sparse_matrix), &(B.sparse_matrix),1,1);
186  ASSERT_(sm)
187  this->copy_fast(sm);
188  cs_spfree(sm);
189 }
190 
192 {
194 
195  cs * sm = cs_multiply(&(A.sparse_matrix), &(B.sparse_matrix));
196  ASSERT_(sm)
197  this->copy_fast(sm);
198  cs_spfree(sm);
199 }
200 
202 {
203  ASSERT_EQUAL_(int(b.size()), int(getColCount()))
204  out_res.resize( getRowCount() );
205  const double * y = &(b[0]);
206  double * x = &(out_res[0]);
207  cs_gaxpy(&sparse_matrix,y,x);
208 }
209 
210 
212 {
213  cs * sm = cs_transpose(&sparse_matrix,1);
214  ASSERT_(sm)
215  CSparseMatrix SM(sm);
216  cs_spfree(sm);
217  return SM;
218 }
219 
220 /** Static method to convert a "cs" structure into a dense representation of the sparse matrix.
221 */
222 void CSparseMatrix::cs2dense(const cs& SM, CMatrixDouble &d_M)
223 {
224  d_M.zeros(SM.m,SM.n);
225  if (SM.nz>=0) // isTriplet ??
226  { // It's in triplet form.
227  for (int idx=0;idx<SM.nz; ++idx)
228  d_M(SM.i[idx],SM.p[idx]) += SM.x[idx]; // += since the convention is that duplicate (i,j) entries add to each other.
229  }
230  else
231  { // Column compressed format:
232  ASSERT_(SM.x) // JL: Could it be NULL and be OK???
233 
234  for (int j = 0 ; j < SM.n ; j++)
235  {
236  const int p0 = SM.p [j];
237  const int p1 = SM.p [j+1];
238  for (int p = p0 ; p < p1 ; p++)
239  d_M(SM.i[p],j) += SM.x[p]; // += since the convention is that duplicate (i,j) entries add to each other.
240  }
241  }
242 }
243 
245 {
246  cs2dense(sparse_matrix, d_M);
247 }
248 
250 {
251  if (!isTriplet())
252  THROW_EXCEPTION("compressFromTriplet(): Matrix is already in column-compressed format.")
253 
254  cs * sm = cs_compress(&this->sparse_matrix);
255  copy_fast(sm);
256  cs_spfree(sm); // This will release just the "cs" structure itself, not the internal buffers, now set to NULL.
257 }
258 
259 
260 /** save as a dense matrix to a text file \return False on any error.
261 */
263 {
264  CMatrixDouble dense;
265  this->get_dense(dense);
266  try
267  {
268  dense.saveToTextFile(filName);
269  return true;
270  }
271  catch(...) { return false; }
272 }
273 
274 // Format:
275 // NUM_ROWS NUM_COLS NUM_NON_ZERO_MAX
276 // row_1 col_1 value_1
277 // row_2 col_2 value_2
279 {
280  FILE *f=fopen(filName.c_str(),"wt");
281  if (!f) return false;
282 
283  // Help notes:
284  fprintf(f,"\
285 # This sparse matrix can be loaded in Octave/Matlab as follows:\n\
286 # D=load('file.txt');\n\
287 # SM=spconvert(D(2:end,:));\n\
288 # or...\n\
289 # m=D(1,1); n=D(1,2); nzmax=D(1,3);\n\
290 # Di=D(2:end,1); Dj=D(2:end,2); Ds=D(2:end,3);\n\
291 # SM=sparse(Di,Dj,Ds, m,n, nzmax);\n\n");
292 
293  // First line:
294  fprintf(f,"%i %i %i\n",sparse_matrix.m, sparse_matrix.n, sparse_matrix.nzmax); // Rows, cols, nzmax
295 
296  // Data lines:
297  if (sparse_matrix.nz>=0) // isTriplet ??
298  { // It's in triplet form.
299  for (int i=0;i<sparse_matrix.nzmax;i++)
300  if (sparse_matrix.x[i]!=0)
301  fprintf(f,"%4i %4i %e\n", 1+sparse_matrix.i[i], 1+sparse_matrix.p[i], sparse_matrix.x[i]);
302  }
303  else
304  { // Column compressed format:
305  ASSERT_(sparse_matrix.x) // JL: Could it be NULL and be OK???
306 
307  for (int j = 0 ; j < sparse_matrix.n ; j++)
308  {
309  const int p0 = sparse_matrix.p [j];
310  const int p1 = sparse_matrix.p [j+1];
311  for (int p = p0 ; p < p1 ; p++)
312  fprintf(f, "%4i %4i %e\n", 1+sparse_matrix.i[p],1+j, sparse_matrix.x[p]);
313  }
314  }
315 
316  fclose(f);
317  return true;
318 }
319 
320 
321 
322 // =============== START OF: CSparseMatrix::CholeskyDecomp inner class ==============================
323 
324 /** Constructor from a square semidefinite-positive sparse matrix.
325 * The actual Cholesky decomposition takes places in this constructor.
326 * \exception std::runtime_error On non-square input matrix.
327 * \exception mrpt::math::CExceptionNotDefPos On non-semidefinite-positive matrix as input.
328 */
330  m_symbolic_structure (NULL),
331  m_numeric_structure (NULL),
332  m_originalSM (&SM)
333 {
334  ASSERT_(SM.getColCount()==SM.getRowCount())
336 
337  // symbolic decomposition:
338  m_symbolic_structure = cs_schol(1 /* order */, &m_originalSM->sparse_matrix );
339 
340  // numeric decomposition:
342 
343  if (!m_numeric_structure)
344  throw mrpt::math::CExceptionNotDefPos("CSparseMatrix::CholeskyDecomp: Not positive definite matrix.");
345 }
346 
347 // Destructor:
349 {
350  cs_nfree(m_numeric_structure);
351  cs_sfree(m_symbolic_structure);
352 }
353 
354 /** Return the L matrix (L*L' = M), as a dense matrix. */
356 {
357  CSparseMatrix::cs2dense(*m_numeric_structure->L, L);
358 }
359 
360 
361 /** Return the vector from a back-substitution step that solves: Ux=b */
363  const Eigen::VectorXd &b,
364  Eigen::VectorXd &sol) const
365 {
366  ASSERT_(b.size()>0)
367  sol.resize(b.size());
368  this->backsub(&b[0],&sol[0],b.size());
369 }
370 
371 /** Return the vector from a back-substitution step that solves: Ux=b */
373  const double *b,
374  double *sol,
375  const size_t N) const
376 {
377  ASSERT_(N>0)
378  std::vector<double> tmp(N);
379 
380  cs_ipvec(m_symbolic_structure->pinv,&b[0],&tmp[0],N); /* tmp = PERMUT*b */
381  //permute con. pivoting
382  cs_lsolve(m_numeric_structure->L,&tmp[0]); /* tmp = L\tmp */
383  cs_ltsolve(m_numeric_structure->L,&tmp[0]); /* tmp = L'\tmp */
384  cs_pvec(m_symbolic_structure->pinv,&tmp[0],&sol[0],N); /* sol = PERMUT'*tmp */
385  //unpermute con. pivoting
386 
387  // Result is now in "sol".
388 }
389 
390 /** Update the Cholesky factorization from an updated vesion of the original input, square definite-positive sparse matrix.
391 * NOTE: This new matrix MUST HAVE exactly the same sparse structure than the original one.
392 */
394 {
395  ASSERTMSG_(m_originalSM->sparse_matrix.nzmax == new_SM.sparse_matrix.nzmax, "New matrix doesn't have the same sparse structure!");
396  ASSERTMSG_(m_originalSM->sparse_matrix.n == new_SM.sparse_matrix.n, "New matrix doesn't have the same sparse structure!");
397 
398  m_originalSM = &new_SM; // Just copy the reference.
399 
400  // Release old data:
401  cs_nfree(m_numeric_structure);
402  m_numeric_structure = NULL;
403 
404  // numeric decomposition:
405  m_numeric_structure = cs_chol(&m_originalSM->sparse_matrix,m_symbolic_structure);
406  if (!m_numeric_structure)
407  throw mrpt::math::CExceptionNotDefPos("CholeskyDecomp::update: Not positive definite matrix.");
408 }
409 
410 // =============== END OF: CSparseMatrix::CholeskyDecomp inner class ==============================
#define ASSERT_EQUAL_(__A, __B)
void BASE_IMPEXP memcpy(void *dest, size_t destSize, const void *src, size_t copyCount) MRPT_NO_THROWS
An OS and compiler independent version of "memcpy".
Definition: os.cpp:358
FILE BASE_IMPEXP * fopen(const char *fileName, const char *mode) MRPT_NO_THROWS
An OS-independent version of fopen.
Definition: os.cpp:255
Classes for serialization, sockets, ini-file manipulation, streams, list of properties-values, timewatch, extensions to STL.
Definition: zip.h:16
size_t getRowCount() const
const CSparseMatrix * m_originalSM
A const reference to the original matrix used to build this decomposition.
int BASE_IMPEXP void BASE_IMPEXP fclose(FILE *f)
An OS-independent version of fclose.
Definition: os.cpp:272
void construct_from_triplet(const cs &triplet)
Initialization from a triplet "cs", which is first compressed.
#define THROW_EXCEPTION(msg)
Column vector, like Eigen::MatrixX*, but automatically initialized to zeros since construction...
Definition: eigen_frwds.h:35
int BASE_IMPEXP fprintf(FILE *fil, const char *format,...) MRPT_NO_THROWS MRPT_printf_format_check(2
An OS-independent version of fprintf.
Definition: os.cpp:412
A sparse matrix structure, wrapping T.
Definition: CSparseMatrix.h:92
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.
Used in mrpt::math::CSparseMatrix.
Definition: CSparseMatrix.h:38
void get_dense(CMatrixDouble &outMat) const
Return a dense representation of the sparse matrix.
EIGEN_STRONG_INLINE size_t getColCount() const
Get number of columns.
Definition: eigen_plugins.h:48
This base provides a set of functions for maths stuff.
Definition: CArrayNumeric.h:19
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.
void add_AB(const CSparseMatrix &A, const CSparseMatrix &B)
this = A+B
int val
Definition: mrpt_jpeglib.h:953
void operator=(const CSparseMatrix &other)
Copy operator from another existing object.
GLubyte GLubyte b
Definition: glext.h:5575
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:3919
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...
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:3533
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...
#define ASSERT_(f)
GLenum GLint GLint y
Definition: glext.h:3516
size_t getColCount() const
EIGEN_STRONG_INLINE size_t getRowCount() const
Get number of rows.
Definition: eigen_plugins.h:46
VECTOR backsub(const VECTOR &b) const
Return the vector from a back-substitution step that solves: Ux=b.
GLenum GLint x
Definition: glext.h:3516
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.
#define ASSERTMSG_(f, __ERROR_MSG)
GLfloat GLfloat p
Definition: glext.h:5587
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)



Page generated by Doxygen 1.8.14 for MRPT 1.5.7 Git: 5902e14cc Wed Apr 24 15:04:01 2019 +0200 at lun oct 28 01:39:17 CET 2019