9 #ifndef CMatrixTemplate_H 10 #define CMatrixTemplate_H 34 inline operator size_t(
void)
const {
return 2; }
71 void realloc(
size_t row,
size_t col,
bool newElementsToZero =
false)
73 if (row!=m_Rows || col!=m_Cols || m_Val==NULL)
76 bool doZeroColumns = newElementsToZero && (col>m_Cols);
77 size_t sizeZeroColumns =
sizeof(T)*(col-m_Cols);
80 for (r=row;r<m_Rows;r++)
89 size_t row_size = col *
sizeof(T);
102 ::memset(&m_Val[r][m_Cols],0,sizeZeroColumns);
122 #if defined(_DEBUG)||(MRPT_ALWAYS_CHECKS_DEBUG_MATRICES) 128 for (
size_t r=0;r<m_Rows;r++)
129 for (
size_t c=0;c<m_Cols;c++)
136 std::swap(m_Val, o.
m_Val );
137 std::swap(m_Rows, o.
m_Rows );
138 std::swap(m_Cols, o.
m_Cols );
158 realloc( cropRowCount, cropColCount );
159 for (
size_t i=0; i < m_Rows; i++)
160 for (
size_t j=0; j < m_Cols; j++)
161 m_Val[i][j] = m.
m_Val[i][j];
172 template <
typename V,
size_t N>
173 CMatrixTemplate (
size_t row,
size_t col, V (&theArray)[N] ) : m_Val(NULL),m_Rows(0),m_Cols(0)
177 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)))
179 for (
size_t i=0; i < m_Rows; i++)
180 for (
size_t j=0; j < m_Cols; j++)
181 m_Val[i][j] = static_cast<T>(theArray[idx++]);
186 template <
typename V>
187 CMatrixTemplate(
size_t row,
size_t col,
const V &theVector ) : m_Val(NULL),m_Rows(0),m_Cols(0)
189 const size_t N = theVector.size();
191 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)))
193 for (
size_t i=0; i < m_Rows; i++)
194 for (
size_t j=0; j < m_Cols; j++)
195 m_Val[i][j] = static_cast<T>( *(it++) );
205 for (
size_t i=0; i < m_Rows; i++)
206 for (
size_t j=0; j < m_Cols; j++)
207 m_Val[i][j] = m.
m_Val[i][j];
221 template <
typename V,
size_t N>
225 if (m_Rows*m_Cols != N)
227 THROW_EXCEPTION(
format(
"Mismatch between matrix size %lu x %lu and array of length %lu",m_Rows,m_Cols,N))
230 for (
size_t i=0; i < m_Rows; i++)
231 for (
size_t j=0; j < m_Cols; j++)
232 m_Val[i][j] = static_cast<T>(theArray[idx++]);
256 void setSize(
size_t row,
size_t col,
bool zeroNewElements=
false)
258 realloc(row,col,zeroNewElements);
264 setSize(siz[0],siz[1],zeroNewElements);
269 inline T& operator () (
size_t row,
size_t col)
271 #if defined(_DEBUG) || (MRPT_ALWAYS_CHECKS_DEBUG_MATRICES) 272 if (row >= m_Rows || col >= m_Cols)
273 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)) );
275 return m_Val[row][col];
280 inline const T &operator () (
size_t row,
size_t col)
const 282 #if defined(_DEBUG) || (MRPT_ALWAYS_CHECKS_DEBUG_MATRICES) 283 if (row >= m_Rows || col >= m_Cols)
284 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)) );
286 return m_Val[row][col];
292 inline T& operator () (
size_t ith)
294 #if defined(_DEBUG) || (MRPT_ALWAYS_CHECKS_DEBUG_MATRICES) 295 ASSERT_(m_Rows==1 || m_Cols==1);
300 #if defined(_DEBUG) || (MRPT_ALWAYS_CHECKS_DEBUG_MATRICES) 304 return m_Val[0][ith];
309 #if defined(_DEBUG) || (MRPT_ALWAYS_CHECKS_DEBUG_MATRICES) 313 return m_Val[ith][0];
320 inline T operator () (
size_t ith)
const 322 #if defined(_DEBUG) || (MRPT_ALWAYS_CHECKS_DEBUG_MATRICES) 323 ASSERT_(m_Rows==1 || m_Cols==1);
328 #if defined(_DEBUG) || (MRPT_ALWAYS_CHECKS_DEBUG_MATRICES) 332 return m_Val[0][ith];
337 #if defined(_DEBUG) || (MRPT_ALWAYS_CHECKS_DEBUG_MATRICES) 341 return m_Val[ith][0];
350 if (row >= m_Rows || col >= m_Cols)
351 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)) );
361 if (row >= m_Rows || col >= m_Cols)
362 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)) );
364 return m_Val[row][col];
372 if (row >= m_Rows || col >= m_Cols)
373 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)) );
375 return m_Val[row][col];
384 THROW_EXCEPTION(
format(
"Row index %lu out of range. Matrix is %lux%lu",static_cast<unsigned long>(row),static_cast<unsigned long>(m_Rows),static_cast<unsigned long>(m_Cols)) );
397 inline CMatrixTemplate<T> operator() (
const size_t row1,
const size_t row2,
const size_t col1,
const size_t col2)
const {
408 int nrows=int(row2)-int(row1)+1;
409 int ncols=int(col2)-int(col1)+1;
410 if (nrows<=0||ncols<=0) {
414 if (row2>=m_Rows||col2>=m_Cols)
THROW_EXCEPTION(
"Indices out of range!");
416 for (
int i=0;i<nrows;i++)
for (
int j=0;j<ncols;j++) out.
m_Val[i][j]=m_Val[i+row1][j+col1];
419 template <
class EIGEN_MATRIX>
420 void extractSubmatrix(
const size_t row1,
const size_t row2,
const size_t col1,
const size_t col2,EIGEN_MATRIX &out)
const 422 int nrows=int(row2)-int(row1)+1;
423 int ncols=int(col2)-int(col1)+1;
424 if (nrows<=0||ncols<=0) {
425 out =
typename EIGEN_MATRIX::PlainObject();
428 if (row2>=m_Rows||col2>=m_Cols)
THROW_EXCEPTION(
"Indices out of range!");
429 out.resize(nrows,ncols);
430 for (
int i=0;i<nrows;i++)
for (
int j=0;j<ncols;j++) out.coeffRef(i,j)=m_Val[i+row1][j+col1];
440 out.
setSize(lastRow-firstRow+1,m_Cols);
450 out.
setSize(m_Rows,lastCol-firstCol+1);
457 void extractCol(
size_t nCol, std::vector<T> &out,
int startingRow = 0)
const 460 #if defined(_DEBUG) || (MRPT_ALWAYS_CHECKS_DEBUG_MATRICES) 465 n = m_Rows - startingRow;
469 out[i] = m_Val[i+startingRow][nCol];
478 #if defined(_DEBUG) || (MRPT_ALWAYS_CHECKS_DEBUG_MATRICES) 483 n = m_Rows - startingRow;
487 out(i,0) = m_Val[i+startingRow][nCol];
510 if (m_Cols==0 || m_Rows==0)
523 m_Val[row][i] = in[i];
533 size_t r=m_Rows,c=m_Cols;
534 if (m_Cols==0||m_Rows==0) {
538 }
else ASSERT_(in.size()==m_Rows);
540 for (
size_t i=0;i<m_Rows;i++) m_Val[i][m_Cols-1]=in[i];
549 if (nCol>=m_Cols)
THROW_EXCEPTION(
"insertCol: Row index out of bounds");
551 size_t n = in.size();
552 ASSERT_( m_Rows >= in.size() );
554 for (
size_t i=0;i<n;i++)
555 m_Val[i][nCol] = in[i];
562 out.reserve(m_Rows*m_Cols);
563 for (
size_t i=0;i<m_Rows;i++) out.insert(out.end(),&(m_Val[i][0]),&(m_Val[i][m_Cols]));
const T & get_unsafe(size_t row, size_t col) const
Fast but unsafe method to read a value from the matrix.
void extractCol(size_t nCol, std::vector< T > &out, int startingRow=0) const
Returns a given column to a vector (without modifying the matrix)
T & get_unsafe(size_t row, size_t col)
Fast but unsafe method to get a reference from the matrix.
void BASE_IMPEXP aligned_free(void *p)
Frees a memory block reserved by aligned_malloc.
virtual ~CMatrixTemplate()
Destructor.
Declares a matrix of booleans (non serializable).
void insertCol(size_t nCol, const std::vector< T > &in)
Inserts a column from a vector, replacing the current contents of that column.
CMatrixTemplate(const CMatrixTemplate &m, const size_t cropRowCount, const size_t cropColCount)
Copy constructor & crop from another matrix.
void BASE_IMPEXP * aligned_realloc(void *old_ptr, size_t bytes, size_t alignment)
Frees a memory block reserved by aligned_malloc.
std::string BASE_IMPEXP format(const char *fmt,...) MRPT_printf_format_check(1
A std::string version of C sprintf.
T * get_unsafe_row(size_t row)
Fast but unsafe method to obtain a pointer to a given row of the matrix (Use only in time critical ap...
#define THROW_EXCEPTION(msg)
#define THROW_EXCEPTION_FMT(_FORMAT_STRING,...)
Auxiliary class used in CMatrixTemplate:size(), CMatrixTemplate::resize(), CMatrixFixedNumeric::size(...
void appendRow(const std::vector< T > &in)
Appends a new row to the MxN matrix from a 1xN vector.
bool operator!=(const CMatrixTemplateSize &o) const
const Scalar * const_iterator
std::ptrdiff_t difference_type
EIGEN_STRONG_INLINE size_t getColCount() const
Get number of columns.
CMatrixTemplate(size_t row, size_t col, V(&theArray)[N])
Constructor from a given size and a C array.
reference operator[](size_type i)
void extractMatrix(const MATORG &M, const size_t first_row, const size_t first_col, MATDEST &outMat)
Extract a submatrix - The output matrix must be set to the required size before call.
CArray< size_t, N > & operator=(const CArray< T2, N > &rhs)
CMatrixTemplate(size_t row=1, size_t col=1)
A STL container (as wrapper) for arrays of constant size defined at compile time. ...
void resize(const CMatrixTemplateSize &siz, bool zeroNewElements=false)
This method just checks has no effects in this class, but raises an exception if the expected size do...
EIGEN_STRONG_INLINE void setSize(size_t row, size_t col)
Changes the size of matrix, maintaining its previous content as possible and padding with zeros where...
void set_unsafe(size_t row, size_t col, const T &v)
Fast but unsafe method to write a value in the matrix.
CMatrixTemplateSize mrpt_autotype
EIGEN_STRONG_INLINE void extractSubmatrix(const size_t row_first, const size_t row_last, const size_t col_first, const size_t col_last, MATRIX &out) const
Get a submatrix, given its bounds: first & last column and row (inclusive).
void getAsVector(std::vector< T > &out) const
Returns a vector containing the matrix's values.
void * aligned_calloc(size_t bytes, size_t alignment)
Identical to aligned_malloc, but it zeroes the reserved memory block.
void extractRows(size_t firstRow, size_t lastRow, CMatrixTemplate< T > &out) const
Gets a series of contiguous rows.
CMatrixTemplateSize size() const
Get a 2-vector with [NROWS NCOLS] (as in MATLAB command size(x))
const T * get_unsafe_row(size_t row) const
Fast but unsafe method to obtain a pointer to a given row of the matrix (Use only in critical applica...
This template class provides the basic functionality for a general 2D any-size, resizable container o...
void extractCol(size_t nCol, CMatrixTemplate< T > &out, int startingRow=0) const
Gets a given column to a vector (without modifying the matrix)
#define MRPT_COMPILE_TIME_ASSERT(f)
void extractColumns(size_t firstCol, size_t lastCol, CMatrixTemplate< T > &out) const
Gets a series of contiguous columns.
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
void extractSubmatrix(const size_t row1, const size_t row2, const size_t col1, const size_t col2, EIGEN_MATRIX &out) const
void appendCol(const std::vector< T > &in)
Appends a new column to the matrix from a vector.
CMatrixTemplate(size_t row, size_t col, const V &theVector)
Constructor from a given size and a STL container (std::vector, std::list,...) with the initial value...
void swap(CMatrixTemplate< T > &o)
Swap with another matrix very efficiently (just swaps a pointer and two integer values).
size_t getColCount() const
Number of columns in the matrix.
size_t getRowCount() const
Number of rows in the matrix.
T value_type
The type of the matrix elements.
void setSize(size_t row, size_t col, bool zeroNewElements=false)
Changes the size of matrix, maintaining the previous contents.
const T & const_reference
CMatrixTemplate(const CMatrixTemplate &m)
Constructors.
EIGEN_STRONG_INLINE size_t getRowCount() const
Get number of rows.
bool operator==(const CMatrixTemplateSize &o) const
void realloc(size_t row, size_t col, bool newElementsToZero=false)
Internal use only: It reallocs the memory for the 2D matrix, maintaining the previous contents if pos...
mrpt::utils::CArray< size_t, 2 > Base
void ASSERT_ENOUGHROOM(size_t r, size_t c) const
Checks whether the rows [r-N,r+N] and the columns [c-N,c+N] are present in the matrix.
CMatrixTemplateSize(const size_t *d)
void fillAll(const T &val)
void extractSubmatrix(const size_t row1, const size_t row2, const size_t col1, const size_t col2, CMatrixTemplate< T > &out) const
Get a submatrix, given its bounds.