Main MRPT website > C++ reference
MRPT logo
CMatrixTemplate.h
Go to the documentation of this file.
1 /* +---------------------------------------------------------------------------+
2  | The Mobile Robot Programming Toolkit (MRPT) C++ library |
3  | |
4  | http://www.mrpt.org/ |
5  | |
6  | Copyright (C) 2005-2012 University of Malaga |
7  | |
8  | This software was written by the Machine Perception and Intelligent |
9  | Robotics Lab, University of Malaga (Spain). |
10  | Contact: Jose-Luis Blanco <jlblanco@ctima.uma.es> |
11  | |
12  | This file is part of the MRPT project. |
13  | |
14  | MRPT is free software: you can redistribute it and/or modify |
15  | it under the terms of the GNU General Public License as published by |
16  | the Free Software Foundation, either version 3 of the License, or |
17  | (at your option) any later version. |
18  | |
19  | MRPT is distributed in the hope that it will be useful, |
20  | but WITHOUT ANY WARRANTY; without even the implied warranty of |
21  | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the |
22  | GNU General Public License for more details. |
23  | |
24  | You should have received a copy of the GNU General Public License |
25  | along with MRPT. If not, see <http://www.gnu.org/licenses/>. |
26  | |
27  +---------------------------------------------------------------------------+ */
28 #ifndef CMatrixTemplate_H
29 #define CMatrixTemplate_H
30 
31 #include <mrpt/utils/utils_defs.h>
32 #include <mrpt/system/memory.h>
33 #include <mrpt/system/datetime.h>
34 
35 #include <mrpt/math/math_frwds.h> // Fordward declarations
36 #include <mrpt/math/matrix_adaptors.h>
37 #include <mrpt/math/CArray.h>
38 
39 namespace mrpt
40 {
41  namespace math
42  {
43 
44  /** This template class provides the basic functionality for a general 2D any-size, resizable container of numerical or non-numerical elements.
45  * NOTES:
46  * - This class is not serializable since it is a template. For using serialization, see mrpt::math::CMatrixNumeric
47  * - First row or column index is "0".
48  * - This class includes range checks with ASSERT_() if compiling with "_DEBUG" or "MRPT_ALWAYS_CHECKS_DEBUG_MATRICES=1".
49  * - Please DO NOT use as template class type any other class. It can be safely used the following types:
50  * - Elemental types (int,char,float,doble,...)
51  * - Data struct (Not classes!)
52  * - Any kind of pointers (user is responsible for allocating and freeing the memory addressed by pointers).
53  *
54  * \note Memory blocks for each row are 16-bytes aligned (since MRPT 0.7.0).
55  * \note For a complete introduction to Matrices and vectors in MRPT, see: http://www.mrpt.org/Matrices_vectors_arrays_and_Linear_Algebra_MRPT_and_Eigen_classes
56  * \sa CMatrixTemplateNumeric
57  * \ingroup mrpt_base_grp
58  */
59  template <class T>
60  class CMatrixTemplate
61  {
62  public:
63  // type definitions
64  typedef T value_type; //!< The type of the matrix elements
65  typedef T& reference;
66  typedef const T& const_reference;
67  typedef std::size_t size_type;
68  typedef std::ptrdiff_t difference_type;
69 
70 
71  protected:
72  T **m_Val;
73  size_t m_Rows, m_Cols;
74 
75  /** Internal use only: It reallocs the memory for the 2D matrix, maintaining the previous contents if posible.
76  */
77  void realloc(size_t row, size_t col, bool newElementsToZero = false)
78  {
79  if (row!=m_Rows || col!=m_Cols || m_Val==NULL)
80  {
81  size_t r;
82  bool doZeroColumns = newElementsToZero && (col>m_Cols);
83  size_t sizeZeroColumns = sizeof(T)*(col-m_Cols);
84 
85  // If we are reducing rows, free that memory:
86  for (r=row;r<m_Rows;r++)
87  mrpt::system::os::aligned_free( m_Val[r] );
88 
89  // Realloc the vector of pointers:
90  if (!row)
91  { mrpt::system::os::aligned_free(m_Val); m_Val=NULL; }
92  else m_Val = static_cast<T**> (mrpt::system::os::aligned_realloc(m_Val, sizeof(T*) * row, 16 ) );
93 
94  // How many new rows/cols?
95  size_t row_size = col * sizeof(T);
96 
97  // Alloc new ROW pointers & resize previously existing rows, as required:
98  for (r=0;r<row;r++)
99  {
100  if (r<m_Rows)
101  {
102  // This was an existing row: Resize the memory:
103  m_Val[r] = static_cast<T*> (mrpt::system::os::aligned_realloc( m_Val[r], row_size, 16));
104 
105  if (doZeroColumns)
106  {
107  // Fill with zeros:
108  ::memset(&m_Val[r][m_Cols],0,sizeZeroColumns);
109  }
110  }
111  else
112  {
113  // This is a new row, alloc the memory for the first time:
114  m_Val[r] = static_cast<T*> ( mrpt::system::os::aligned_calloc( row_size, 16 ));
115  }
116  }
117  // Done!
118  m_Rows = row;
119  m_Cols = col;
120  }
121  }
122 
123  public:
124  /**
125  * Checks whether the rows [r-N,r+N] and the columns [c-N,c+N] are present in the matrix.
126  */
127  template<size_t N> inline void ASSERT_ENOUGHROOM(size_t r,size_t c) const {
128  #if defined(_DEBUG)||(MRPT_ALWAYS_CHECKS_DEBUG_MATRICES)
129  ASSERT_((r>=N)&&(r+N<getRowCount())&&(c>=N)&&(c+N<getColCount()));
130  #endif
131  }
132  /*! Fill all the elements with a given value (Note: named "fillAll" since "fill" will be used by child classes) */
133  void fillAll(const T &val) {
134  for (size_t r=0;r<m_Rows;r++)
135  for (size_t c=0;c<m_Cols;c++)
136  m_Val[r][c]=val;
137  }
138 
139  /** Swap with another matrix very efficiently (just swaps a pointer and two integer values). */
140  inline void swap(CMatrixTemplate<T> &o)
141  {
142  std::swap(m_Val, o.m_Val );
143  std::swap(m_Rows, o.m_Rows );
144  std::swap(m_Cols, o.m_Cols );
145  }
146 
147  /** Constructors */
148  CMatrixTemplate (const CMatrixTemplate& m) : m_Val(NULL),m_Rows(0),m_Cols(0)
149  {
150  (*this) = m;
151  }
152 
153  CMatrixTemplate (size_t row = 1, size_t col = 1) : m_Val(NULL),m_Rows(0),m_Cols(0)
154  {
155  realloc(row,col);
156  }
157 
158  /** Copy constructor & crop from another matrix
159  */
160  CMatrixTemplate (const CMatrixTemplate& m, const size_t cropRowCount, const size_t cropColCount) : m_Val(NULL),m_Rows(0),m_Cols(0)
161  {
162  ASSERT_(m.m_Rows>=cropRowCount)
163  ASSERT_(m.m_Cols>=cropColCount)
164  realloc( cropRowCount, cropColCount );
165  for (size_t i=0; i < m_Rows; i++)
166  for (size_t j=0; j < m_Cols; j++)
167  m_Val[i][j] = m.m_Val[i][j];
168  }
169 
170  /** Constructor from a given size and a C array. The array length must match cols x row.
171  * \code
172  * const double numbers[] = {
173  * 1,2,3,
174  * 4,5,6 };
175  * CMatrixDouble M(3,2, numbers);
176  * \endcode
177  */
178  template <typename V, size_t N>
179  CMatrixTemplate (size_t row, size_t col, V (&theArray)[N] ) : m_Val(NULL),m_Rows(0),m_Cols(0)
180  {
181  MRPT_COMPILE_TIME_ASSERT(N!=0)
182  realloc(row,col);
183  if (m_Rows*m_Cols != N) THROW_EXCEPTION(format("Mismatch between matrix size %lu x %lu and array of length %lu",static_cast<long unsigned>(m_Rows),static_cast<long unsigned>(m_Cols),static_cast<long unsigned>(N)))
184  size_t idx=0;
185  for (size_t i=0; i < m_Rows; i++)
186  for (size_t j=0; j < m_Cols; j++)
187  m_Val[i][j] = static_cast<T>(theArray[idx++]);
188  }
189 
190  /** Constructor from a given size and a STL container (std::vector, std::list,...) with the initial values. The vector length must match cols x row.
191  */
192  template <typename V>
193  CMatrixTemplate(size_t row, size_t col, const V &theVector ) : m_Val(NULL),m_Rows(0),m_Cols(0)
194  {
195  const size_t N = theVector.size();
196  realloc(row,col);
197  if (m_Rows*m_Cols != N) THROW_EXCEPTION(format("Mismatch between matrix size %lu x %lu and array of length %lu",static_cast<long unsigned>(m_Rows),static_cast<long unsigned>(m_Cols),static_cast<long unsigned>(N)))
198  typename V::const_iterator it = theVector.begin();
199  for (size_t i=0; i < m_Rows; i++)
200  for (size_t j=0; j < m_Cols; j++)
201  m_Val[i][j] = static_cast<T>( *(it++) );
202  }
203 
204  /** Destructor */
205  virtual ~CMatrixTemplate() { realloc(0,0); }
206 
207  /** Assignment operator from another matrix */
208  CMatrixTemplate& operator = (const CMatrixTemplate& m)
209  {
210  realloc( m.m_Rows, m.m_Cols );
211  for (size_t i=0; i < m_Rows; i++)
212  for (size_t j=0; j < m_Cols; j++)
213  m_Val[i][j] = m.m_Val[i][j];
214  return *this;
215  }
216 
217  /** Assignment operator for initializing from a C array (The matrix must be set to the correct size before invoking this asignament)
218  * \code
219  * CMatrixDouble M(3,2);
220  * const double numbers[] = {
221  * 1,2,3,
222  * 4,5,6 };
223  * M = numbers;
224  * \endcode
225  * Refer also to the constructor with initialization data CMatrixTemplate::CMatrixTemplate
226  */
227  template <typename V, size_t N>
228  CMatrixTemplate& operator = (V (&theArray)[N] )
229  {
230  MRPT_COMPILE_TIME_ASSERT(N!=0)
231  if (m_Rows*m_Cols != N)
232  {
233  THROW_EXCEPTION(format("Mismatch between matrix size %lu x %lu and array of length %lu",m_Rows,m_Cols,N))
234  }
235  size_t idx=0;
236  for (size_t i=0; i < m_Rows; i++)
237  for (size_t j=0; j < m_Cols; j++)
238  m_Val[i][j] = static_cast<T>(theArray[idx++]);
239  return *this;
240  }
241 
242  /** Number of rows in the matrix
243  * \sa getRowCount, getColCount, nr, nc
244  */
245  inline size_t getRowCount() const { return m_Rows; }
246 
247  /** Number of columns in the matrix
248  * \sa getRowCount, getColCount, nr, nc
249  */
250  inline size_t getColCount() const { return m_Cols; }
251 
252  /** Get a 2-vector with [NROWS NCOLS] (as in MATLAB command size(x)) */
253  inline CMatrixTemplateSize size() const
254  {
255  CMatrixTemplateSize dims;
256  dims[0]=m_Rows;
257  dims[1]=m_Cols;
258  return dims;
259  }
260 
261  /** Changes the size of matrix, maintaining the previous contents. */
262  void setSize(size_t row, size_t col,bool zeroNewElements=false)
263  {
264  realloc(row,col,zeroNewElements);
265  }
266 
267  /** This method just checks has no effects in this class, but raises an exception if the expected size does not match */
268  inline void resize(const CMatrixTemplateSize &siz,bool zeroNewElements=false)
269  {
270  setSize(siz[0],siz[1],zeroNewElements);
271  }
272 
273  /** Subscript operator to get/set individual elements
274  */
275  inline T& operator () (size_t row, size_t col)
276  {
277  #if defined(_DEBUG) || (MRPT_ALWAYS_CHECKS_DEBUG_MATRICES)
278  if (row >= m_Rows || col >= m_Cols)
279  THROW_EXCEPTION( format("Indexes (%lu,%lu) out of range. Matrix is %lux%lu",static_cast<unsigned long>(row),static_cast<unsigned long>(col),static_cast<unsigned long>(m_Rows),static_cast<unsigned long>(m_Cols)) );
280  #endif
281  return m_Val[row][col];
282  }
283 
284  /** Subscript operator to get individual elements
285  */
286  inline const T &operator () (size_t row, size_t col) const
287  {
288  #if defined(_DEBUG) || (MRPT_ALWAYS_CHECKS_DEBUG_MATRICES)
289  if (row >= m_Rows || col >= m_Cols)
290  THROW_EXCEPTION( format("Indexes (%lu,%lu) out of range. Matrix is %lux%lu",static_cast<unsigned long>(row),static_cast<unsigned long>(col),static_cast<unsigned long>(m_Rows),static_cast<unsigned long>(m_Cols)) );
291  #endif
292  return m_Val[row][col];
293  }
294 
295  /** Subscript operator to get/set an individual element from a row or column matrix.
296  * \exception std::exception If the object is not a column or row matrix.
297  */
298  inline T& operator () (size_t ith)
299  {
300  #if defined(_DEBUG) || (MRPT_ALWAYS_CHECKS_DEBUG_MATRICES)
301  ASSERT_(m_Rows==1 || m_Cols==1);
302  #endif
303  if (m_Rows==1)
304  {
305  // A row matrix:
306  #if defined(_DEBUG) || (MRPT_ALWAYS_CHECKS_DEBUG_MATRICES)
307  if (ith >= m_Cols)
308  THROW_EXCEPTION_CUSTOM_MSG1( "Index %u out of range!",static_cast<unsigned>(ith) );
309  #endif
310  return m_Val[0][ith];
311  }
312  else
313  {
314  // A columns matrix:
315  #if defined(_DEBUG) || (MRPT_ALWAYS_CHECKS_DEBUG_MATRICES)
316  if (ith >= m_Rows)
317  THROW_EXCEPTION_CUSTOM_MSG1( "Index %u out of range!",static_cast<unsigned>(ith) );
318  #endif
319  return m_Val[ith][0];
320  }
321  }
322 
323  /** Subscript operator to get/set an individual element from a row or column matrix.
324  * \exception std::exception If the object is not a column or row matrix.
325  */
326  inline T operator () (size_t ith) const
327  {
328  #if defined(_DEBUG) || (MRPT_ALWAYS_CHECKS_DEBUG_MATRICES)
329  ASSERT_(m_Rows==1 || m_Cols==1);
330  #endif
331  if (m_Rows==1)
332  {
333  // A row matrix:
334  #if defined(_DEBUG) || (MRPT_ALWAYS_CHECKS_DEBUG_MATRICES)
335  if (ith >= m_Cols)
336  THROW_EXCEPTION_CUSTOM_MSG1( "Index %u out of range!",static_cast<unsigned>(ith) );
337  #endif
338  return m_Val[0][ith];
339  }
340  else
341  {
342  // A columns matrix:
343  #if defined(_DEBUG) || (MRPT_ALWAYS_CHECKS_DEBUG_MATRICES)
344  if (ith >= m_Rows)
345  THROW_EXCEPTION_CUSTOM_MSG1( "Index %u out of range!",static_cast<unsigned>(ith) );
346  #endif
347  return m_Val[ith][0];
348  }
349  }
350 
351  /** Fast but unsafe method to write a value in the matrix
352  */
353  inline void set_unsafe(size_t row, size_t col,const T &v)
354  {
355  #ifdef _DEBUG
356  if (row >= m_Rows || col >= m_Cols)
357  THROW_EXCEPTION( format("Indexes (%lu,%lu) out of range. Matrix is %lux%lu",static_cast<unsigned long>(row),static_cast<unsigned long>(col),static_cast<unsigned long>(m_Rows),static_cast<unsigned long>(m_Cols)) );
358  #endif
359  m_Val[row][col] = v;
360  }
361 
362  /** Fast but unsafe method to read a value from the matrix
363  */
364  inline const T &get_unsafe(size_t row, size_t col) const
365  {
366  #ifdef _DEBUG
367  if (row >= m_Rows || col >= m_Cols)
368  THROW_EXCEPTION( format("Indexes (%lu,%lu) out of range. Matrix is %lux%lu",static_cast<unsigned long>(row),static_cast<unsigned long>(col),static_cast<unsigned long>(m_Rows),static_cast<unsigned long>(m_Cols)) );
369  #endif
370  return m_Val[row][col];
371  }
372 
373  /** Fast but unsafe method to get a reference from the matrix
374  */
375  inline T &get_unsafe(size_t row,size_t col)
376  {
377  #ifdef _DEBUG
378  if (row >= m_Rows || col >= m_Cols)
379  THROW_EXCEPTION( format("Indexes (%lu,%lu) out of range. Matrix is %lux%lu",static_cast<unsigned long>(row),static_cast<unsigned long>(col),static_cast<unsigned long>(m_Rows),static_cast<unsigned long>(m_Cols)) );
380  #endif
381  return m_Val[row][col];
382  }
383 
384  /** Fast but unsafe method to obtain a pointer to a given row of the matrix (Use only in time critical applications)
385  */
386  inline T* get_unsafe_row(size_t row)
387  {
388  #ifdef _DEBUG
389  if (row >= m_Rows)
390  THROW_EXCEPTION( format("Row index %"PRIuPTR" out of range. Matrix is %"PRIuPTR"x%"PRIuPTR,static_cast<unsigned long>(row),static_cast<unsigned long>(m_Rows),static_cast<unsigned long>(m_Cols)) );
391  #endif
392  return m_Val[row];
393  }
394 
395  /** Fast but unsafe method to obtain a pointer to a given row of the matrix (Use only in critical applications)
396  */
397  inline const T* get_unsafe_row(size_t row) const {
398  return m_Val[row];
399  }
400 
401  /** Subscript operator to get a submatrix
402  */
403  inline CMatrixTemplate<T> operator() (const size_t row1,const size_t row2,const size_t col1,const size_t col2) const {
404  CMatrixTemplate<T> val(0,0);
405  extractSubmatrix(row1,row2,col1,col2,val);
406  return val;
407  }
408 
409  /** Get a submatrix, given its bounds
410  * \sa extractSubmatrixSymmetricalBlocks
411  */
412  void extractSubmatrix(const size_t row1,const size_t row2,const size_t col1,const size_t col2,CMatrixTemplate<T> &out) const
413  {
414  size_t nrows=row2-row1+1;
415  size_t ncols=col2-col1+1;
416  if (nrows<=0||ncols<=0) {
417  out.realloc(0,0);
418  return;
419  }
420  if (row1<0||row2>=m_Rows||col1<0||col2>=m_Cols) THROW_EXCEPTION("Indices out of range!");
421  out.realloc(nrows,ncols);
422  for (size_t i=0;i<nrows;i++) for (size_t j=0;j<ncols;j++) out.m_Val[i][j]=m_Val[i+row1][j+col1];
423  }
424  /// @overload
425  template <class Derived>
426  void extractSubmatrix(const size_t row1,const size_t row2,const size_t col1,const size_t col2,Eigen::MatrixBase<Derived> &out) const
427  {
428  size_t nrows=row2-row1+1;
429  size_t ncols=col2-col1+1;
430  if (nrows<=0||ncols<=0) {
431  out = typename Eigen::MatrixBase<Derived>::PlainObject();
432  return;
433  }
434  if (row1<0||row2>=m_Rows||col1<0||col2>=m_Cols) THROW_EXCEPTION("Indices out of range!");
435  out.resize(nrows,ncols);
436  for (size_t i=0;i<nrows;i++) for (size_t j=0;j<ncols;j++) out.coeffRef(i,j)=m_Val[i+row1][j+col1];
437  }
438 
439 
440  /** Gets a series of contiguous rows.
441  * \exception std::logic_error On index out of bounds
442  * \sa extractRow
443  * \sa extractColumns
444  */
445  inline void extractRows(size_t firstRow,size_t lastRow,CMatrixTemplate<T> &out) const {
446  out.setSize(lastRow-firstRow+1,m_Cols);
447  detail::extractMatrix(*this,firstRow,0,out);
448  }
449 
450  /** Gets a series of contiguous columns.
451  * \exception std::logic_error On index out of bounds
452  * \sa extractColumn
453  * \sa extractRows
454  */
455  inline void extractColumns(size_t firstCol,size_t lastCol,CMatrixTemplate<T> &out) const {
456  out.setSize(m_Rows,lastCol-firstCol+1);
457  detail::extractMatrix(*this,0,firstCol,out);
458  }
459 
460  /** Returns a given column to a vector (without modifying the matrix)
461  * \exception std::exception On index out of bounds
462  */
463  void extractCol(size_t nCol, std::vector<T> &out, int startingRow = 0) const
464  {
465  size_t i,n;
466  #if defined(_DEBUG) || (MRPT_ALWAYS_CHECKS_DEBUG_MATRICES)
467  if (nCol>=m_Cols)
468  THROW_EXCEPTION("extractCol: Column index out of bounds");
469  #endif
470 
471  n = m_Rows - startingRow;
472  out.resize( n );
473 
474  for (i=0;i<n;i++)
475  out[i] = m_Val[i+startingRow][nCol];
476  }
477 
478  /** Gets a given column to a vector (without modifying the matrix)
479  * \exception std::exception On index out of bounds
480  */
481  void extractCol(size_t nCol, CMatrixTemplate<T> &out, int startingRow = 0) const
482  {
483  size_t i,n;
484  #if defined(_DEBUG) || (MRPT_ALWAYS_CHECKS_DEBUG_MATRICES)
485  if (nCol>=m_Cols)
486  THROW_EXCEPTION("extractCol: Column index out of bounds");
487  #endif
488 
489  n = m_Rows - startingRow;
490  out.setSize(n,1);
491 
492  for (i=0;i<n;i++)
493  out(i,0) = m_Val[i+startingRow][nCol];
494  }
495 
496  /** Appends a new row to the MxN matrix from a 1xN vector.
497  * The lenght of the vector must match the width of the matrix, unless it's empty: in that case the matrix is resized to 1xN.
498  * \code
499  * CMatrixDouble M(0,0);
500  * vector_double v(7),w(7);
501  * // ...
502  * M.appendRow(v);
503  * M.appendRow(w);
504  * \endcode
505  * \exception std::exception On incorrect vector length.
506  * \sa extractRow
507  * \sa appendCol
508  */
509  void appendRow(const std::vector<T> &in)
510  {
511  size_t i,n, row;
512 
513  n = m_Cols;
514  row = m_Rows;
515 
516  if (m_Cols==0 || m_Rows==0)
517  {
518  ASSERT_(!in.empty());
519  n=m_Cols=in.size();
520  }
521  else
522  {
523  ASSERT_(in.size()==m_Cols);
524  }
525 
526  realloc( row+1,n );
527 
528  for (i=0;i<n;i++)
529  m_Val[row][i] = in[i];
530  }
531 
532  /** Appends a new column to the matrix from a vector.
533  * The length of the vector must match the number of rows of the matrix, unless it is (0,0).
534  * \exception std::exception On size mismatch.
535  * \sa extractCol
536  * \sa appendRow
537  */
538  void appendCol(const std::vector<T> &in) {
539  size_t r=m_Rows,c=m_Cols;
540  if (m_Cols==0||m_Rows==0) {
541  ASSERT_(!in.empty());
542  r=in.size();
543  c=0;
544  } else ASSERT_(in.size()==m_Rows);
545  realloc(r,c+1);
546  for (size_t i=0;i<m_Rows;i++) m_Val[i][m_Cols-1]=in[i];
547  }
548 
549  /** Inserts a column from a vector, replacing the current contents of that column.
550  * \exception std::exception On index out of bounds
551  * \sa extractCol
552  */
553  void insertCol(size_t nCol, const std::vector<T> &in)
554  {
555  if (nCol>=m_Cols) THROW_EXCEPTION("insertCol: Row index out of bounds");
556 
557  size_t n = in.size();
558  ASSERT_( m_Rows >= in.size() );
559 
560  for (size_t i=0;i<n;i++)
561  m_Val[i][nCol] = in[i];
562  }
563 
564  /** Returns a vector containing the matrix's values.
565  */
566  void getAsVector(std::vector<T> &out) const {
567  out.clear();
568  out.reserve(m_Rows*m_Cols);
569  for (size_t i=0;i<m_Rows;i++) out.insert(out.end(),&(m_Val[i][0]),&(m_Val[i][m_Cols]));
570  }
571 
572  }; // end of class CMatrixTemplate
573 
574 
575  } // End of namespace
576 } // End of namespace
577 
578 
579 #endif


Page generated by Doxygen 1.8.3 for MRPT 0.9.6 SVN: at Fri Feb 15 22:05:02 EST 2013