Main MRPT website > C++ reference
MRPT logo

eigen_plugins_impl.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 #ifndef MRPT_EIGEN_PLUGINS_IMPL_H
00030 #define MRPT_EIGEN_PLUGINS_IMPL_H
00031 
00032 // -------------------------------------------------------------------------
00033 //  This file implements some templates which had to be left only declared
00034 //   in the "plug-in" headers "eigen_plugins.h" within Eigen::MatrixBase<>
00035 // -------------------------------------------------------------------------
00036 
00037 namespace internal_mrpt
00038 {
00039         // Generic version for all kind of matrices:
00040         template<int R, int C>
00041         struct MatOrVecResizer
00042         {
00043                 template <typename S, int Opt, int MaxR, int MaxC>
00044                 static inline void doit(Eigen::Matrix<S,R,C,Opt,MaxR,MaxC> &mat, size_t new_rows,size_t new_cols)
00045                 {
00046 			::mrpt::math::detail::TAuxResizer<Eigen::Matrix<S,R,C,Opt,MaxR,MaxC>,Eigen::Matrix<S,R,C,Opt,MaxR,MaxC>::SizeAtCompileTime>::internal_resize(mat,new_rows,new_cols);
00047                         //mat.derived().conservativeResize(new_rows,new_cols);
00048                 }
00049         };
00050         // Specialization for column matrices:
00051         template<int R>
00052         struct MatOrVecResizer<R,1>
00053         {
00054                 template <typename S, int Opt, int MaxR, int MaxC>
00055                 static inline void doit(Eigen::Matrix<S,R,1,Opt,MaxR,MaxC> &mat, size_t new_rows,size_t new_cols)
00056                 {
00057 			::mrpt::math::detail::TAuxResizer<Eigen::Matrix<S,R,1,Opt,MaxR,MaxC>,Eigen::Matrix<S,R,1,Opt,MaxR,MaxC>::SizeAtCompileTime>::internal_resize(mat,new_rows);
00058                         //mat.derived().conservativeResize(new_rows);
00059                 }
00060         };
00061         // Specialization for row matrices:
00062         template<int C>
00063         struct MatOrVecResizer<1,C>
00064         {
00065                 template <typename S, int Opt, int MaxR, int MaxC>
00066                 static inline void doit(Eigen::Matrix<S,1,C,Opt,MaxR,MaxC> &mat, size_t new_rows,size_t new_cols)
00067                 {
00068 			::mrpt::math::detail::TAuxResizer<Eigen::Matrix<S,1,C,Opt,MaxR,MaxC>,Eigen::Matrix<S,1,C,Opt,MaxR,MaxC>::SizeAtCompileTime>::internal_resize(mat,new_cols);
00069                         //mat.derived().conservativeResize(new_cols);
00070                 }
00071         };
00072         template<>
00073         struct MatOrVecResizer<1,1>
00074         {
00075                 template <typename S, int Opt, int MaxR, int MaxC>
00076                 static inline void doit(Eigen::Matrix<S,1,1,Opt,MaxR,MaxC> &mat, size_t new_rows,size_t new_cols)
00077                 {
00078 			::mrpt::math::detail::TAuxResizer<Eigen::Matrix<S,1,1,Opt,MaxR,MaxC>,Eigen::Matrix<S,1,1,Opt,MaxR,MaxC>::SizeAtCompileTime>::internal_resize(mat,new_cols);
00079                         //mat.derived().conservativeResize(new_cols);
00080                 }
00081         };
00082 }
00083 
00084 /** Compute the eigenvectors and eigenvalues, both returned as matrices: eigenvectors are the columns, and eigenvalues
00085   */
00086 template <class Derived>
00087 template <class MATRIX1,class MATRIX2>
00088 EIGEN_STRONG_INLINE void Eigen::MatrixBase<Derived>::eigenVectors( MATRIX1 & eVecs, MATRIX2 & eVals ) const
00089 {
00090         Matrix<Scalar,Dynamic,1> evals;
00091         eigenVectorsVec(eVecs,evals);
00092         eVals.resize(evals.size(),evals.size());
00093         eVals.setZero();
00094         eVals.diagonal()=evals;
00095 }
00096 
00097 /** Compute the eigenvectors and eigenvalues, both returned as matrices: eigenvectors are the columns, and eigenvalues
00098   */
00099 template <class Derived>
00100 template <class MATRIX1,class VECTOR1>
00101 EIGEN_STRONG_INLINE void Eigen::MatrixBase<Derived>::eigenVectorsVec( MATRIX1 & eVecs, VECTOR1 & eVals ) const
00102 {
00103         Eigen::EigenSolver< Derived > es(*this, true);
00104         eVecs = es.eigenvectors().real(); // Keep only the real part of complex matrix
00105         eVals = es.eigenvalues().real(); // Keep only the real part of complex matrix
00106 
00107         // Sort by ascending eigenvalues:
00108         std::vector<std::pair<Scalar,Index> > D;
00109         D.reserve(eVals.size());
00110         for (Index i=0;i<eVals.size();i++)
00111                 D.push_back(std::make_pair<Scalar,Index>(eVals.coeff(i,0),i));
00112         std::sort(D.begin(),D.end());
00113         MATRIX1 sortedEigs;
00114         sortedEigs.resizeLike(eVecs);
00115         for (int i=0;i<eVals.size();i++)
00116         {
00117                 eVals.coeffRef(i,0)=D[i].first;
00118                 sortedEigs.col(i)=eVecs.col(D[i].second);
00119         }
00120         eVecs = sortedEigs;
00121 }
00122 
00123 /** Compute the eigenvectors and eigenvalues, both returned as matrices: eigenvectors are the columns, and eigenvalues
00124   */
00125 template <class Derived>
00126 template <class MATRIX1,class MATRIX2>
00127 EIGEN_STRONG_INLINE void Eigen::MatrixBase<Derived>::eigenVectorsSymmetric( MATRIX1 & eVecs, MATRIX2 & eVals ) const
00128 {
00129         Matrix<Scalar,Dynamic,1> evals;
00130         eigenVectorsSymmetricVec(eVecs,evals);
00131         eVals.resize(evals.size(),evals.size());
00132         eVals.setZero();
00133         eVals.diagonal()=evals;
00134 }
00135 
00136 /** Compute the eigenvectors and eigenvalues, both returned as matrices: eigenvectors are the columns, and eigenvalues
00137   */
00138 template <class Derived>
00139 template <class MATRIX1,class VECTOR1>
00140 EIGEN_STRONG_INLINE void Eigen::MatrixBase<Derived>::eigenVectorsSymmetricVec( MATRIX1 & eVecs, VECTOR1 & eVals ) const
00141 {
00142         // This solver returns the eigenvectors already sorted.
00143         Eigen::SelfAdjointEigenSolver<Derived> eigensolver(*this);
00144         eVecs = eigensolver.eigenvectors();
00145         eVals = eigensolver.eigenvalues();
00146 }
00147 
00148 
00149 template <class Derived>
00150 bool Eigen::MatrixBase<Derived>::fromMatlabStringFormat(const std::string &s, bool dumpErrorMsgToStdErr)
00151 {
00152         // Start with a (0,0) matrix:
00153         if ( Derived::RowsAtCompileTime==Eigen::Dynamic)
00154                 (*this) = Derived();
00155 
00156         // Look for starting "[".
00157         size_t  ini = s.find_first_not_of(" \t\r\n");
00158         if (ini==std::string::npos || s[ini]!='[') { return false; }
00159 
00160         size_t  end = s.find_last_not_of(" \t\r\n");
00161         if (end==std::string::npos || s[end]!=']') return false;
00162 
00163         if (ini>end) return false;
00164 
00165         std::vector<Scalar> lstElements;
00166 
00167         size_t i = ini+1;
00168         size_t nRow = 0;
00169 
00170         while (i<end)
00171         {
00172                 // Extract one row:
00173                 size_t end_row = s.find_first_of(";]",i);
00174                 if (end_row==std::string::npos) { return false; }
00175 
00176                 // We have one row in s[ i : (end_row-1) ]
00177                 std::stringstream  ss(s.substr(i, end_row-i ));
00178                 lstElements.clear();
00179                 try
00180                 {
00181                         while (!ss.eof())
00182                         {
00183                                 Scalar val;
00184                                 ss >> val;
00185                                 if (ss.bad() || ss.fail()) break;
00186                                 lstElements.push_back(val);
00187                         }
00188                 } catch (...) { }  // end of line
00189 
00190                 // Empty row? Only for the first row, then this is an empty matrix:
00191                 if (lstElements.empty())
00192                 {
00193                         if (nRow>0)
00194                                 return false;
00195                         else
00196                         {
00197                                 // Else, this may be an empty matrix... if there is no next row, we'll return with a (0,0) matrix
00198                                 if ( Derived::RowsAtCompileTime==Eigen::Dynamic )
00199                                         (*this) = Derived();
00200                         }
00201                 }
00202                 else
00203                 {
00204                         const size_t N = lstElements.size();
00205 
00206                         // Check valid width: All rows must have the same width
00207                         if ((nRow>0 && size_t(cols())!=N) ||
00208                                 (nRow==0 && Derived::ColsAtCompileTime!=Eigen::Dynamic && Derived::ColsAtCompileTime!=int(N)) )
00209                         {
00210                                 if (dumpErrorMsgToStdErr)
00211                                         std::cerr << "[fromMatlabStringFormat] Row " << nRow+1 << " has invalid number of columns.\n";
00212                                 return false;
00213                         }
00214 
00215                         // Append to the matrix:
00216                         if ( Derived::RowsAtCompileTime==Eigen::Dynamic || Derived::ColsAtCompileTime==Eigen::Dynamic )
00217                                 internal_mrpt::MatOrVecResizer<Derived::RowsAtCompileTime,Derived::ColsAtCompileTime>::doit(derived(),nRow+1,N);
00218                         else if (Derived::RowsAtCompileTime!=Eigen::Dynamic && int(nRow)>=Derived::RowsAtCompileTime)
00219                         {
00220                                 if (dumpErrorMsgToStdErr)
00221                                         std::cerr << "[fromMatlabStringFormat] Read more rows than the capacity of the fixed sized matrix.\n";
00222                                 return false;
00223                         }
00224 
00225                         for (size_t q=0;q<N;q++)
00226                                 coeffRef(nRow,q) = lstElements[q];
00227 
00228                         // Go for the next row:
00229                         nRow++;
00230                 }
00231 
00232                 i = end_row+1;
00233         }
00234         // For fixed sized matrices, check size:
00235         if (Derived::RowsAtCompileTime!=Eigen::Dynamic && int(nRow)!=Derived::RowsAtCompileTime)
00236         {
00237                 if (dumpErrorMsgToStdErr)
00238                         std::cerr << "[fromMatlabStringFormat] Read less rows than the capacity of the fixed sized matrix.\n";
00239                 return false;
00240         }
00241         return true; // Ok
00242 }
00243 
00244 template <class Derived>
00245 std::string  Eigen::MatrixBase<Derived>::inMatlabFormat(const size_t decimal_digits) const
00246 {
00247         std::stringstream  s;
00248         s << "[" << std::scientific;
00249         s.precision(decimal_digits);
00250         for (Index i=0;i<rows();i++)
00251         {
00252                 for (Index j=0;j<cols();j++)
00253                         s << coeff(i,j) << " ";
00254                 if (i<rows()-1) s << ";";
00255         }
00256         s << "]";
00257         return s.str();
00258 }
00259 
00260 template <class Derived>
00261 void Eigen::MatrixBase<Derived>::saveToTextFile(
00262         const std::string &file,
00263         mrpt::math::TMatrixTextFileFormat fileFormat,
00264         bool    appendMRPTHeader,
00265         const std::string &userHeader
00266         ) const
00267 {
00268         FILE *f= ::fopen(file.c_str(),"wt");
00269         if (!f)
00270                 throw std::runtime_error(std::string("saveToTextFile: Error opening file ")+file+std::string("' for writing a matrix as text."));
00271 
00272         if (!userHeader.empty())
00273                 fprintf(f,"%s",userHeader.c_str() );
00274 
00275         if (appendMRPTHeader)
00276         {
00277                 time_t rawtime;
00278                 ::time(&rawtime);
00279                 struct tm * timeinfo = ::localtime(&rawtime);
00280 
00281                 fprintf(f,"%% File generated with MRPT %s at %s\n%%-----------------------------------------------------------------\n",
00282                         mrpt::system::MRPT_getVersion().c_str(),
00283                         asctime(timeinfo) );
00284         }
00285 
00286         for (Index i=0; i < rows(); i++)
00287         {
00288                 for (Index j=0; j < cols(); j++)
00289                 {
00290                         switch(fileFormat)
00291                         {
00292                         case mrpt::math::MATRIX_FORMAT_ENG: ::fprintf(f,"%.16e",static_cast<double>(coeff(i,j))); break;
00293                         case mrpt::math::MATRIX_FORMAT_FIXED: ::fprintf(f,"%.16f",static_cast<double>(coeff(i,j))); break;
00294                         case mrpt::math::MATRIX_FORMAT_INT: ::fprintf(f,"%i",static_cast<int>(coeff(i,j))); break;
00295                         default:
00296                                 throw std::runtime_error("Unsupported value for the parameter 'fileFormat'!");
00297                         };
00298                         // Separating blank space
00299                         if (j<(cols()-1)) ::fprintf(f," ");
00300                 }
00301 		::fprintf(f,"\n");
00302         }
00303 	::fclose(f);
00304 }
00305 
00306 
00307 template <class Derived>
00308 void Eigen::MatrixBase<Derived>::loadFromTextFile(const std::string &file)
00309 {
00310         std::ifstream f(file.c_str());
00311         if (f.fail()) throw std::runtime_error(std::string("loadFromTextFile: can't open file:") + file);
00312         loadFromTextFile(f);
00313 }
00314 
00315 template <class Derived>
00316 void Eigen::MatrixBase<Derived>::loadFromTextFile(std::istream &f)
00317 {
00318         // This matrix is NROWS x NCOLS
00319         std::string             str;
00320         std::vector<double>     fil(512);
00321         size_t  nRows = 0;
00322         while ( !f.eof() )
00323         {
00324                 std::getline(f,str);
00325                 if (str.size() && str[0]!='#' && str[0]!='%')
00326                 {
00327                         // Parse row to floats:
00328                         const char *ptr = str.c_str();
00329                         char *ptrEnd = NULL;
00330                         size_t i=0;
00331                         // Process each number in this row:
00332                         while ( ptr[0] && ptr!=ptrEnd )
00333                         {
00334                                 // Find next number: (non white-space character):
00335                                 while (ptr[0] && (ptr[0]==' ' || ptr[0]=='\t' || ptr[0]=='\r' || ptr[0]=='\n'))
00336                                         ptr++;
00337                                 if (fil.size()<=i)      fil.resize(fil.size()+512);
00338                                 // Convert to "double":
00339                                 fil[i] = strtod(ptr,&ptrEnd);
00340                                 // A valid conversion has been done?
00341                                 if (ptr!=ptrEnd)
00342                                 {
00343                                         i++;    // Yes
00344                                         ptr = ptrEnd;
00345                                         ptrEnd = NULL;
00346                                 }
00347                         }; // end while procesing this row
00348 
00349                         // "i": # of columns:
00350                         if ((Derived::ColsAtCompileTime!=Eigen::Dynamic && Index(i)!=Derived::ColsAtCompileTime) )
00351                                 throw std::runtime_error("loadFromTextFile: The matrix in the text file does not match fixed matrix size");
00352                         if (Derived::ColsAtCompileTime==Eigen::Dynamic && nRows>0 && Index(i)!=cols() )
00353                                 throw std::runtime_error("loadFromTextFile: The matrix in the text file does not have the same number of columns in all rows");
00354 
00355                         // Append to the matrix:
00356                         if ( Derived::RowsAtCompileTime==Eigen::Dynamic || Derived::ColsAtCompileTime==Eigen::Dynamic )
00357                                 internal_mrpt::MatOrVecResizer<Derived::RowsAtCompileTime,Derived::ColsAtCompileTime>::doit(derived(),nRows+1,i);
00358                         else if (Derived::RowsAtCompileTime!=Eigen::Dynamic && int(nRows)>=Derived::RowsAtCompileTime)
00359                                 throw std::runtime_error("loadFromTextFile: Read more rows than the capacity of the fixed sized matrix.");
00360 
00361                         for (size_t q=0;q<i;q++)
00362                                 coeffRef(nRows,q) = Scalar(fil[q]);
00363 
00364                         nRows++;
00365                 } // end if fgets
00366         } // end while not feof
00367 
00368         // Report error as exception
00369         if (!nRows)
00370                 throw std::runtime_error("loadFromTextFile: Error loading from text file");
00371 }
00372 
00373 
00374 #endif // guard define



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