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
00029 #ifndef MRPT_EIGEN_PLUGINS_IMPL_H
00030 #define MRPT_EIGEN_PLUGINS_IMPL_H
00031
00032
00033
00034
00035
00036
00037 namespace internal_mrpt
00038 {
00039
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
00048 }
00049 };
00050
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
00059 }
00060 };
00061
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
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
00080 }
00081 };
00082 }
00083
00084
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
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();
00105 eVals = es.eigenvalues().real();
00106
00107
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
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
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
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
00153 if ( Derived::RowsAtCompileTime==Eigen::Dynamic)
00154 (*this) = Derived();
00155
00156
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
00173 size_t end_row = s.find_first_of(";]",i);
00174 if (end_row==std::string::npos) { return false; }
00175
00176
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 (...) { }
00189
00190
00191 if (lstElements.empty())
00192 {
00193 if (nRow>0)
00194 return false;
00195 else
00196 {
00197
00198 if ( Derived::RowsAtCompileTime==Eigen::Dynamic )
00199 (*this) = Derived();
00200 }
00201 }
00202 else
00203 {
00204 const size_t N = lstElements.size();
00205
00206
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
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
00229 nRow++;
00230 }
00231
00232 i = end_row+1;
00233 }
00234
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;
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
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
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
00328 const char *ptr = str.c_str();
00329 char *ptrEnd = NULL;
00330 size_t i=0;
00331
00332 while ( ptr[0] && ptr!=ptrEnd )
00333 {
00334
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
00339 fil[i] = strtod(ptr,&ptrEnd);
00340
00341 if (ptr!=ptrEnd)
00342 {
00343 i++;
00344 ptr = ptrEnd;
00345 ptrEnd = NULL;
00346 }
00347 };
00348
00349
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
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 }
00366 }
00367
00368
00369 if (!nRows)
00370 throw std::runtime_error("loadFromTextFile: Error loading from text file");
00371 }
00372
00373
00374 #endif // guard define