00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028 #ifndef CMatrixFixedNumeric_H
00029 #define CMatrixFixedNumeric_H
00030
00031 #include <mrpt/math/CArray.h>
00032 #include <mrpt/math/math_frwds.h>
00033 #include <mrpt/math/matrices_metaprogramming.h>
00034 #include <mrpt/math/CMatrixViews.h>
00035 #include <mrpt/utils/CSerializable.h>
00036
00037 namespace mrpt
00038 {
00039 namespace math
00040 {
00041 using namespace mrpt::system;
00042 using namespace mrpt::poses;
00043
00044
00045
00046
00047
00048
00049
00050
00051
00052
00053
00054 template <typename T,size_t NROWS,size_t NCOLS>
00055 class CMatrixFixedNumeric
00056 {
00057 public:
00058 typedef CArray<T,NROWS*NCOLS> array_type;
00059
00060 typedef T value_type;
00061 typedef T& reference;
00062 typedef const T& const_reference;
00063 typedef std::size_t size_type;
00064 typedef std::ptrdiff_t difference_type;
00065
00066
00067 typedef CMatrixFixedNumeric<T,NROWS,NCOLS> mrpt_autotype;
00068 DECLARE_MRPT_CONTAINER_TYPES
00069 DECLARE_MRPT_CONTAINER_IS_MATRIX_FIXED(NROWS,NCOLS)
00070
00071 DECLARE_COMMON_MATRIX_MEMBERS(T)
00072
00073
00074
00075
00076 typedef typename array_type::iterator iterator;
00077 typedef typename array_type::const_iterator const_iterator;
00078 typedef typename array_type::reverse_iterator reverse_iterator;
00079 typedef typename array_type::const_reverse_iterator const_reverse_iterator;
00080
00081 inline iterator begin() { return m_Val.begin(); }
00082 inline iterator end() { return m_Val.end(); }
00083 inline const_iterator begin() const { return m_Val.begin(); }
00084 inline const_iterator end() const { return m_Val.end(); }
00085
00086 inline reverse_iterator rbegin() { return m_Val.rbegin(); }
00087 inline reverse_iterator rend() { return m_Val.rend(); }
00088 inline const_reverse_iterator rbegin() const { return m_Val.rbegin(); }
00089 inline const_reverse_iterator rend() const { return m_Val.rend(); }
00090
00091
00092
00093
00094
00095 #if MRPT_HAS_SSE2 && defined(MRPT_USE_SSE2)
00096 MRPT_ALIGN16
00097 #endif
00098 array_type m_Val;
00099
00100 public:
00101
00102 CMatrixFixedNumeric() {
00103 #if defined(_DEBUG) && MRPT_HAS_SSE2 && defined(MRPT_USE_SSE2)
00104 if ((uintptr_t(m_Val) & 0x0f) != 0 )
00105 THROW_EXCEPTION("16-unaligned memory!")
00106 #endif
00107 if (NROWS*NCOLS) ::memset(&m_Val[0],0,sizeof(T)*NROWS*NCOLS);
00108 }
00109
00110
00111
00112
00113 CMatrixFixedNumeric(bool ,bool, bool ) {
00114 }
00115
00116
00117
00118 template <typename R>
00119 inline CMatrixFixedNumeric(const CMatrixTemplateNumeric<R>& M) {
00120 *this = M;
00121 }
00122
00123
00124 template <class OTHERMATRIX> inline RET_TYPE_ASSERT_MRPTMATRIX(OTHERMATRIX,mrpt_autotype)&
00125 operator =(const OTHERMATRIX& M) {
00126 return this->assignMatrix(M);
00127 }
00128
00129
00130 template <typename R>
00131 inline mrpt_autotype& operator =(const CMatrixTemplate<R>& M) {
00132 return this->assignMatrix(M);
00133 }
00134
00135
00136
00137
00138
00139
00140
00141
00142
00143 template <typename V, size_t N>
00144 inline CMatrixFixedNumeric ( V (&theArray)[N] ) {
00145 loadFromArray(theArray);
00146 }
00147
00148
00149
00150
00151
00152
00153
00154
00155
00156
00157
00158 template <typename V, size_t N>
00159 void loadFromArray(V (&theArray)[N] )
00160 {
00161 MRPT_COMPILE_TIME_ASSERT(N!=0)
00162 MRPT_COMPILE_TIME_ASSERT(N==NROWS * NCOLS)
00163 if (sizeof(V)==sizeof(T))
00164 ::memcpy(&m_Val[0],theArray,sizeof(m_Val));
00165 else
00166 for (size_t i=0;i<N;i++)
00167 m_Val[i] = static_cast<T>(theArray[i]);
00168 }
00169
00170
00171
00172
00173 template <size_t N,size_t M>
00174 explicit CMatrixFixedNumeric(const CMatrixFixedNumeric<T,N,M> &B)
00175 {
00176 ::memset(&m_Val[0],0,sizeof(m_Val));
00177 const size_t nr = std::min(NROWS,N);
00178 const size_t nc = std::min(NCOLS,M);
00179 for (size_t r=0;r<nr;r++)
00180 ::memcpy(&m_Val[0]+NCOLS*r, &B.m_Val[0]+M*r, sizeof(T)*nc );
00181 }
00182
00183
00184
00185 template <typename R,size_t N,size_t M>
00186 explicit CMatrixFixedNumeric(const CMatrixFixedNumeric<R,N,M> &B)
00187 {
00188 ::memset(&m_Val[0],0,sizeof(m_Val));
00189 const size_t nr = std::min(NROWS,N);
00190 const size_t nc = std::min(NCOLS,M);
00191 for (size_t r=0;r<nr;r++)
00192 for (size_t c=0;c<nc;c++)
00193 this->set_unsafe(r,c, B.get_unsafe(r,c) );
00194 }
00195
00196 inline CMatrixFixedNumeric( const TPose2D &p) { mrpt::math::containerFromPoseOrPoint(*this,p); }
00197 inline CMatrixFixedNumeric( const TPose3D &p) { mrpt::math::containerFromPoseOrPoint(*this,p); }
00198 inline CMatrixFixedNumeric( const TPose3DQuat &p) { mrpt::math::containerFromPoseOrPoint(*this,p); }
00199 inline CMatrixFixedNumeric( const TPoint2D &p) { mrpt::math::containerFromPoseOrPoint(*this,p); }
00200 inline CMatrixFixedNumeric( const TPoint3D &p) { mrpt::math::containerFromPoseOrPoint(*this,p); }
00201
00202 inline CMatrixFixedNumeric( const mrpt::poses::CPose2D &p) { mrpt::math::containerFromPoseOrPoint(*this,p); }
00203 inline CMatrixFixedNumeric( const mrpt::poses::CPose3D &p) { mrpt::math::containerFromPoseOrPoint(*this,p); }
00204 inline CMatrixFixedNumeric( const mrpt::poses::CPose3DQuat &p) { mrpt::math::containerFromPoseOrPoint(*this,p); }
00205 inline CMatrixFixedNumeric( const mrpt::poses::CPoint2D &p) { mrpt::math::containerFromPoseOrPoint(*this,p); }
00206 inline CMatrixFixedNumeric( const mrpt::poses::CPoint3D &p) { mrpt::math::containerFromPoseOrPoint(*this,p); }
00207
00208
00209 static inline size_t getRowCount() { return NROWS; }
00210
00211
00212 static inline size_t getColCount() { return NCOLS; }
00213
00214
00215 static inline const CMatrixTemplateSize & size() {
00216 static size_t dims_arr[] = {NROWS, NCOLS};
00217 static const CMatrixTemplateSize dims(dims_arr);
00218 return dims;
00219 }
00220
00221
00222 static inline void setSize(const size_t nRows, const size_t nCols) {
00223 if (nRows!=NROWS || nCols!=NCOLS)
00224 throw std::logic_error(format("Try to change the size of a %ux%u fixed-sized matrix to %ux%u.",static_cast<unsigned>(NROWS),static_cast<unsigned>(NCOLS),static_cast<unsigned>(nRows),static_cast<unsigned>(nCols)));
00225 }
00226
00227 static inline void resize(const size_t nRows_times_nCols) {
00228 if (nRows_times_nCols!=NROWS*NCOLS)
00229 throw std::logic_error(format("Try to change the size of a %ux%u fixed-sized matrix to %u elements.",static_cast<unsigned>(NROWS),static_cast<unsigned>(NCOLS),static_cast<unsigned>(nRows_times_nCols)));
00230 }
00231
00232
00233
00234 inline void unit() {
00235 ::memset(&m_Val[0],0,sizeof(m_Val));
00236 for (size_t i=0;i<NROWS * NCOLS;i+=(NROWS+1))
00237 m_Val[i] = 1;
00238 }
00239
00240
00241 inline void eye() { this->unit(); }
00242
00243
00244 inline void zeros() {
00245 ::memset(&m_Val[0],0,sizeof(m_Val));
00246 }
00247
00248
00249 inline void zeros(size_t N, size_t M) {
00250 ASSERT_(N==NROWS && M==NCOLS);
00251 this->zeros();
00252 }
00253
00254
00255 inline T get_unsafe(const size_t row, const size_t col) const {
00256 #ifdef _DEBUG
00257 if (row >= NROWS || col >= NCOLS)
00258 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>(NROWS),static_cast<unsigned long>(NCOLS)) );
00259 #endif
00260 return m_Val[NCOLS*row+col];
00261 }
00262
00263
00264 inline T& get_unsafe(const size_t row, const size_t col) {
00265 #ifdef _DEBUG
00266 if (row >= NROWS || col >= NCOLS)
00267 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>(NROWS),static_cast<unsigned long>(NCOLS)) );
00268 #endif
00269 return m_Val[NCOLS*row+col];
00270 }
00271
00272
00273 inline void set_unsafe(const size_t row, const size_t col, const T val) {
00274 #ifdef _DEBUG
00275 if (row >= NROWS || col >= NCOLS)
00276 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>(NROWS),static_cast<unsigned long>(NCOLS)) );
00277 #endif
00278 m_Val[NCOLS*row+col] = val;
00279 }
00280
00281
00282
00283 inline T& operator () (const size_t row, const size_t col)
00284 {
00285 #if defined(_DEBUG) || (MRPT_ALWAYS_CHECKS_DEBUG_MATRICES)
00286 if (row >= NROWS || col >= NCOLS)
00287 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>(NROWS),static_cast<unsigned long>(NCOLS)) );
00288 #endif
00289 return m_Val[NCOLS*row+col];
00290 }
00291
00292
00293
00294 inline T operator () (const size_t row, const size_t col) const
00295 {
00296 #if defined(_DEBUG) || (MRPT_ALWAYS_CHECKS_DEBUG_MATRICES)
00297 if (row >= NROWS || col >= NCOLS)
00298 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>(NROWS),static_cast<unsigned long>(NCOLS)) );
00299 #endif
00300 return m_Val[NCOLS*row+col];
00301 }
00302
00303
00304
00305
00306
00307
00308
00309
00310
00311
00312
00313
00314 bool fromMatlabStringFormat(const std::string &s)
00315 {
00316 CMatrixTemplate<T> M;
00317 if (!M.fromMatlabStringFormat(s)) return false;
00318 if (M.getColCount()!=NCOLS || M.getRowCount()!=NROWS) return false;
00319 *this = M;
00320 return true;
00321 }
00322
00323
00324 void force_symmetry() {
00325 for (size_t i=0;i<NROWS;i++)
00326 for (size_t j=i+1;j<NCOLS;j++)
00327 get_unsafe(i,j) = get_unsafe(j,i);
00328 }
00329
00330
00331
00332
00333
00334
00335
00336
00337 void loadFromTextFile(const std::string &file)
00338 {
00339
00340 std::ifstream f(file.c_str());
00341 if (f.fail()) THROW_EXCEPTION_CUSTOM_MSG1("loadFromTextFile: can't open file:'%s'",file.c_str());
00342
00343 std::string str;
00344 std::vector<double> fil(512);
00345
00346 const char *ptr;
00347 char *ptrEnd;
00348 size_t i;
00349 size_t nRows = 0;
00350
00351 while ( !f.eof() )
00352 {
00353 std::getline(f,str);
00354
00355 if (str.size() && str[0]!='#' && str[0]!='%')
00356 {
00357
00358 ptr = str.c_str();
00359
00360 ptrEnd = NULL;
00361 i=0;
00362
00363
00364 while ( ptr[0] && ptr!=ptrEnd )
00365 {
00366
00367 while (ptr[0] && (ptr[0]==' ' || ptr[0]=='\t' || ptr[0]=='\r' || ptr[0]=='\n'))
00368 ptr++;
00369
00370 if (fil.size()<=i) fil.resize(fil.size()+512);
00371
00372
00373 fil[i] = strtod(ptr,&ptrEnd);
00374
00375
00376 if (ptr!=ptrEnd)
00377 {
00378 i++;
00379 ptr = ptrEnd;
00380 ptrEnd = NULL;
00381 }
00382 };
00383
00384
00385 if (i!=NCOLS) THROW_EXCEPTION(format("The matrix in the text file does not match fixed matrix size %ux%u",static_cast<unsigned>(NROWS),static_cast<unsigned>(NCOLS)));
00386 if (nRows>=NROWS) THROW_EXCEPTION(format("The matrix in the text file does not match fixed matrix size %ux%u",static_cast<unsigned>(NROWS),static_cast<unsigned>(NCOLS)));
00387
00388
00389 for (size_t j=0;j<NCOLS;j++)
00390 get_unsafe(nRows,j) = static_cast<T>(fil[j]);
00391
00392 nRows++;
00393 }
00394 }
00395
00396
00397 if (!nRows)
00398 THROW_EXCEPTION("loadFromTextFile: Error loading from text file");
00399 }
00400
00401
00402
00403 void multiplyColumnByScalar(
00404 size_t c,
00405 T scalar)
00406 {
00407 ASSERT_( c < NCOLS );
00408 T *c1 = &m_Val[0]+c;
00409 for(unsigned int k = 0; k < NCOLS; k++)
00410 {
00411 *c1 *= scalar;
00412 c1 += NCOLS;
00413 }
00414 }
00415
00416 inline void swapRows(size_t i1,size_t i2)
00417 {
00418 ASSERT_( i1 < NROWS && i2 < NROWS );
00419 T tmprow[NCOLS];
00420 ::memcpy(tmprow, &m_Val[0]+i1*NCOLS, sizeof(tmprow));
00421 ::memcpy(&m_Val[0]+i1*NCOLS,&m_Val[0]+i2*NCOLS, sizeof(tmprow));
00422 ::memcpy(&m_Val[0]+i2*NCOLS,tmprow, sizeof(tmprow));
00423 }
00424
00425 void swapCols(size_t i1,size_t i2)
00426 {
00427 ASSERT_( i1 < NCOLS && i2 < NCOLS );
00428 T *c1 = &m_Val[0]+i1, *c2 = &m_Val[0]+i2;
00429
00430 for( unsigned int k = 0; k < NROWS; k++)
00431 {
00432 T aux = *c1;
00433 *c1 = *c2;
00434 *c2 = aux;
00435
00436 c1 += NCOLS;
00437 c2 += NCOLS;
00438 }
00439 }
00440
00441 template<size_t N> inline void ASSERT_ENOUGHROOM(size_t r,size_t c) const {
00442 #if defined(_DEBUG)||(MRPT_ALWAYS_CHECKS_DEBUG_MATRICES)
00443 ASSERT_((r>=N)&&(r<NROWS-N)&&(c>=N)&&(c<NCOLS-N));
00444 #endif
00445 }
00446
00447 template<size_t N,typename ReturnType> inline ReturnType getVicinity(size_t c,size_t r) const {
00448 return detail::getVicinity<CMatrixFixedNumeric<T,NROWS,NCOLS>,T,ReturnType,N>::get(c,r,*this);
00449 }
00450
00451 };
00452
00453
00454
00455 typedef CMatrixFixedNumeric<double,2,2> CMatrixDouble22;
00456 typedef CMatrixFixedNumeric<double,3,3> CMatrixDouble33;
00457 typedef CMatrixFixedNumeric<double,4,4> CMatrixDouble44;
00458 typedef CMatrixFixedNumeric<double,6,6> CMatrixDouble66;
00459 typedef CMatrixFixedNumeric<double,7,7> CMatrixDouble77;
00460 typedef CMatrixFixedNumeric<double,1,3> CMatrixDouble13;
00461 typedef CMatrixFixedNumeric<double,3,1> CMatrixDouble31;
00462 typedef CMatrixFixedNumeric<double,1,2> CMatrixDouble12;
00463 typedef CMatrixFixedNumeric<double,2,1> CMatrixDouble21;
00464 typedef CMatrixFixedNumeric<double,6,1> CMatrixDouble61;
00465 typedef CMatrixFixedNumeric<double,1,6> CMatrixDouble16;
00466 typedef CMatrixFixedNumeric<double,7,1> CMatrixDouble71;
00467 typedef CMatrixFixedNumeric<double,1,7> CMatrixDouble17;
00468 typedef CMatrixFixedNumeric<double,5,1> CMatrixDouble51;
00469 typedef CMatrixFixedNumeric<double,1,5> CMatrixDouble15;
00470
00471 typedef CMatrixFixedNumeric<float,2,2> CMatrixFloat22;
00472 typedef CMatrixFixedNumeric<float,3,3> CMatrixFloat33;
00473 typedef CMatrixFixedNumeric<float,4,4> CMatrixFloat44;
00474 typedef CMatrixFixedNumeric<float,6,6> CMatrixFloat66;
00475 typedef CMatrixFixedNumeric<float,7,7> CMatrixFloat77;
00476 typedef CMatrixFixedNumeric<float,1,3> CMatrixFloat13;
00477 typedef CMatrixFixedNumeric<float,3,1> CMatrixFloat31;
00478 typedef CMatrixFixedNumeric<float,1,2> CMatrixFloat12;
00479 typedef CMatrixFixedNumeric<float,2,1> CMatrixFloat21;
00480 typedef CMatrixFixedNumeric<float,6,1> CMatrixFloat61;
00481 typedef CMatrixFixedNumeric<float,1,6> CMatrixFloat16;
00482 typedef CMatrixFixedNumeric<float,7,1> CMatrixFloat71;
00483 typedef CMatrixFixedNumeric<float,1,7> CMatrixFloat17;
00484 typedef CMatrixFixedNumeric<float,5,1> CMatrixFloat51;
00485 typedef CMatrixFixedNumeric<float,1,5> CMatrixFloat15;
00486
00487
00488
00489 namespace detail {
00490
00491
00492
00493 template<typename T,size_t D> class VicinityTraits<CMatrixFixedNumeric<T,D,D> > {
00494 public:
00495 inline static void initialize(CMatrixFixedNumeric<T,D,D> &mat,size_t N) {
00496 ASSERT_(N==D);
00497 }
00498 inline static void insertInContainer(CMatrixFixedNumeric<T,D,D> &mat,size_t r,size_t c,const T &t) {
00499 mat.get_unsafe(r,c)=t;
00500 }
00501 };
00502 }
00503
00504
00505 }
00506
00507 namespace utils
00508 {
00509
00510 template<typename T,size_t N,size_t M> struct TTypeName <mrpt::math::CMatrixFixedNumeric<T,N,M> > {
00511 static std::string get() { return mrpt::format("CMatrixFixedNumeric<%s,%u,%u>",TTypeName<T>::get().c_str(),(unsigned int)N,(unsigned int)M); }
00512 };
00513 }
00514
00515 }
00516
00517 #endif