Main MRPT website > C++ reference
MRPT logo

eigen_plugins.h

Go to the documentation of this file.
00001 /* +---------------------------------------------------------------------------+
00002    |          The Mobile Robot Programming Toolkit (MRPT) C++ library          |
00003    |                                                                           |
00004    |                   http://mrpt.sourceforge.net/                            |
00005    |                                                                           |
00006    |   Copyright (C) 2005-2011  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 
00029 // -------------------------------------------------------------------------
00030 // Note: This file will be included within the body of Eigen::MatrixBase
00031 // -------------------------------------------------------------------------
00032 public:
00033         /** @name MRPT plugin: Types
00034           *  @{ */
00035         typedef Scalar value_type; //!< Type of the elements
00036         // size is constant
00037         enum { static_size = RowsAtCompileTime*ColsAtCompileTime };
00038         /** @} */
00039 
00040 
00041         /** @name MRPT plugin: Basic iterators. These iterators are intended for 1D matrices only, i.e. column or row vectors.
00042           *  @{ */
00043         typedef Scalar* iterator;
00044         typedef const Scalar* const_iterator;
00045 
00046         EIGEN_STRONG_INLINE iterator begin() { return derived().data(); }
00047         EIGEN_STRONG_INLINE iterator end()   { return &(derived().data()[size()-1]); }
00048         EIGEN_STRONG_INLINE const_iterator begin() const { return derived().data(); }
00049         EIGEN_STRONG_INLINE const_iterator end() const   { return &(derived().data()[size()-1]); }
00050 
00051         /** @} */
00052 
00053 
00054         /** @name MRPT plugin: Set/get/load/save and other miscelaneous methods
00055           *  @{ */
00056 
00057         /*! Fill all the elements with a given value */
00058         EIGEN_STRONG_INLINE void fill(const Scalar v) { derived().setConstant(v); }
00059 
00060         /*! Fill all the elements with a given value */
00061         EIGEN_STRONG_INLINE void assign(const Scalar v) { derived().setConstant(v); }
00062         /*! Resize to N and set all the elements to a given value */
00063         EIGEN_STRONG_INLINE void assign(size_t N, const Scalar v) { derived().resize(N); derived().setConstant(v); }
00064 
00065         /** Get number of rows */
00066         EIGEN_STRONG_INLINE size_t getRowCount() const { return rows(); }
00067         /** Get number of columns */
00068         EIGEN_STRONG_INLINE size_t getColCount() const { return cols(); }
00069 
00070         /** Make the matrix an identity matrix (the diagonal values can be 1.0 or any other value) */
00071         EIGEN_STRONG_INLINE void unit(const size_t nRows, const Scalar diag_vals) {
00072                 if (diag_vals==1)
00073                         derived().setIdentity(nRows,nRows);
00074                 else {
00075                         derived().setZero(nRows,nRows);
00076                         derived().diagonal().setConstant(diag_vals);
00077                 }
00078         }
00079 
00080         /** Make the matrix an identity matrix  */
00081         EIGEN_STRONG_INLINE void unit() { derived().setIdentity(); }
00082         /** Make the matrix an identity matrix  */
00083         EIGEN_STRONG_INLINE void eye() { derived().setIdentity(); }
00084 
00085         /** Set all elements to zero */
00086         EIGEN_STRONG_INLINE void zeros() { derived().setZero(); }
00087         /** Resize and set all elements to zero */
00088         EIGEN_STRONG_INLINE void zeros(const size_t row,const size_t col) { derived().setZero(row,col); }
00089 
00090         /** Resize matrix and set all elements to one */
00091         EIGEN_STRONG_INLINE void  ones(const size_t row, const size_t col) { derived().setOnes(row,col); }
00092         /** Set all elements to one */
00093         EIGEN_STRONG_INLINE void  ones() { derived().setOnes(); }
00094 
00095         /** Fast but unsafe method to obtain a pointer to a given row of the matrix (Use only in time critical applications)
00096           * VERY IMPORTANT WARNING: You must be aware of the memory layout, either Column or Row-major ordering.
00097           */
00098         EIGEN_STRONG_INLINE Scalar      * get_unsafe_row(size_t row)       { return &derived().coeffRef(row,0); }
00099         EIGEN_STRONG_INLINE const Scalar* get_unsafe_row(size_t row) const { return &derived().coeff(row,0); }
00100 
00101         /** Read-only access to one element (Use with caution, bounds are not checked!) */
00102         EIGEN_STRONG_INLINE Scalar get_unsafe(const size_t row, const size_t col) const {
00103 #ifdef _DEBUG
00104                 return derived()(row,col);
00105 #else
00106                 return derived().coeff(row,col);
00107 #endif
00108         }
00109         /** Reference access to one element (Use with caution, bounds are not checked!) */
00110         EIGEN_STRONG_INLINE Scalar& get_unsafe(const size_t row, const size_t col) {
00111 #ifdef _DEBUG
00112                 return derived()(row,col);
00113 #endif
00114                 return derived().coeffRef(row,col);
00115         }
00116         /** Sets an element  (Use with caution, bounds are not checked!) */
00117         EIGEN_STRONG_INLINE void set_unsafe(const size_t row, const size_t col, const Scalar val) {
00118 #ifdef _DEBUG
00119                 derived()(row,col) = val;
00120 #endif
00121                 derived().coeffRef(row,col) = val;
00122         }
00123 
00124         /** Insert an element at the end of the container (for 1D vectors/arrays) */
00125         EIGEN_STRONG_INLINE void push_back(Scalar val)
00126         {
00127                 const Index N = size();
00128                 derived().conservativeResize(N+1);
00129                 coeffRef(N) = val;
00130         }
00131 
00132         EIGEN_STRONG_INLINE bool isSquare() const { return cols()==rows(); }
00133         EIGEN_STRONG_INLINE bool isSingular(const Scalar absThreshold = 0) const { return std::abs(derived().determinant())<absThreshold; }
00134 
00135         /** Read a matrix from a string in Matlab-like format, for example "[1 0 2; 0 4 -1]"
00136           *  The string must start with '[' and end with ']'. Rows are separated by semicolons ';' and
00137           *  columns in each row by one or more whitespaces ' ', commas ',' or tabs '\t'.
00138           *
00139           *  This format is also used for CConfigFile::read_matrix.
00140           *
00141           *  This template method can be instantiated for matrices of the types: int, long, unsinged int, unsigned long, float, double, long double
00142           *
00143           * \return true on success. false if the string is malformed, and then the matrix will be resized to 0x0.
00144           * \sa inMatlabFormat, CConfigFile::read_matrix
00145           */
00146         bool fromMatlabStringFormat(const std::string &s, bool dumpErrorMsgToStdErr = true);
00147         // Method implemented in eigen_plugins_impl.h
00148 
00149         /** Dump matrix in matlab format.
00150           *  This template method can be instantiated for matrices of the types: int, long, unsinged int, unsigned long, float, double, long double
00151           * \sa fromMatlabStringFormat
00152           */
00153         std::string  inMatlabFormat(const size_t decimal_digits = 6) const;
00154         // Method implemented in eigen_plugins_impl.h
00155 
00156         /** Save matrix to a text file, compatible with MATLAB text format (see also the methods of matrix classes themselves).
00157                 * \param theMatrix It can be a CMatrixTemplate or a CMatrixFixedNumeric.
00158                 * \param file The target filename.
00159                 * \param fileFormat See TMatrixTextFileFormat. The format of the numbers in the text file.
00160                 * \param appendMRPTHeader Insert this header to the file "% File generated by MRPT. Load with MATLAB with: VAR=load(FILENAME);"
00161                 * \param userHeader Additional text to be written at the head of the file. Typically MALAB comments "% This file blah blah". Final end-of-line is not needed.
00162                 * \sa loadFromTextFile, CMatrixTemplate::inMatlabFormat, SAVE_MATRIX
00163                 */
00164         void saveToTextFile(
00165                 const std::string &file,
00166                 mrpt::math::TMatrixTextFileFormat fileFormat = mrpt::math::MATRIX_FORMAT_ENG,
00167                 bool    appendMRPTHeader = false,
00168                 const std::string &userHeader = std::string()
00169                 ) const;
00170         // Method implemented in eigen_plugins_impl.h
00171 
00172         /** Load matrix from a text file, compatible with MATLAB text format.
00173           *  Lines starting with '%' or '#' are interpreted as comments and ignored.
00174           * \sa saveToTextFile, fromMatlabStringFormat
00175           */
00176         void  loadFromTextFile(const std::string &file);
00177         // Method implemented in eigen_plugins_impl.h
00178 
00179         //! \overload
00180         void  loadFromTextFile(std::istream &_input_text_stream);
00181         // Method implemented in eigen_plugins_impl.h
00182 
00183         EIGEN_STRONG_INLINE void multiplyColumnByScalar(size_t c, Scalar s) { derived().col(c)*=s; }
00184         EIGEN_STRONG_INLINE void multiplyRowByScalar(size_t r, Scalar s)    { derived().row(r)*=s; }
00185 
00186         EIGEN_STRONG_INLINE void swapCols(size_t i1,size_t i2) { derived().col(i1).swap( derived().col(i2) ); }
00187         EIGEN_STRONG_INLINE void swapRows(size_t i1,size_t i2) { derived().row(i1).swap( derived().row(i2) ); }
00188 
00189         EIGEN_STRONG_INLINE size_t countNonZero() const { return ((*static_cast<const Derived*>(this))!= 0).count(); }
00190 
00191         /** [VECTORS OR MATRICES] Finds the maximum value
00192           * \exception std::exception On an empty input container
00193           */
00194         EIGEN_STRONG_INLINE Scalar maximum() const
00195         {
00196                 if (size()==0) throw std::runtime_error("maximum: container is empty");
00197                 return derived().maxCoeff();
00198         }
00199 
00200         /** [VECTORS OR MATRICES] Finds the minimum value
00201           * \sa maximum, minimum_maximum
00202           * \exception std::exception On an empty input container */
00203         EIGEN_STRONG_INLINE Scalar minimum() const
00204         {
00205                 if (size()==0) throw std::runtime_error("minimum: container is empty");
00206                 return derived().minCoeff();
00207         }
00208 
00209         /** [VECTORS OR MATRICES] Compute the minimum and maximum of a container at once
00210           * \sa maximum, minimum
00211           * \exception std::exception On an empty input container */
00212         EIGEN_STRONG_INLINE void minimum_maximum(
00213                 Scalar & out_min,
00214                 Scalar & out_max) const
00215         {
00216                 out_min = minimum();
00217                 out_max = maximum();
00218         }
00219 
00220 
00221         /** [VECTORS ONLY] Finds the maximum value (and the corresponding zero-based index) from a given container.
00222           * \exception std::exception On an empty input vector
00223           */
00224         EIGEN_STRONG_INLINE Scalar maximum(size_t *maxIndex) const
00225         {
00226                 if (size()==0) throw std::runtime_error("maximum: container is empty");
00227                 Index idx;
00228                 const Scalar m = derived().maxCoeff(&idx);
00229                 if (maxIndex) *maxIndex = idx;
00230                 return m;
00231         }
00232 
00233         /** [VECTORS OR MATRICES] Finds the maximum value (and the corresponding zero-based index) from a given container.
00234           * \exception std::exception On an empty input vector
00235           */
00236         void find_index_max_value(size_t &u,size_t &v,Scalar &valMax) const
00237         {
00238                 if (cols()==0 || rows()==0) throw std::runtime_error("find_index_max_value: container is empty");
00239                 Index idx1,idx2;
00240                 valMax = derived().maxCoeff(&idx1,&idx2);
00241                 u = idx1; v = idx2;
00242         }
00243 
00244 
00245         /** [VECTORS ONLY] Finds the minimum value (and the corresponding zero-based index) from a given container.
00246           * \sa maximum, minimum_maximum
00247           * \exception std::exception On an empty input vector  */
00248         EIGEN_STRONG_INLINE Scalar minimum(size_t *minIndex) const
00249         {
00250                 if (size()==0) throw std::runtime_error("minimum: container is empty");
00251                 Index idx;
00252                 const Scalar m =derived().minCoeff(&idx);
00253                 if (minIndex) *minIndex = idx;
00254                 return m;
00255         }
00256 
00257         /** [VECTORS ONLY] Compute the minimum and maximum of a container at once
00258           * \sa maximum, minimum
00259           * \exception std::exception On an empty input vector */
00260         EIGEN_STRONG_INLINE void minimum_maximum(
00261                 Scalar & out_min,
00262                 Scalar & out_max,
00263                 size_t *minIndex,
00264                 size_t *maxIndex) const
00265         {
00266                 out_min = minimum(minIndex);
00267                 out_max = maximum(maxIndex);
00268         }
00269 
00270         /** Compute the norm-infinite of a vector ($f[ ||\mathbf{v}||_\infnty $f]), ie the maximum absolute value of the elements. */
00271         EIGEN_STRONG_INLINE Scalar norm_inf() const { return lpNorm<Eigen::Infinity>(); }
00272 
00273         /** Compute the square norm of a vector/array/matrix (the Euclidean distance to the origin, taking all the elements as a single vector). \sa norm */
00274         EIGEN_STRONG_INLINE Scalar squareNorm() const { return squaredNorm(); }
00275 
00276         /*! Sum all the elements, returning a value of the same type than the container  */
00277         EIGEN_STRONG_INLINE Scalar sumAll() const { return derived().sum(); }
00278 
00279         /** Computes the laplacian of this square graph weight matrix.
00280           *  The laplacian matrix is L = D - W, with D a diagonal matrix with the degree of each node, W the
00281           */
00282         template<typename OtherDerived>
00283         EIGEN_STRONG_INLINE void laplacian(Eigen::MatrixBase <OtherDerived>& ret) const
00284         {
00285                 if (rows()!=cols()) throw std::runtime_error("laplacian: Defined for square matrixes only");
00286                 const Index N = rows();
00287                 ret = -(*this);
00288                 for (Index i=0;i<N;i++)
00289                 {
00290                         Scalar deg = 0;
00291                         for (Index j=0;j<N;j++) deg+= derived().coeff(j,i);
00292                         ret.coeffRef(i,i)+=deg;
00293                 }
00294         }
00295 
00296 
00297         /** Changes the size of matrix, maintaining its previous content as possible and padding with zeros where applicable.
00298           *  **WARNING**: MRPT's add-on method \a setSize() pads with zeros, while Eigen's \a resize() does NOT (new elements are undefined).
00299           */
00300         EIGEN_STRONG_INLINE void setSize(size_t row, size_t col)
00301         {
00302 #ifdef _DEBUG
00303                 if ((Derived::RowsAtCompileTime!=Eigen::Dynamic && Derived::RowsAtCompileTime!=int(row)) || (Derived::ColsAtCompileTime!=Eigen::Dynamic && Derived::ColsAtCompileTime!=int(col))) {
00304                         std::stringstream ss; ss << "setSize: Trying to change a fixed sized matrix from " << rows() << "x" << cols() << " to " << row << "x" << col;
00305                         throw std::runtime_error(ss.str());
00306                 }
00307 #endif
00308                 const Index oldCols = cols();
00309                 const Index oldRows = rows();
00310                 const int nNewCols = int(col) - int(cols());
00311                 const int nNewRows = int(row) - int(rows());
00312 		::mrpt::math::detail::TAuxResizer<Eigen::MatrixBase<Derived>,SizeAtCompileTime>::internal_resize(*this,row,col);
00313                 if (nNewCols>0) derived().block(0,oldCols,row,nNewCols).setZero();
00314                 if (nNewRows>0) derived().block(oldRows,0,nNewRows,col).setZero();
00315         }
00316 
00317         /** Efficiently computes only the biggest eigenvector of the matrix using the Power Method, and returns it in the passed vector "x". */
00318         template <class OUTVECT>
00319         void largestEigenvector(
00320                 OUTVECT &x,
00321                 Scalar resolution = Scalar(0.01),
00322                 size_t maxIterations = 6,
00323                 int    *out_Iterations = NULL,
00324                 float  *out_estimatedResolution = NULL ) const
00325         {
00326                 // Apply the iterative Power Method:
00327                 size_t iter=0;
00328                 const Index n = rows();
00329                 x.resize(n);
00330                 x.setConstant(1);  // Initially, set to all ones, for example...
00331                 Scalar dif;
00332                 do  // Iterative loop:
00333                 {
00334                         Eigen::Matrix<Scalar,Derived::RowsAtCompileTime,1> xx = (*this) * x;
00335                         xx *= Scalar(1.0/xx.norm());
00336                         dif = (x-xx).array().abs().sum();       // Compute diference between iterations:
00337                         x = xx; // Set as current estimation:
00338                         iter++; // Iteration counter:
00339                 } while (iter<maxIterations && dif>resolution);
00340                 if (out_Iterations) *out_Iterations=static_cast<int>(iter);
00341                 if (out_estimatedResolution) *out_estimatedResolution=dif;
00342         }
00343 
00344         /** Combined matrix power and assignment operator */
00345         MatrixBase<Derived>& operator ^= (const unsigned int pow)
00346         {
00347                 if (pow==0)
00348                         derived().setIdentity();
00349                 else
00350                 for (unsigned int i=1;i<pow;i++)
00351                         derived() *= derived();
00352                 return *this;
00353         }
00354 
00355         /** Scalar power of all elements to a given power, this is diferent of ^ operator. */
00356         EIGEN_STRONG_INLINE void scalarPow(const Scalar s) { (*this)=array().pow(s); }
00357 
00358         /** Checks for matrix type */
00359         EIGEN_STRONG_INLINE bool isDiagonal() const
00360         {
00361                 for (Index c=0;c<cols();c++)
00362                         for (Index r=0;r<rows();r++)
00363                                 if (r!=c && coeff(r,c)!=0)
00364                                         return false;
00365                 return true;
00366         }
00367 
00368         /** Finds the maximum value in the diagonal of the matrix. */
00369         EIGEN_STRONG_INLINE Scalar maximumDiagonal() const { return diagonal().maximum(); }
00370 
00371         /** Computes the mean of the entire matrix
00372           * \sa meanAndStdAll */
00373         EIGEN_STRONG_INLINE double mean() const
00374         {
00375                 if ( size()==0) throw std::runtime_error("mean: Empty container.");
00376                 return derived().sum()/static_cast<double>(size());
00377         }
00378 
00379         /** Computes a row with the mean values of each column in the matrix and the associated vector with the standard deviation of each column.
00380           * \sa mean,meanAndStdAll \exception std::exception If the matrix/vector is empty.
00381           * \param unbiased_variance Standard deviation is sum(vals-mean)/K, with K=N-1 or N for unbiased_variance=true or false, respectively.
00382           */
00383         template <class VEC>
00384         void  meanAndStd(
00385                 VEC &outMeanVector,
00386                 VEC &outStdVector,
00387                 const bool unbiased_variance = true ) const
00388         {
00389                 const double N = rows();
00390                 if (N==0) throw std::runtime_error("meanAndStd: Empty container.");
00391                 const double N_inv = 1.0/N;
00392                 const double N_    = unbiased_variance ? (N>1 ? 1.0/(N-1) : 1.0) : 1.0/N;
00393                 outMeanVector.resize(cols());
00394                 outStdVector.resize(cols());
00395                 for (Index i=0;i<cols();i++)
00396                 {
00397                         outMeanVector[i]= col(i).array().sum() * N_inv;
00398                         outStdVector[i] = std::sqrt( (col(i).array()-outMeanVector[i]).square().sum() * N_ );
00399                 }
00400         }
00401 
00402         /** Computes the mean and standard deviation of all the elements in the matrix as a whole.
00403           * \sa mean,meanAndStd  \exception std::exception If the matrix/vector is empty.
00404           * \param unbiased_variance Standard deviation is sum(vals-mean)/K, with K=N-1 or N for unbiased_variance=true or false, respectively.
00405           */
00406         void  meanAndStdAll(
00407                 double &outMean,
00408                 double &outStd,
00409                 const bool unbiased_variance = true )  const
00410         {
00411                 const double N = size();
00412                 if (N==0) throw std::runtime_error("meanAndStdAll: Empty container.");
00413                 const double N_ = unbiased_variance ? (N>1 ? 1.0/(N-1) : 1.0) : 1.0/N;
00414                 outMean = derived().array().sum()/static_cast<double>(size());
00415                 outStd  = std::sqrt( (this->array() - outMean).square().sum()*N_);
00416         }
00417 
00418         /** Insert matrix "m" into this matrix at indices (r,c), that is, (*this)(r,c)=m(0,0) and so on */
00419         template <typename MAT>
00420         EIGEN_STRONG_INLINE void insertMatrix(size_t r,size_t c, const MAT &m) { derived().block(r,c,m.rows(),m.cols())=m; }
00421 
00422         template <typename MAT>
00423         EIGEN_STRONG_INLINE void insertMatrixTranspose(size_t r,size_t c, const MAT &m) { derived().block(r,c,m.cols(),m.rows())=m.adjoint(); }
00424 
00425         template <typename MAT> EIGEN_STRONG_INLINE void insertRow(size_t nRow, const MAT & aRow) { this->row(nRow) = aRow; }
00426         template <typename MAT> EIGEN_STRONG_INLINE void insertCol(size_t nCol, const MAT & aCol) { this->col(nCol) = aCol; }
00427 
00428         /** Remove columns of the matrix.*/
00429         EIGEN_STRONG_INLINE void removeColumns(const std::vector<size_t> &idxsToRemove)
00430         {
00431                 std::vector<size_t> idxs = idxsToRemove;
00432                 std::sort( idxs.begin(), idxs.end() );
00433                 std::vector<size_t>::iterator itEnd = std::unique( idxs.begin(), idxs.end() );
00434                 idxs.resize( itEnd - idxs.begin() );
00435 
00436                 unsafeRemoveColumns( idxs );
00437         }
00438 
00439         /** Remove columns of the matrix. The unsafe version assumes that, the indices are sorted in ascending order. */
00440         EIGEN_STRONG_INLINE void unsafeRemoveColumns(const std::vector<size_t> &idxs)
00441         {
00442                 size_t k = 1;
00443                 for (std::vector<size_t>::const_reverse_iterator it = idxs.rbegin(); it != idxs.rend(); it++, k++)
00444                 {
00445                         const size_t nC = cols() - *it - k;
00446                         if( nC > 0 )
00447                                 derived().block(0,*it,rows(),nC) = derived().block(0,*it+1,rows(),nC).eval();
00448                 }
00449                 derived().conservativeResize(NoChange,cols()-idxs.size());
00450         }
00451 
00452         /** Remove rows of the matrix. */
00453         EIGEN_STRONG_INLINE void removeRows(const std::vector<size_t> &idxsToRemove)
00454         {
00455                 std::vector<size_t> idxs = idxsToRemove;
00456                 std::sort( idxs.begin(), idxs.end() );
00457                 std::vector<size_t>::iterator itEnd = std::unique( idxs.begin(), idxs.end() );
00458                 idxs.resize( itEnd - idxs.begin() );
00459 
00460                 unsafeRemoveRows( idxs );
00461         }
00462 
00463         /** Remove rows of the matrix. The unsafe version assumes that, the indices are sorted in ascending order. */
00464         EIGEN_STRONG_INLINE void unsafeRemoveRows(const std::vector<size_t> &idxs)
00465         {
00466                 size_t k = 1;
00467                 for (std::vector<size_t>::reverse_iterator it = idxs.rbegin(); it != idxs.rend(); it++, k++)
00468                 {
00469                         const size_t nR = rows() - *it - k;
00470                         if( nR > 0 )
00471                                 derived().block(*it,0,nR,cols()) = derived().block(*it+1,0,nR,cols()).eval();
00472                 }
00473                 derived().conservativeResize(rows()-idxs.size(),NoChange);
00474         }
00475 
00476         /** Transpose */
00477         EIGEN_STRONG_INLINE const AdjointReturnType t() const { return derived().adjoint(); }
00478 
00479         EIGEN_STRONG_INLINE PlainObject inv() const { PlainObject outMat = derived().inverse().eval(); return outMat; }
00480         template <class MATRIX> EIGEN_STRONG_INLINE void inv(MATRIX &outMat) const { outMat = derived().inverse().eval(); }
00481         template <class MATRIX> EIGEN_STRONG_INLINE void inv_fast(MATRIX &outMat) const { outMat = derived().inverse().eval(); }
00482         EIGEN_STRONG_INLINE Scalar det() const { return derived().determinant(); }
00483 
00484         /** @} */  // end miscelaneous
00485 
00486 
00487         /** @name Multiply and extra addition functions
00488                 @{ */
00489 
00490         EIGEN_STRONG_INLINE bool empty() const { return this->getColCount()==0 || this->getRowCount()==0; }
00491 
00492         /*! Add c (scalar) times A to this matrix: this += A * c  */
00493         template<typename OTHERMATRIX> EIGEN_STRONG_INLINE void add_Ac(const OTHERMATRIX &m,const Scalar c)     { (*this)+=c*m; }
00494         /*! Substract c (scalar) times A to this matrix: this -= A * c  */
00495         template<typename OTHERMATRIX> EIGEN_STRONG_INLINE void substract_Ac(const OTHERMATRIX &m,const Scalar c)       { (*this) -= c*m; }
00496 
00497         /*! Substract A transposed to this matrix: this -= A.adjoint() */
00498         template<typename OTHERMATRIX> EIGEN_STRONG_INLINE void substract_At(const OTHERMATRIX &m)      { (*this) -= m.adjoint(); }
00499 
00500         /*! Substract n (integer) times A to this matrix: this -= A * n  */
00501         template<typename OTHERMATRIX> EIGEN_STRONG_INLINE void substract_An(const OTHERMATRIX& m, const size_t n)      { this->noalias() -= n * m; }
00502 
00503         /*! this += A + A<sup>T</sup>  */
00504         template<typename OTHERMATRIX> EIGEN_STRONG_INLINE void add_AAt(const OTHERMATRIX &A) { this->noalias() += A; this->noalias() += A.adjoint(); }
00505 
00506         /*! this -= A + A<sup>T</sup>  */ \
00507         template<typename OTHERMATRIX> EIGEN_STRONG_INLINE void substract_AAt(const OTHERMATRIX &A)     { this->noalias() -= A; this->noalias() -= A.adjoint(); }
00508 
00509 
00510         template <class MATRIX1,class MATRIX2> EIGEN_STRONG_INLINE void multiply( const MATRIX1& A, const MATRIX2 &B ) /*!< this = A * B */ { (*this)= A*B; }
00511 
00512         template <class MATRIX1,class MATRIX2>
00513         EIGEN_STRONG_INLINE void multiply_AB( const MATRIX1& A, const MATRIX2 &B ) /*!< this = A * B */ {
00514                 (*this)= A*B;
00515         }
00516 
00517         template <typename MATRIX1,typename MATRIX2>
00518         EIGEN_STRONG_INLINE void multiply_AtB(const MATRIX1 &A,const MATRIX2 &B) /*!< this=A^t * B */ {
00519                 *this = A.adjoint() * B;
00520         }
00521 
00522         /*! Computes the vector vOut = this * vIn, where "vIn" is a column vector of the appropriate length. */
00523         template<typename OTHERVECTOR1,typename OTHERVECTOR2>
00524         EIGEN_STRONG_INLINE void multiply_Ab(const OTHERVECTOR1 &vIn,OTHERVECTOR2 &vOut,bool accumToOutput = false) const {
00525                 if (accumToOutput) vOut.noalias() += (*this) * vIn;
00526                 else vOut = (*this) * vIn;
00527         }
00528 
00529         /*! Computes the vector vOut = this<sup>T</sup> * vIn, where "vIn" is a column vector of the appropriate length. */ \
00530         template<typename OTHERVECTOR1,typename OTHERVECTOR2>
00531         EIGEN_STRONG_INLINE void multiply_Atb(const OTHERVECTOR1 &vIn,OTHERVECTOR2 &vOut,bool accumToOutput = false) const {
00532                 if (accumToOutput) vOut.noalias() += this->adjoint() * vIn;
00533                 else vOut = this->adjoint() * vIn;
00534         }
00535 
00536         template <typename MAT_C, typename MAT_R>
00537         EIGEN_STRONG_INLINE void multiply_HCHt(const MAT_C &C,MAT_R &R,bool accumResultInOutput=false) const /*!< R = this * C * this<sup>T</sup>  */ {
00538                 if (accumResultInOutput)
00539                       R.noalias() += (*this) * C * this->adjoint();
00540                 else  R.noalias()  = (*this) * C * this->adjoint();
00541         }
00542 
00543         template <typename MAT_C, typename MAT_R>
00544         EIGEN_STRONG_INLINE void multiply_HtCH(const MAT_C &C,MAT_R &R,bool accumResultInOutput=false) const /*!< R = this<sup>T</sup> * C * this  */ {
00545                 if (accumResultInOutput)
00546                       R.noalias() += this->adjoint() * C * (*this);
00547                 else  R.noalias()  = this->adjoint() * C * (*this);
00548         }
00549 
00550         /*! R = H * C * H<sup>T</sup> (with a vector H and a symmetric matrix C) In fact when H is a vector, multiply_HCHt_scalar and multiply_HtCH_scalar are exactly equivalent */
00551         template <typename MAT_C>
00552         EIGEN_STRONG_INLINE Scalar multiply_HCHt_scalar(const MAT_C &C) const {
00553                 return ( (*this) * C * this->adjoint() ).eval()(0,0);
00554         }
00555 
00556         /*! R = H<sup>T</sup> * C * H (with a vector H and a symmetric matrix C) In fact when H is a vector, multiply_HCHt_scalar and multiply_HtCH_scalar are exactly equivalent */
00557         template <typename MAT_C>
00558         EIGEN_STRONG_INLINE Scalar multiply_HtCH_scalar(const MAT_C &C) const {
00559                 return ( this->adjoint() * C * (*this) ).eval()(0,0);
00560         }
00561 
00562         /*! this = C * C<sup>T</sup> * f (with a matrix C and a scalar f). */
00563         template<typename MAT_A>
00564         EIGEN_STRONG_INLINE void multiply_AAt_scalar(const MAT_A &A,typename MAT_A::value_type f)       {
00565                 *this = (A * A.adjoint()) * f;
00566         }
00567 
00568         /*! this = C<sup>T</sup> * C * f (with a matrix C and a scalar f). */
00569         template<typename MAT_A> EIGEN_STRONG_INLINE void multiply_AtA_scalar(const MAT_A &A,typename MAT_A::value_type f)      {
00570                 *this = (A.adjoint() * A) * f;
00571         }
00572 
00573         /*! this = A * skew(v), with \a v being a 3-vector (or 3-array) and skew(v) the skew symmetric matrix of v (see mrpt::math::skew_symmetric3) */
00574         template <class MAT_A,class SKEW_3VECTOR> void multiply_A_skew3(const MAT_A &A,const SKEW_3VECTOR &v) {
00575                 mrpt::math::multiply_A_skew3(A,v,*this); }
00576 
00577         /*! this = skew(v)*A, with \a v being a 3-vector (or 3-array) and skew(v) the skew symmetric matrix of v (see mrpt::math::skew_symmetric3) */
00578         template <class SKEW_3VECTOR,class MAT_A> void multiply_skew3_A(const SKEW_3VECTOR &v,const MAT_A &A) {
00579                 mrpt::math::multiply_skew3_A(v,A,*this); }
00580 
00581         /** outResult = this * A
00582           */
00583         template <class MAT_A,class MAT_OUT>
00584         EIGEN_STRONG_INLINE void multiply_subMatrix(const MAT_A &A,MAT_OUT &outResult,const size_t A_cols_offset,const size_t A_rows_offset,const size_t A_col_count) const  {
00585                 outResult = derived() * A.block(A_rows_offset,A_cols_offset,derived().cols(),A_col_count);
00586         }
00587 
00588         template <class MAT_A,class MAT_B,class MAT_C>
00589         void multiply_ABC(const MAT_A &A, const MAT_B &B, const MAT_C &C) /*!< this = A*B*C  */ {
00590                 *this = A*B*C;
00591         }
00592 
00593         template <class MAT_A,class MAT_B,class MAT_C>
00594         void multiply_ABCt(const MAT_A &A, const MAT_B &B, const MAT_C &C) /*!< this = A*B*(C<sup>T</sup>) */ {
00595                 *this = A*B*C.adjoint();
00596         }
00597 
00598         template <class MAT_A,class MAT_B,class MAT_C>
00599         void multiply_AtBC(const MAT_A &A, const MAT_B &B, const MAT_C &C) /*!< this = A(<sup>T</sup>)*B*C */ {
00600                 *this = A.adjoint()*B*C;
00601         }
00602 
00603         template <class MAT_A,class MAT_B>
00604         EIGEN_STRONG_INLINE void multiply_ABt(const MAT_A &A,const MAT_B &B) /*!< this = A * B<sup>T</sup> */ {
00605                 *this = A*B.adjoint();
00606         }
00607 
00608         template <class MAT_A>
00609         EIGEN_STRONG_INLINE void multiply_AAt(const MAT_A &A) /*!< this = A * A<sup>T</sup> */ {
00610                 *this = A*A.adjoint();
00611         }
00612 
00613         template <class MAT_A>
00614         EIGEN_STRONG_INLINE void multiply_AtA(const MAT_A &A) /*!< this = A<sup>T</sup> * A */ {
00615                 *this = A.adjoint()*A;
00616         }
00617 
00618         template <class MAT_A,class MAT_B>
00619         EIGEN_STRONG_INLINE void multiply_result_is_symmetric(const MAT_A &A,const MAT_B &B) /*!< this = A * B (result is symmetric) */ {
00620                 *this = A*B;
00621         }
00622 
00623 
00624         /** Matrix left divide: RES = A<sup>-1</sup> * this , with A being squared (using the Eigen::ColPivHouseholderQR method) */
00625         template<class MAT2,class MAT3 >
00626         EIGEN_STRONG_INLINE void leftDivideSquare(const MAT2 &A, MAT3 &RES) const
00627         {
00628                 Eigen::ColPivHouseholderQR<PlainObject> QR = A.colPivHouseholderQr();
00629                 if (!QR.isInvertible()) throw std::runtime_error("leftDivideSquare: Matrix A is not invertible");
00630                 RES = QR.inverse() * (*this);
00631         }
00632 
00633         /** Matrix right divide: RES = this * B<sup>-1</sup>, with B being squared  (using the Eigen::ColPivHouseholderQR method)  */
00634         template<class MAT2,class MAT3 >
00635         EIGEN_STRONG_INLINE void rightDivideSquare(const MAT2 &B, MAT3 &RES) const
00636         {
00637                 Eigen::ColPivHouseholderQR<PlainObject> QR = B.colPivHouseholderQr();
00638                 if (!QR.isInvertible()) throw std::runtime_error("rightDivideSquare: Matrix B is not invertible");
00639                 RES = (*this) * QR.inverse();
00640         }
00641 
00642         /** @} */  // end multiply functions
00643 
00644 
00645         /** @name Eigenvalue / Eigenvectors
00646             @{  */
00647 
00648         /** [For square matrices only] Compute the eigenvectors and eigenvalues (sorted), both returned as matrices: eigenvectors are the columns in "eVecs", and eigenvalues in ascending order as the diagonal of "eVals".
00649           *   \note Warning: Only the real part of complex eigenvectors and eigenvalues are returned.
00650           *   \sa eigenVectorsSymmetric, eigenVectorsVec
00651           */
00652         template <class MATRIX1,class MATRIX2>
00653         EIGEN_STRONG_INLINE void eigenVectors( MATRIX1 & eVecs, MATRIX2 & eVals ) const;
00654         // Implemented in eigen_plugins_impl.h (can't be here since Eigen::SelfAdjointEigenSolver isn't defined yet at this point.
00655 
00656         /** [For square matrices only] Compute the eigenvectors and eigenvalues (sorted), eigenvectors are the columns in "eVecs", and eigenvalues are returned in in ascending order in the vector "eVals".
00657           *   \note Warning: Only the real part of complex eigenvectors and eigenvalues are returned.
00658           *   \sa eigenVectorsSymmetric, eigenVectorsVec
00659           */
00660         template <class MATRIX1,class VECTOR1>
00661         EIGEN_STRONG_INLINE void eigenVectorsVec( MATRIX1 & eVecs, VECTOR1 & eVals ) const;
00662         // Implemented in eigen_plugins_impl.h
00663 
00664         /** [For square matrices only] Compute the eigenvectors and eigenvalues (sorted), and return only the eigenvalues in the vector "eVals".
00665           *   \note Warning: Only the real part of complex eigenvectors and eigenvalues are returned.
00666           *   \sa eigenVectorsSymmetric, eigenVectorsVec
00667           */
00668         template <class VECTOR>
00669         EIGEN_STRONG_INLINE void eigenValues( VECTOR & eVals ) const
00670         {
00671                 PlainObject eVecs;
00672                 eVecs.resizeLike(*this);
00673                 this->eigenVectorsVec(eVecs,eVals);
00674         }
00675 
00676         /** [For symmetric matrices only] Compute the eigenvectors and eigenvalues (in no particular order), both returned as matrices: eigenvectors are the columns, and eigenvalues \sa eigenVectors
00677           */
00678         template <class MATRIX1,class MATRIX2>
00679         EIGEN_STRONG_INLINE void eigenVectorsSymmetric( MATRIX1 & eVecs, MATRIX2 & eVals ) const;
00680         // Implemented in eigen_plugins_impl.h (can't be here since Eigen::SelfAdjointEigenSolver isn't defined yet at this point.
00681 
00682         /** [For symmetric matrices only] Compute the eigenvectors and eigenvalues (in no particular order), both returned as matrices: eigenvectors are the columns, and eigenvalues \sa eigenVectorsVec
00683           */
00684         template <class MATRIX1,class VECTOR1>
00685         EIGEN_STRONG_INLINE void eigenVectorsSymmetricVec( MATRIX1 & eVecs, VECTOR1 & eVals ) const;
00686         // Implemented in eigen_plugins_impl.h
00687 
00688 
00689         /** @} */  // end eigenvalues
00690 
00691 
00692 
00693         /** @name Linear algebra & decomposition-based methods
00694             @{ */
00695 
00696         /** Cholesky M=U<sup>T</sup> * U decomposition for simetric matrix (upper-half of the matrix will be actually ignored) */
00697         template <class MATRIX> EIGEN_STRONG_INLINE bool chol(MATRIX &U) const
00698         {
00699                 Eigen::LLT<PlainObject> Chol = derived().selfadjointView<Eigen::Lower>().llt();
00700                 if (Chol.info()==Eigen::NoConvergence)
00701                         return false;
00702                 U = PlainObject(Chol.matrixU());
00703                 return true;
00704         }
00705 
00706         /** Gets the rank of the matrix via the Eigen::ColPivHouseholderQR method
00707           * \param threshold If set to >0, it's used as threshold instead of Eigen's default one.
00708           */
00709         EIGEN_STRONG_INLINE size_t rank(double threshold=0) const
00710         {
00711                 Eigen::ColPivHouseholderQR<PlainObject> QR = this->colPivHouseholderQr();
00712                 if (threshold>0) QR.setThreshold(threshold);
00713                 return QR.rank();
00714         }
00715 
00716         /** @} */   // end linear algebra
00717 
00718 
00719 
00720         /** @name Scalar and element-wise extra operators
00721             @{ */
00722 
00723         EIGEN_STRONG_INLINE MatrixBase<Derived>& Sqrt()       { (*this) = this->array().sqrt(); return *this; }
00724         EIGEN_STRONG_INLINE PlainObject          Sqrt() const { PlainObject res = this->array().sqrt(); return res; }
00725 
00726         EIGEN_STRONG_INLINE MatrixBase<Derived>& Abs()       { (*this) = this->array().abs(); return *this; }
00727         EIGEN_STRONG_INLINE PlainObject          Abs() const { PlainObject res = this->array().abs(); return res; }
00728 
00729         EIGEN_STRONG_INLINE MatrixBase<Derived>& Log()       { (*this) = this->array().log(); return *this; }
00730         EIGEN_STRONG_INLINE PlainObject          Log() const { PlainObject res = this->array().log(); return res; }
00731 
00732         EIGEN_STRONG_INLINE MatrixBase<Derived>& Exp()       { (*this) = this->array().exp(); return *this; }
00733         EIGEN_STRONG_INLINE PlainObject          Exp() const { PlainObject res = this->array().exp(); return res; }
00734 
00735         EIGEN_STRONG_INLINE MatrixBase<Derived>& Square()       { (*this) = this->array().square(); return *this; }
00736         EIGEN_STRONG_INLINE PlainObject          Square() const { PlainObject res = this->array().square(); return res; }
00737 
00738         /** Scales all elements such as the minimum & maximum values are shifted to the given values */
00739         void normalize(Scalar valMin, Scalar valMax)
00740         {
00741                 if (size()==0) return;
00742                 Scalar curMin,curMax;
00743                 minimum_maximum(curMin,curMax);
00744                 Scalar minMaxDelta = curMax - curMin;
00745                 if (minMaxDelta==0) minMaxDelta = 1;
00746                 const Scalar minMaxDelta_ = (valMax-valMin)/minMaxDelta;
00747                 this->array() = (this->array()-curMin)*minMaxDelta_+valMin;
00748         }
00749         //! \overload
00750         inline void adjustRange(Scalar valMin, Scalar valMax) { normalize(valMin,valMax); }
00751 
00752         /** @} */  // end Scalar
00753 
00754 
00755         /** Extract one row from the matrix into a row vector */
00756         template <class OtherDerived> EIGEN_STRONG_INLINE void extractRow(size_t nRow, Eigen::EigenBase<OtherDerived> &v, size_t startingCol = 0) const {
00757                 v = derived().block(nRow,startingCol,1,cols()-startingCol);
00758         }
00759         //! \overload
00760         template <typename T> inline void extractRow(size_t nRow, std::vector<T> &v, size_t startingCol = 0) const {
00761                 const size_t N = cols()-startingCol;
00762                 v.resize(N);
00763                 for (size_t i=0;i<N;i++) v[i]=(*this)(nRow,startingCol+i);
00764         }
00765         /** Extract one row from the matrix into a column vector */
00766         template <class VECTOR> EIGEN_STRONG_INLINE void extractRowAsCol(size_t nRow, VECTOR &v, size_t startingCol = 0) const
00767         {
00768                 v = derived().adjoint().block(startingCol,nRow,cols()-startingCol,1);
00769         }
00770 
00771 
00772         /** Extract one column from the matrix into a column vector */
00773         template <class VECTOR> EIGEN_STRONG_INLINE void extractCol(size_t nCol, VECTOR &v, size_t startingRow = 0) const {
00774                 v = derived().block(startingRow,nCol,rows()-startingRow,1);
00775         }
00776         //! \overload
00777         template <typename T> inline void extractCol(size_t nCol, std::vector<T> &v, size_t startingRow = 0) const {
00778                 const size_t N = rows()-startingRow;
00779                 v.resize(N);
00780                 for (size_t i=0;i<N;i++) v[i]=(*this)(startingRow+i,nCol);
00781         }
00782 
00783         template <class MATRIX> EIGEN_STRONG_INLINE void extractMatrix(const size_t firstRow, const size_t firstCol, MATRIX &m) const
00784         {
00785                 m = derived().block(firstRow,firstCol,m.rows(),m.cols());
00786         }
00787         template <class MATRIX> EIGEN_STRONG_INLINE void extractMatrix(const size_t firstRow, const size_t firstCol, const size_t nRows, const size_t nCols, MATRIX &m) const
00788         {
00789                 m.resize(nRows,nCols);
00790                 m = derived().block(firstRow,firstCol,nRows,nCols);
00791         }
00792 
00793         /** Get a submatrix, given its bounds: first & last column and row (inclusive). */
00794         template <class MATRIX>
00795         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
00796         {
00797                 out.resize(row_last-row_first+1,col_last-col_first+1);
00798                 out = derived().block(row_first,col_first,row_last-row_first+1,col_last-col_first+1);
00799         }
00800 
00801         /** Get a submatrix from a square matrix, by collecting the elements M(idxs,idxs), where idxs is a sequence {block_indices(i):block_indices(i)+block_size-1} for all "i" up to the size of block_indices.
00802           *  A perfect application of this method is in extracting covariance matrices of a subset of variables from the full covariance matrix.
00803           * \sa extractSubmatrix, extractSubmatrixSymmetrical
00804           */
00805         template <class MATRIX>
00806         void extractSubmatrixSymmetricalBlocks(
00807                 const size_t                    block_size,
00808                 const std::vector<size_t>       &block_indices,
00809                 MATRIX& out) const
00810         {
00811                 if (block_size<1) throw std::runtime_error("extractSubmatrixSymmetricalBlocks: block_size must be >=1");
00812                 if (cols()!=rows()) throw std::runtime_error("extractSubmatrixSymmetricalBlocks: Matrix is not square.");
00813 
00814                 const size_t N = block_indices.size();
00815                 const size_t nrows_out=N*block_size;
00816                 out.resize(nrows_out,nrows_out);
00817                 if (!N) return; // Done
00818                 for (size_t dst_row_blk=0;dst_row_blk<N; ++dst_row_blk )
00819                 {
00820                         for (size_t dst_col_blk=0;dst_col_blk<N; ++dst_col_blk )
00821                         {
00822 #if defined(_DEBUG)
00823                                 if (block_indices[dst_col_blk]*block_size + block_size-1>=size_t(cols())) throw std::runtime_error("extractSubmatrixSymmetricalBlocks: Indices out of range!");
00824 #endif
00825                                 out.block(dst_row_blk * block_size,dst_col_blk * block_size, block_size,block_size)
00826                                 =
00827                                 derived().block(block_indices[dst_row_blk] * block_size, block_indices[dst_col_blk] * block_size, block_size,block_size);
00828                         }
00829                 }
00830         }
00831 
00832 
00833         /** Get a submatrix from a square matrix, by collecting the elements M(idxs,idxs), where idxs is the sequence of indices passed as argument.
00834           *  A perfect application of this method is in extracting covariance matrices of a subset of variables from the full covariance matrix.
00835           * \sa extractSubmatrix, extractSubmatrixSymmetricalBlocks
00836           */
00837         template <class MATRIX>
00838         void extractSubmatrixSymmetrical(
00839                 const std::vector<size_t>       &indices,
00840                 MATRIX& out) const
00841         {
00842                 if (cols()!=rows()) throw std::runtime_error("extractSubmatrixSymmetrical: Matrix is not square.");
00843 
00844                 const size_t N = indices.size();
00845                 const size_t nrows_out=N;
00846                 out.resize(nrows_out,nrows_out);
00847                 if (!N) return; // Done
00848                 for (size_t dst_row_blk=0;dst_row_blk<N; ++dst_row_blk )
00849                         for (size_t dst_col_blk=0;dst_col_blk<N; ++dst_col_blk )
00850                                 out.coeffRef(dst_row_blk,dst_col_blk) = this->coeff(indices[dst_row_blk],indices[dst_col_blk]);
00851         }
00852 



Page generated by Doxygen 1.7.3 for MRPT 0.9.4 SVN: at Sat Mar 26 06:40:17 UTC 2011