9 #ifndef CMatrixTemplate_H
10 #define CMatrixTemplate_H
56 void realloc(
size_t row,
size_t col,
bool newElementsToZero =
false)
58 if (row!=m_Rows || col!=m_Cols || m_Val==NULL)
61 bool doZeroColumns = newElementsToZero && (col>
m_Cols);
62 size_t sizeZeroColumns =
sizeof(T)*(col-m_Cols);
74 size_t row_size = col *
sizeof(T);
87 ::memset(&m_Val[r][m_Cols],0,sizeZeroColumns);
107 #if defined(_DEBUG)||(MRPT_ALWAYS_CHECKS_DEBUG_MATRICES)
113 for (
size_t r=0;r<
m_Rows;r++)
114 for (
size_t c=0;c<
m_Cols;c++)
121 std::swap(m_Val, o.
m_Val );
122 std::swap(m_Rows, o.
m_Rows );
123 std::swap(m_Cols, o.
m_Cols );
143 realloc( cropRowCount, cropColCount );
144 for (
size_t i=0; i <
m_Rows; i++)
145 for (
size_t j=0; j <
m_Cols; j++)
146 m_Val[i][j] = m.
m_Val[i][j];
157 template <
typename V,
size_t N>
158 CMatrixTemplate (
size_t row,
size_t col, V (&theArray)[N] ) : m_Val(NULL),m_Rows(0),m_Cols(0)
162 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)))
164 for (
size_t i=0; i <
m_Rows; i++)
165 for (
size_t j=0; j <
m_Cols; j++)
166 m_Val[i][j] = static_cast<T>(theArray[idx++]);
171 template <
typename V>
172 CMatrixTemplate(
size_t row,
size_t col,
const V &theVector ) : m_Val(NULL),m_Rows(0),m_Cols(0)
174 const size_t N = theVector.size();
176 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)))
178 for (
size_t i=0; i <
m_Rows; i++)
179 for (
size_t j=0; j <
m_Cols; j++)
180 m_Val[i][j] = static_cast<T>( *(it++) );
190 for (
size_t i=0; i <
m_Rows; i++)
191 for (
size_t j=0; j <
m_Cols; j++)
192 m_Val[i][j] = m.
m_Val[i][j];
206 template <
typename V,
size_t N>
210 if (m_Rows*m_Cols != N)
212 THROW_EXCEPTION(
format(
"Mismatch between matrix size %lu x %lu and array of length %lu",m_Rows,m_Cols,N))
215 for (
size_t i=0; i <
m_Rows; i++)
216 for (
size_t j=0; j <
m_Cols; j++)
217 m_Val[i][j] = static_cast<T>(theArray[idx++]);
241 void setSize(
size_t row,
size_t col,
bool zeroNewElements=
false)
243 realloc(row,col,zeroNewElements);
249 setSize(siz[0],siz[1],zeroNewElements);
256 #if defined(_DEBUG) || (MRPT_ALWAYS_CHECKS_DEBUG_MATRICES)
257 if (row >= m_Rows || col >= m_Cols)
258 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)) );
260 return m_Val[row][col];
267 #if defined(_DEBUG) || (MRPT_ALWAYS_CHECKS_DEBUG_MATRICES)
268 if (row >= m_Rows || col >= m_Cols)
269 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)) );
271 return m_Val[row][col];
279 #if defined(_DEBUG) || (MRPT_ALWAYS_CHECKS_DEBUG_MATRICES)
280 ASSERT_(m_Rows==1 || m_Cols==1);
285 #if defined(_DEBUG) || (MRPT_ALWAYS_CHECKS_DEBUG_MATRICES)
289 return m_Val[0][ith];
294 #if defined(_DEBUG) || (MRPT_ALWAYS_CHECKS_DEBUG_MATRICES)
298 return m_Val[ith][0];
307 #if defined(_DEBUG) || (MRPT_ALWAYS_CHECKS_DEBUG_MATRICES)
308 ASSERT_(m_Rows==1 || m_Cols==1);
313 #if defined(_DEBUG) || (MRPT_ALWAYS_CHECKS_DEBUG_MATRICES)
317 return m_Val[0][ith];
322 #if defined(_DEBUG) || (MRPT_ALWAYS_CHECKS_DEBUG_MATRICES)
326 return m_Val[ith][0];
335 if (row >= m_Rows || col >= m_Cols)
336 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)) );
346 if (row >= m_Rows || col >= m_Cols)
347 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)) );
349 return m_Val[row][col];
357 if (row >= m_Rows || col >= m_Cols)
358 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)) );
360 return m_Val[row][col];
369 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)) );
393 int nrows=int(row2)-int(row1)+1;
394 int ncols=int(col2)-int(col1)+1;
395 if (nrows<=0||ncols<=0) {
399 if (row2>=m_Rows||col2>=m_Cols)
THROW_EXCEPTION(
"Indices out of range!");
401 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];
404 template <
class EIGEN_MATRIX>
405 void extractSubmatrix(
const size_t row1,
const size_t row2,
const size_t col1,
const size_t col2,EIGEN_MATRIX &out)
const
407 int nrows=int(row2)-int(row1)+1;
408 int ncols=int(col2)-int(col1)+1;
409 if (nrows<=0||ncols<=0) {
410 out =
typename EIGEN_MATRIX::PlainObject();
413 if (row2>=m_Rows||col2>=m_Cols)
THROW_EXCEPTION(
"Indices out of range!");
414 out.resize(nrows,ncols);
415 for (
int i=0;i<nrows;i++)
for (
int j=0;j<ncols;j++) out.coeffRef(i,j)=m_Val[i+row1][j+col1];
425 out.
setSize(lastRow-firstRow+1,m_Cols);
435 out.
setSize(m_Rows,lastCol-firstCol+1);
442 void extractCol(
size_t nCol, std::vector<T> &out,
int startingRow = 0)
const
445 #if defined(_DEBUG) || (MRPT_ALWAYS_CHECKS_DEBUG_MATRICES)
450 n = m_Rows - startingRow;
454 out[i] = m_Val[i+startingRow][nCol];
463 #if defined(_DEBUG) || (MRPT_ALWAYS_CHECKS_DEBUG_MATRICES)
468 n = m_Rows - startingRow;
472 out(i,0) = m_Val[i+startingRow][nCol];
495 if (m_Cols==0 || m_Rows==0)
508 m_Val[row][i] = in[i];
519 if (m_Cols==0||m_Rows==0) {
525 for (
size_t i=0;i<
m_Rows;i++) m_Val[i][m_Cols-1]=in[i];
534 if (nCol>=m_Cols)
THROW_EXCEPTION(
"insertCol: Row index out of bounds");
536 size_t n = in.size();
537 ASSERT_( m_Rows >= in.size() );
539 for (
size_t i=0;i<n;i++)
540 m_Val[i][nCol] = in[i];
547 out.reserve(m_Rows*m_Cols);
548 for (
size_t i=0;i<
m_Rows;i++) out.insert(out.end(),&(m_Val[i][0]),&(m_Val[i][m_Cols]));
T & operator()(size_t row, size_t col)
Subscript operator to get/set individual elements.
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.
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.
const T & get_unsafe(size_t row, size_t col) const
Fast but unsafe method to read a value from the 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)
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.
void extractCol(size_t nCol, std::vector< T > &out, int startingRow=0) const
Returns a given column to a vector (without modifying the matrix) On index out of bounds...
const Scalar * const_iterator
std::ptrdiff_t difference_type
void extractRows(size_t firstRow, size_t lastRow, CMatrixTemplate< T > &out) const
Gets a series of contiguous rows.
CMatrixTemplate(size_t row, size_t col, V(&theArray)[N])
Constructor from a given size and a C array.
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.
void getAsVector(std::vector< T > &out) const
Returns a vector containing the matrix's values.
CMatrixTemplate(size_t row=1, size_t col=1)
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...
void set_unsafe(size_t row, size_t col, const T &v)
Fast but unsafe method to write a value in the matrix.
void extractColumns(size_t firstCol, size_t lastCol, CMatrixTemplate< T > &out) const
Gets a series of contiguous columns.
void extractCol(size_t nCol, CMatrixTemplate< T > &out, int startingRow=0) const
Gets a given column to a vector (without modifying the matrix) On index out of bounds.
void * aligned_calloc(size_t bytes, size_t alignment)
Identical to aligned_malloc, but it zeroes the reserved memory block.
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...
#define MRPT_COMPILE_TIME_ASSERT(f)
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
size_t getColCount() const
Number of columns in the matrix.
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...
size_t getRowCount() const
Number of rows in the matrix.
CMatrixTemplateSize size() const
Get a 2-vector with [NROWS NCOLS] (as in MATLAB command size(x))
void swap(CMatrixTemplate< T > &o)
Swap with another matrix very efficiently (just swaps a pointer and two integer values).
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
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.
CMatrixTemplate(const CMatrixTemplate &m)
Constructors.
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...
CMatrixTemplate & operator=(const CMatrixTemplate &m)
Assignment operator from another matrix.
#define THROW_EXCEPTION_CUSTOM_MSG1(msg, param1)
void fillAll(const T &val)
void extractSubmatrix(const size_t row1, const size_t row2, const size_t col1, const size_t col2, EIGEN_MATRIX &out) const