00001 /* +---------------------------------------------------------------------------+ 00002 | The Mobile Robot Programming Toolkit (MRPT) C++ library | 00003 | | 00004 | http://mrpt.sourceforge.net/ | 00005 | | 00006 | Copyright (C) 2005-2010 University of Malaga | 00007 | | 00008 | This software was written by the Machine Perception and Intelligent | 00009 | Robotics Lab, University of Malaga (Spain). | 00010 | Contact: Jose-Luis Blanco <jlblanco@ctima.uma.es> | 00011 | | 00012 | This file is part of the MRPT project. | 00013 | | 00014 | MRPT is free software: you can redistribute it and/or modify | 00015 | it under the terms of the GNU General Public License as published by | 00016 | the Free Software Foundation, either version 3 of the License, or | 00017 | (at your option) any later version. | 00018 | | 00019 | MRPT is distributed in the hope that it will be useful, | 00020 | but WITHOUT ANY WARRANTY; without even the implied warranty of | 00021 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the | 00022 | GNU General Public License for more details. | 00023 | | 00024 | You should have received a copy of the GNU General Public License | 00025 | along with MRPT. If not, see <http://www.gnu.org/licenses/>. | 00026 | | 00027 +---------------------------------------------------------------------------+ */ 00028 #ifndef CMatrixTemplateNumeric_H 00029 #define CMatrixTemplateNumeric_H 00030 00031 #include <mrpt/math/CMatrixTemplate.h> 00032 #include <mrpt/math/matrix_iterators.h> 00033 #include <mrpt/math/matrices_metaprogramming.h> // TMatrixProductType, ... 00034 #include <mrpt/math/CMatrixViews.h> 00035 00036 #include <mrpt/system/os.h> 00037 #include <cmath> 00038 #include <limits> 00039 00040 namespace mrpt 00041 { 00042 namespace poses 00043 { 00044 class CPose2D; 00045 class CPose3D; 00046 class CPoint2D; 00047 class CPoint3D; 00048 } 00049 namespace math 00050 { 00051 using namespace mrpt::system; 00052 00053 00054 /** This template class extends the class "CMatrixTemplate" with many common operations with numerical matrixes. 00055 * 00056 * The template can be instanced for data types: float, double, long double 00057 * 00058 * The following operators have been implemented: 00059 <P> 00060 <TABLE cellSpacing=1 cellPadding=1 width="75%" align=center border=1> 00061 <TR> 00062 <TD align=center width="30%"><b>Implemented Operators</b></TD> 00063 <TD width="70%"><center><b>Meaning</b></center></TD></TR> 00064 <TR> 00065 <TD align=center>x=M(i,j)<br>M(i,j)=x</TD> 00066 <TD>This () operator is used to access/change the element at i'th row, j'th column. First index is 0.</TD></TR> 00067 <TR> 00068 <TD align=center>!M</TD> 00069 <TD>The matrix inverse M<sup>-1</sup></TD></TR> 00070 <TR> 00071 <TD align=center>~M</TD> 00072 <TD>The matrix transpose M<sup>T</sup></TD></TR> 00073 <TR> 00074 <TD align=center>(M^n)</TD> 00075 <TD>Power of a matrix: (M*M*M*...M) n times. Use parenthesis with this operator. Use "scalarPow" for the power of individual elements in the matrix. </TD></TR> 00076 <TR> 00077 <TD align=center> M1 = M2</TD> 00078 <TD>Assignment operator: Copy matrix M2 to M1</TD></TR> 00079 <TR> 00080 <TD align=center>M1 == M2<br>M1 != M2</TD> 00081 <TD>Logical comparison: Returns true or false if all elements are identical.</TD></TR> 00082 <TR> 00083 <TD align=center>x * M</TD> 00084 <TD>Scalar multiplication</TD></TR> 00085 <TR> 00086 <TD align=center>M1 * M2</TD> 00087 <TD>Matrix multiplication, with the usual mathematical meaning</TD></TR> 00088 <TR> 00089 <TD align=center>M1 + M2<br>M1 M2</TD> 00090 <TD>Matrixes addition and substraction.</TD></TR> 00091 <TR> 00092 <TD align=center>M / x</TD> 00093 <TD>Scalar division</TD></TR> 00094 <TR> 00095 <TD align=center>M1 / M2</TD> 00096 <TD>Equivalent to (M1 * M2<sup>-1</sup>)</TD></TR> 00097 <TR> 00098 <TD align=center>stream << M;</TD> 00099 <TD>Write to a binary CStream, since this class is CSerializable</TD></TR> 00100 <TR> 00101 <TD align=center>stream >> M;</TD> 00102 <TD>Read from a binary CStream, since this class is CSerializable</TD></TR> 00103 </TABLE> 00104 </P> 00105 * See also this useful methods: 00106 * - SAVE_MATRIX 00107 * \sa CMatrixTemplate 00108 */ 00109 template <class T> 00110 class MRPTDLLIMPEXP CMatrixTemplateNumeric : public CMatrixTemplate<T> 00111 { 00112 public: 00113 typedef CMatrixTemplate<T> BASE; 00114 typedef CMatrixTemplateNumeric<T> mrpt_autotype; 00115 DECLARE_MRPT_CONTAINER_TYPES 00116 DECLARE_MRPT_CONTAINER_IS_MATRIX 00117 DECLARE_MRPT_MATRIX_ITERATORS 00118 DECLARE_COMMON_MATRIX_MEMBERS(T) 00119 00120 /** Copy constructor from a matrix of any type 00121 */ 00122 template <class R> 00123 CMatrixTemplateNumeric(const CMatrixTemplate<R>& m) 00124 { 00125 CMatrixTemplate<T>::realloc( m.getRowCount(), m.getColCount() ); 00126 00127 for (size_t i=0; i < CMatrixTemplate<T>::getRowCount(); i++) 00128 for (size_t j=0; j < CMatrixTemplate<T>::getColCount(); j++) 00129 CMatrixTemplate<T>::m_Val[i][j] = static_cast<T> (m.get_unsafe(i,j)); 00130 } 00131 00132 /** Default constructor, builds a 1x1 matrix */ 00133 CMatrixTemplateNumeric(); 00134 00135 /** Default constructor, builds a 0x0 matrix, with a signature identical to that of the fixed matrix constructor for uninitialized matrix constructor */ 00136 inline CMatrixTemplateNumeric(bool,bool,bool) : CMatrixTemplate<T>( 0,0 ) 00137 { 00138 } 00139 00140 /** Constructor */ 00141 CMatrixTemplateNumeric(size_t row, size_t col); 00142 00143 /** Copy & crop constructor, which copies the given matrix but only up to the given size */ 00144 CMatrixTemplateNumeric(const CMatrixTemplate<T> & m, const size_t cropRowCount, const size_t cropColCount) : CMatrixTemplate<T>(m,cropRowCount,cropColCount) 00145 { } 00146 00147 /** Constructor from a given size and a C array. The array length must match cols x row. 00148 * \code 00149 * const double numbers[] = { 00150 * 1,2,3, 00151 * 4,5,6 }; 00152 * CMatrixDouble M(3,2, numbers); 00153 * \endcode 00154 */ 00155 template <typename V, size_t N> 00156 CMatrixTemplateNumeric(size_t row, size_t col, V (&theArray)[N] ) : CMatrixTemplate<T>( row, col, theArray ) 00157 { } 00158 00159 /** 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. 00160 */ 00161 template <typename V> 00162 CMatrixTemplateNumeric(size_t row, size_t col, const V &theVector ) : CMatrixTemplate<T>( row, col, theVector ) 00163 { } 00164 00165 /** Destructor 00166 */ 00167 virtual ~CMatrixTemplateNumeric() 00168 { } 00169 00170 /** Constructor from a mrpt::poses::CPose2D, which generates a 3x1 matrix \f$ [x y \phi]^T \f$ 00171 */ 00172 explicit CMatrixTemplateNumeric( const mrpt::poses::CPose2D &p); 00173 00174 /** Constructor from a mrpt::poses::CPose6D, which generates a 6x1 matrix \f$ [x y z yaw pitch roll]^T \f$ 00175 */ 00176 explicit CMatrixTemplateNumeric( const mrpt::poses::CPose3D &p); 00177 00178 /** Constructor from a mrpt::poses::CPoint2D, which generates a 2x1 matrix \f$ [x y]^T \f$ 00179 */ 00180 explicit CMatrixTemplateNumeric( const mrpt::poses::CPoint2D &p); 00181 00182 /** Constructor from a mrpt::poses::CPoint3D, which generates a 3x1 matrix \f$ [x y z]^T \f$ 00183 */ 00184 explicit CMatrixTemplateNumeric( const mrpt::poses::CPoint3D &p); 00185 00186 /** Copy constructor from a fixed-size matrix 00187 * \note Method implemented in CMatrixFixedNumeric.h 00188 */ 00189 template <size_t NROWS,size_t NCOLS> 00190 explicit CMatrixTemplateNumeric(const CMatrixFixedNumeric<T,NROWS,NCOLS> &M ) { 00191 detail::fixedToDynMatrix(M,*this); 00192 } 00193 00194 /** Copy operator from a fixed-size matrix 00195 * \note Method implemented in CMatrixFixedNumeric.h 00196 */ 00197 template <size_t NROWS,size_t NCOLS> 00198 CMatrixTemplateNumeric& operator =(const CMatrixFixedNumeric<T,NROWS,NCOLS> &M ) { 00199 detail::fixedToDynMatrix(M,*this); 00200 return *this; 00201 } 00202 00203 /** Assignment operator of other types 00204 */ 00205 template <class R> 00206 CMatrixTemplateNumeric<T>& operator = (const CMatrixTemplateNumeric<R>& m) 00207 { 00208 CMatrixTemplate<T>::realloc( m.getRowCount(), m.getColCount() ); 00209 00210 for (size_t i=0; i < CMatrixTemplate<T>::getRowCount(); i++) 00211 for (size_t j=0; j < CMatrixTemplate<T>::getColCount(); j++) 00212 CMatrixTemplate<T>::m_Val[i][j] = static_cast<T>(m.get_unsafe(i,j)); 00213 return *this; 00214 } 00215 00216 /** Assignment operator for initializing from a C array (The matrix must be set to the correct size before invoking this asignament) 00217 * \code 00218 * CMatrixDouble M(3,2); 00219 * const double numbers[] = { 00220 * 1,2,3, 00221 * 4,5,6 }; 00222 * M = numbers; 00223 * \endcode 00224 * Refer also to the constructor with initialization data CMatrixTemplate::CMatrixTemplate 00225 */ 00226 template <typename V, size_t N> 00227 CMatrixTemplateNumeric& operator = (V (&theArray)[N] ) 00228 { 00229 CMatrixTemplate<T>::operator = (theArray); 00230 return *this; 00231 } 00232 00233 /** Assignment operator for the same type 00234 */ 00235 CMatrixTemplateNumeric<T>& operator = (const CMatrixTemplateNumeric<T>& m); 00236 00237 /** Changes the size of matrix, maintaining its previous content as possible and padding with zeros where applicable. 00238 * setSize and resize are exactly equivalent methods. 00239 */ 00240 void setSize(size_t row, size_t col); 00241 00242 /** Changes the size of matrix, maintaining its previous content as possible and padding with zeros where applicable. 00243 * setSize and resize are exactly equivalent methods. 00244 */ 00245 void resize(size_t row, size_t col); 00246 00247 /** This method just checks has no effects in this class, but raises an exception if the expected size does not match */ 00248 inline void resize(const CMatrixTemplateSize &siz) { 00249 setSize(siz[0],siz[1]); 00250 } 00251 00252 /** Computes the laplacian of the matrix, useful for graph matrixes. 00253 */ 00254 void laplacian( CMatrixTemplateNumeric<T> &ret ) const; 00255 00256 /** Computes the SVD (Singular Value Decomposition) of the matrix. 00257 * If "this" matrix is named A with dimensions M x N, this method computes: <br> 00258 * A = U * W * V' <br> 00259 * <br> 00260 * , where U is a M x N column orthogonal matrix, W is a diagonal matrix 00261 * containing the singular values, and V is a NxN matrix. <br> 00262 * This method returns the U matrix, the N elements in the diagonal of W as a vector, 00263 * and the matrix V, NOT TRANSPOSED. 00264 */ 00265 void svd(CMatrixTemplateNumeric<T> &U, std::vector<T> &W,CMatrixTemplateNumeric<T> &V) const; 00266 00267 /** Efficiently computes only the biggest eigenvector of the matrix using the Power Method, and returns it as a column vector. 00268 * The computation time for this method, in a Pentium 4 1.4Ghz is:<br> 00269 * T = 7.0867e-008*n<sup>2</sup> + 1.9191e-005*n + 0.0017494 seconds <br> 00270 * where N is the matrix size. 00271 */ 00272 CMatrixTemplateNumeric<T> largestEigenvector( 00273 T resolution = 0.01f, 00274 size_t maxIterations = 6, 00275 int *out_Iterations = NULL, 00276 float *out_estimatedResolution = NULL ) const; 00277 00278 /** combined power and assignment operator 00279 */ 00280 CMatrixTemplateNumeric<T>& operator ^= (const unsigned int& pow); 00281 00282 /** Scalar power of all elements to a given power, this is diferent of ^ operator. 00283 */ 00284 void scalarPow(T s); 00285 00286 /** Set all elements to zero 00287 */ 00288 void zeros(const size_t row, const size_t col); 00289 00290 /** Set all elements to zero 00291 */ 00292 void zeros(); 00293 00294 /** Set all elements to one 00295 */ 00296 void ones(const size_t row, const size_t col); 00297 00298 /** Set all elements to one 00299 */ 00300 void ones(); 00301 00302 /** Build an unit matrix. 00303 */ 00304 void unit (const size_t row); 00305 00306 /** Build an unit matrix. 00307 */ 00308 void unit(); 00309 00310 /** Solve the matrix as linear equations system. 00311 */ 00312 CMatrixTemplateNumeric<T> solve (const CMatrixTemplateNumeric<T>& v) const; 00313 00314 /** Computes the adjunt of matrix. 00315 */ 00316 CMatrixTemplateNumeric<T> adj() const; 00317 00318 /** Computes the rank of the matrix using a slight variation of Gauss method. 00319 */ 00320 //size_t rank(T eps=0.0) const; 00321 00322 /** Computes the cofact. 00323 */ 00324 T cofact (size_t row, size_t col) const; 00325 00326 /** Computes the cond. 00327 */ 00328 T cond(); 00329 00330 /** Checks for matrix type 00331 */ 00332 bool isDiagonal() const; 00333 00334 /** Checks for matrix type 00335 */ 00336 bool isScalar() const; 00337 00338 /** Checks for matrix type 00339 */ 00340 bool isUnit() const; 00341 00342 /** Checks for matrix type 00343 */ 00344 bool isNull() const; 00345 00346 /** Checks for matrix type 00347 */ 00348 bool isSymmetric() const; 00349 00350 /** Checks for matrix type 00351 */ 00352 bool isSkewSymmetric() const; 00353 00354 /** Checks for matrix type 00355 */ 00356 bool isUpperTriangular() const; 00357 00358 /** Checks for matrix type 00359 */ 00360 bool isLowerTriangular() const; 00361 00362 /** Round towards minus infinity modifying the matrix 00363 * (by AJOGD @ JAN-2007) 00364 */ 00365 void matrix_floor(); 00366 00367 /** Round towards minus infinity 00368 * (by AJOGD @ JAN-2007) 00369 */ 00370 void matrix_floor(CMatrixTemplateNumeric<T> &out); 00371 00372 /** Round towards plus infinity 00373 * (by AJOGD @ JAN-2007) 00374 */ 00375 void matrix_ceil(); 00376 00377 /** Finds the maximum value in the matrix, and returns its position. 00378 * (by AJOGD @ JAN-2007) 00379 */ 00380 void find_index_max_value(size_t &umax, size_t &vmax, T &max_val) const; 00381 00382 /** Finds the maximum value in the diagonal of the matrix. 00383 */ 00384 T maximumDiagonal() const; 00385 00386 /** Finds the minimum value in the matrix, and returns its position. 00387 * (by AJOGD @ JAN-2007) 00388 */ 00389 void find_index_min_value(size_t &umin, size_t &vmin, T &min_val) const; 00390 00391 /** Force symmetry in the matrix 00392 * (by AJOGD @ JAN-2007) 00393 */ 00394 void force_symmetry(); 00395 00396 /** Computes a row with the mean values of each column in the matrix. 00397 * \sa meanAndStdAll 00398 */ 00399 void mean( std::vector<T> &outMeanVector ) const; 00400 00401 /** Computes a row with the mean values of each column in the matrix and the associated vector with the standard deviation of each column. 00402 * \sa mean,meanAndStdAll 00403 */ 00404 void meanAndStd( 00405 std::vector<T> &outMeanVector, 00406 std::vector<T> &outStdVector ) const; 00407 00408 /** Computes the mean and standard deviation of all the elements in the matrix as a whole. 00409 * \sa mean,meanAndStd 00410 */ 00411 void meanAndStdAll( 00412 T &outMean, 00413 T &outStd ) const; 00414 00415 void asCol(CMatrixTemplateNumeric<T> &aux) const; 00416 00417 void asRow(CMatrixTemplateNumeric<T> &aux) const; 00418 00419 /** Finds elements whose values are a given number of times above (or below) the mean, in 1D Mahalanobis distance. 00420 * This returns two lists with the "row" and "column" indexes (i,j) of those elements m[i][j] such as: 00421 * m[i][j] > mean(matrix) + stdTimes·std(matrix) 00422 * The elements below the threshold 00423 * mean(matrix) - stdTimes·std(matrix) 00424 * can also be obtained setting "below" to "true". 00425 */ 00426 void findElementsPassingMahalanobisThreshold( 00427 double stdTimes, 00428 std::vector<size_t> &rowIndexes, 00429 std::vector<size_t> &colIndexes, 00430 bool below = false ) const; 00431 00432 /** Returns the sum of a given part of the matrix. 00433 * The default value (std::numeric_limits<size_t>::max()) for the last column/row means to sum up to the last column/row. 00434 * \sa sumAll 00435 */ 00436 T sum( 00437 size_t firstRow = 0, 00438 size_t firstCol = 0, 00439 size_t lastRow = std::numeric_limits<size_t>::max(), 00440 size_t lastCol = std::numeric_limits<size_t>::max() ) const; 00441 00442 /** Computes: R = H * C * H^t , where H is this matrix. 00443 * 00444 */ 00445 void multiplyByMatrixAndByTransposeNonSymmetric( 00446 const CMatrixTemplateNumeric<T> &C, 00447 CMatrixTemplateNumeric<T> &R, 00448 bool accumOnOutput = false, 00449 bool substractInsteadOfSum = false 00450 ) const; 00451 00452 }; // end of class definition 00453 00454 00455 /** Declares a matrix of float numbers (non serializable). 00456 * For a serializable version, use math::CMatrix 00457 * \sa CMatrixDouble, CMatrix, CMatrixD 00458 */ 00459 typedef CMatrixTemplateNumeric<float> CMatrixFloat; 00460 00461 /** Declares a matrix of double numbers (non serializable). 00462 * For a serializable version, use math::CMatrixD 00463 * \sa CMatrixFloat, CMatrix, CMatrixD 00464 */ 00465 typedef CMatrixTemplateNumeric<double> CMatrixDouble; 00466 00467 /** Declares a matrix of unsigned ints (non serializable). 00468 * \sa CMatrixDouble, CMatrixFloat 00469 */ 00470 typedef CMatrixTemplateNumeric<unsigned int> CMatrixUInt; 00471 00472 /** Declares a matrix of booleans (non serializable). 00473 * \sa CMatrixDouble, CMatrixFloat, CMatrixB 00474 */ 00475 typedef CMatrixTemplate<bool> CMatrixBool; 00476 00477 #ifdef HAVE_LONG_DOUBLE 00478 /** Declares a matrix of "long doubles" (non serializable), or of "doubles" if the compiler does not support "long double". 00479 * \sa CMatrixDouble, CMatrixFloat 00480 */ 00481 typedef CMatrixTemplateNumeric<long double> CMatrixLongDouble; 00482 #else 00483 /** Declares a matrix of "long doubles" (non serializable), or of "doubles" if the compiler does not support "long double". 00484 * \sa CMatrixDouble, CMatrixFloat 00485 */ 00486 typedef CMatrixTemplateNumeric<double> CMatrixLongDouble; 00487 #endif 00488 00489 namespace detail 00490 { 00491 // Specializations of "isMatrixTypeResizable": 00492 template <> inline bool isMatrixTypeResizable<CMatrixFloat>(const CMatrixFloat&) { return true; } 00493 template <> inline bool isMatrixTypeResizable<CMatrixDouble>(const CMatrixDouble&) { return true; } 00494 template <> inline bool isMatrixTypeResizable<CMatrixLongDouble>(const CMatrixLongDouble&) { return true; } 00495 } 00496 00497 namespace detail { 00498 /** 00499 * Vicinity traits class specialization for fixed size matrices. 00500 */ 00501 template<typename T> class VicinityTraits<CMatrixTemplateNumeric<T> > { 00502 public: 00503 inline static void initialize(CMatrixTemplateNumeric<T> &mat,size_t N) { 00504 mat.setSize(N,N); 00505 mat.fill(0); 00506 } 00507 inline static void insertInContainer(CMatrixTemplateNumeric<T> &mat,size_t r,size_t c,const T &t) { 00508 mat.get_unsafe(r,c)=t; 00509 } 00510 }; 00511 } //End of detail namespace. 00512 00513 } // End of namespace 00514 } // End of namespace 00515 00516 #endif
| Page generated by Doxygen 1.7.1 for MRPT 0.8.1 SVN:exported at Sun Sep 19 14:53:45 UTC 2010 |