Main MRPT website > C++ reference for MRPT 1.4.0
eigen_plugins_impl.h
Go to the documentation of this file.
1 /* +---------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | http://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2016, Individual contributors, see AUTHORS file |
6  | See: http://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See details in http://www.mrpt.org/License |
8  +---------------------------------------------------------------------------+ */
9 
10 #ifndef MRPT_EIGEN_PLUGINS_IMPL_H
11 #define MRPT_EIGEN_PLUGINS_IMPL_H
12 
13 // -------------------------------------------------------------------------
14 // This file implements some templates which had to be left only declared
15 // in the "plug-in" headers "eigen_plugins.h" within Eigen::MatrixBase<>
16 // -------------------------------------------------------------------------
17 
18 namespace internal_mrpt
19 {
20  // Generic version for all kind of matrices:
21  template<int R, int C>
23  {
24  template <typename S, int Opt, int MaxR, int MaxC>
25  static inline void doit(Eigen::Matrix<S,R,C,Opt,MaxR,MaxC> &mat, size_t new_rows,size_t new_cols)
26  {
27  ::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);
28  }
29  };
30  // Specialization for column matrices:
31  template<int R>
32  struct MatOrVecResizer<R,1>
33  {
34  template <typename S, int Opt, int MaxR, int MaxC>
35  static inline void doit(Eigen::Matrix<S,R,1,Opt,MaxR,MaxC> &mat, size_t new_rows,size_t )
36  {
37  ::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);
38  }
39  };
40  // Specialization for row matrices:
41  template<int C>
42  struct MatOrVecResizer<1,C>
43  {
44  template <typename S, int Opt, int MaxR, int MaxC>
45  static inline void doit(Eigen::Matrix<S,1,C,Opt,MaxR,MaxC> &mat, size_t ,size_t new_cols)
46  {
47  ::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);
48  }
49  };
50  template<>
51  struct MatOrVecResizer<1,1>
52  {
53  template <typename S, int Opt, int MaxR, int MaxC>
54  static inline void doit(Eigen::Matrix<S,1,1,Opt,MaxR,MaxC> &mat, size_t ,size_t new_cols)
55  {
56  ::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);
57  }
58  };
59 }
60 
61 /** Compute the eigenvectors and eigenvalues, both returned as matrices: eigenvectors are the columns, and eigenvalues
62  */
63 template <class Derived>
64 template <class MATRIX1,class MATRIX2>
65 EIGEN_STRONG_INLINE void Eigen::MatrixBase<Derived>::eigenVectors( MATRIX1 & eVecs, MATRIX2 & eVals ) const
66 {
67  Matrix<Scalar,Dynamic,1> evals;
68  eigenVectorsVec(eVecs,evals);
69  eVals.resize(evals.size(),evals.size());
70  eVals.setZero();
71  eVals.diagonal()=evals;
72 }
73 
74 /** Compute the eigenvectors and eigenvalues, both returned as matrices: eigenvectors are the columns, and eigenvalues
75  */
76 template <class Derived>
77 template <class MATRIX1,class VECTOR1>
78 EIGEN_STRONG_INLINE void Eigen::MatrixBase<Derived>::eigenVectorsVec( MATRIX1 & eVecs, VECTOR1 & eVals ) const
79 {
80  Eigen::EigenSolver< Derived > es(*this, true);
81  eVecs = es.eigenvectors().real(); // Keep only the real part of complex matrix
82  eVals = es.eigenvalues().real(); // Keep only the real part of complex matrix
83 
84  // Sort by ascending eigenvalues:
85  std::vector<std::pair<Scalar,Index> > D;
86  D.reserve(eVals.size());
87  for (Index i=0;i<eVals.size();i++)
88  D.push_back(std::pair<Scalar,Index>(eVals.coeff(i,0),i));
89  std::sort(D.begin(),D.end());
90  MATRIX1 sortedEigs;
91  sortedEigs.resizeLike(eVecs);
92  for (int i=0;i<eVals.size();i++)
93  {
94  eVals.coeffRef(i,0)=D[i].first;
95  sortedEigs.col(i)=eVecs.col(D[i].second);
96  }
97  eVecs = sortedEigs;
98 }
99 
100 /** Compute the eigenvectors and eigenvalues, both returned as matrices: eigenvectors are the columns, and eigenvalues
101  */
102 template <class Derived>
103 template <class MATRIX1,class MATRIX2>
104 EIGEN_STRONG_INLINE void Eigen::MatrixBase<Derived>::eigenVectorsSymmetric( MATRIX1 & eVecs, MATRIX2 & eVals ) const
105 {
106  Matrix<Scalar,Dynamic,1> evals;
107  eigenVectorsSymmetricVec(eVecs,evals);
108  eVals.resize(evals.size(),evals.size());
109  eVals.setZero();
110  eVals.diagonal()=evals;
111 }
112 
113 /** Compute the eigenvectors and eigenvalues, both returned as matrices: eigenvectors are the columns, and eigenvalues
114  */
115 template <class Derived>
116 template <class MATRIX1,class VECTOR1>
117 EIGEN_STRONG_INLINE void Eigen::MatrixBase<Derived>::eigenVectorsSymmetricVec( MATRIX1 & eVecs, VECTOR1 & eVals ) const
118 {
119  // This solver returns the eigenvectors already sorted.
120  Eigen::SelfAdjointEigenSolver<Derived> eigensolver(*this);
121  eVecs = eigensolver.eigenvectors();
122  eVals = eigensolver.eigenvalues();
123 }
124 
125 
126 template <class Derived>
127 bool Eigen::MatrixBase<Derived>::fromMatlabStringFormat(const std::string &s, std::ostream *dump_errors_here)
128 {
129  // Start with a (0,0) matrix:
130  if ( Derived::RowsAtCompileTime==Eigen::Dynamic)
131  (*this) = Derived();
132 
133  // Look for starting "[".
134  size_t ini = s.find_first_not_of(" \t\r\n");
135  if (ini==std::string::npos || s[ini]!='[') { return false; }
136 
137  size_t end = s.find_last_not_of(" \t\r\n");
138  if (end==std::string::npos || s[end]!=']') return false;
139 
140  if (ini>end) return false;
141 
142  std::vector<Scalar> lstElements;
143 
144  size_t i = ini+1;
145  size_t nRow = 0;
146 
147  while (i<end)
148  {
149  // Extract one row:
150  size_t end_row = s.find_first_of(";]",i);
151  if (end_row==std::string::npos) { return false; }
152 
153  // We have one row in s[ i : (end_row-1) ]
154  std::stringstream ss(s.substr(i, end_row-i ));
155  lstElements.clear();
156  try
157  {
158  while (!ss.eof())
159  {
160  Scalar val;
161  ss >> val;
162  if (ss.bad() || ss.fail()) break;
163  lstElements.push_back(val);
164  }
165  } catch (...) { } // end of line
166 
167  // Empty row? Only for the first row, then this is an empty matrix:
168  if (lstElements.empty())
169  {
170  if (nRow>0)
171  return false;
172  else
173  {
174  // Else, this may be an empty matrix... if there is no next row, we'll return with a (0,0) matrix
175  if ( Derived::RowsAtCompileTime==Eigen::Dynamic )
176  (*this) = Derived();
177  }
178  }
179  else
180  {
181  const size_t N = lstElements.size();
182 
183  // Check valid width: All rows must have the same width
184  if ((nRow>0 && size_t(cols())!=N) ||
185  (nRow==0 && Derived::ColsAtCompileTime!=Eigen::Dynamic && Derived::ColsAtCompileTime!=int(N)) )
186  {
187  if (dump_errors_here)
188  (*dump_errors_here) << "[fromMatlabStringFormat] Row " << nRow+1 << " has invalid number of columns.\n";
189  return false;
190  }
191 
192  // Append to the matrix:
193  if ( Derived::RowsAtCompileTime==Eigen::Dynamic || Derived::ColsAtCompileTime==Eigen::Dynamic )
195  else if (Derived::RowsAtCompileTime!=Eigen::Dynamic && int(nRow)>=Derived::RowsAtCompileTime)
196  {
197  if (dump_errors_here)
198  (*dump_errors_here) << "[fromMatlabStringFormat] Read more rows than the capacity of the fixed sized matrix.\n";
199  return false;
200  }
201 
202  for (size_t q=0;q<N;q++)
203  coeffRef(nRow,q) = lstElements[q];
204 
205  // Go for the next row:
206  nRow++;
207  }
208 
209  i = end_row+1;
210  }
211  // For fixed sized matrices, check size:
212  if (Derived::RowsAtCompileTime!=Eigen::Dynamic && int(nRow)!=Derived::RowsAtCompileTime)
213  {
214  if (dump_errors_here)
215  (*dump_errors_here) << "[fromMatlabStringFormat] Read less rows than the capacity of the fixed sized matrix.\n";
216  return false;
217  }
218  return true; // Ok
219 }
220 
221 template <class Derived>
222 std::string Eigen::MatrixBase<Derived>::inMatlabFormat(const size_t decimal_digits) const
223 {
224  std::stringstream s;
225  s << "[" << std::scientific;
226  s.precision(decimal_digits);
227  for (Index i=0;i<rows();i++)
228  {
229  for (Index j=0;j<cols();j++)
230  s << coeff(i,j) << " ";
231  if (i<rows()-1) s << ";";
232  }
233  s << "]";
234  return s.str();
235 }
236 
237 template <class Derived>
239  const std::string &file,
241  bool appendMRPTHeader,
242  const std::string &userHeader
243  ) const
244 {
245 #if defined(_MSC_VER) && (_MSC_VER>=1400) // Use a secure version in Visual Studio 2005+
246  FILE *f;
247  if (0!=::fopen_s(&f,file.c_str(),"wt")) f= NULL;
248 #else
249  FILE *f= ::fopen(file.c_str(),"wt");
250 #endif
251  if (!f)
252  throw std::runtime_error(std::string("saveToTextFile: Error opening file ")+file+std::string("' for writing a matrix as text."));
253 
254  if (!userHeader.empty())
255  fprintf(f,"%s",userHeader.c_str() );
256 
257  if (appendMRPTHeader)
258  {
259  time_t rawtime;
260  ::time(&rawtime);
261 #if defined(_MSC_VER) && (_MSC_VER>=1400) // Use a secure version in Visual Studio 2005+
262  struct tm timeinfo_data;
263  struct tm * timeinfo;
264  if (0!=::localtime_s(&timeinfo_data,&rawtime)) timeinfo=NULL;
265  else timeinfo = &timeinfo_data;
266 #else
267  struct tm * timeinfo = ::localtime(&rawtime);
268 #endif
269 
270 #if defined(_MSC_VER) && (_MSC_VER>=1400) // Use a secure version in Visual Studio 2005+
271  char strTimeBuf[100];
272  if (0!=asctime_s(strTimeBuf,sizeof(strTimeBuf),timeinfo)) strTimeBuf[0]='\0';
273  char *strTime = &strTimeBuf[0];
274 #else
275  char *strTime = asctime(timeinfo);
276 #endif
277  fprintf(f,"%% File generated with %s at %s\n%%-----------------------------------------------------------------\n",
279  strTime);
280  }
281 
282  for (Index i=0; i < rows(); i++)
283  {
284  for (Index j=0; j < cols(); j++)
285  {
286  switch(fileFormat)
287  {
288  case mrpt::math::MATRIX_FORMAT_ENG: ::fprintf(f,"%.16e",static_cast<double>(coeff(i,j))); break;
289  case mrpt::math::MATRIX_FORMAT_FIXED: ::fprintf(f,"%.16f",static_cast<double>(coeff(i,j))); break;
290  case mrpt::math::MATRIX_FORMAT_INT: ::fprintf(f,"%i",static_cast<int>(coeff(i,j))); break;
291  default:
292  throw std::runtime_error("Unsupported value for the parameter 'fileFormat'!");
293  };
294  // Separating blank space
295  if (j<(cols()-1)) ::fprintf(f," ");
296  }
297  ::fprintf(f,"\n");
298  }
299  ::fclose(f);
300 }
301 
302 
303 template <class Derived>
304 void Eigen::MatrixBase<Derived>::loadFromTextFile(const std::string &file)
305 {
306  std::ifstream f(file.c_str());
307  if (f.fail()) throw std::runtime_error(std::string("loadFromTextFile: can't open file:") + file);
308  loadFromTextFile(f);
309 }
310 
311 template <class Derived>
313 {
314  // This matrix is NROWS x NCOLS
315  std::string str;
316  std::vector<double> fil(512);
317  size_t nRows = 0;
318  while ( !f.eof() && !f.fail() )
319  {
320  std::getline(f,str);
321  if (str.size() && str[0]!='#' && str[0]!='%')
322  {
323  // Parse row to floats:
324  const char *ptr = str.c_str();
325  char *ptrEnd = NULL;
326  size_t i=0;
327  // Process each number in this row:
328  while ( ptr[0] && ptr!=ptrEnd )
329  {
330  // Find next number: (non white-space character):
331  while (ptr[0] && (ptr[0]==' ' || ptr[0]==',' || ptr[0]=='\t' || ptr[0]=='\r' || ptr[0]=='\n'))
332  ptr++;
333  if (fil.size()<=i) fil.resize(fil.size()+ (fil.size()>>1));
334  // Convert to "double":
335  fil[i] = strtod(ptr,&ptrEnd);
336  // A valid conversion has been done?
337  if (ptr!=ptrEnd)
338  {
339  i++; // Yes
340  ptr = ptrEnd;
341  ptrEnd = NULL;
342  }
343  }; // end while procesing this row
344 
345  // "i": # of columns:
346  if ((Derived::ColsAtCompileTime!=Eigen::Dynamic && Index(i)!=Derived::ColsAtCompileTime) )
347  throw std::runtime_error("loadFromTextFile: The matrix in the text file does not match fixed matrix size");
348  if (Derived::ColsAtCompileTime==Eigen::Dynamic && nRows>0 && Index(i)!=cols() )
349  throw std::runtime_error("loadFromTextFile: The matrix in the text file does not have the same number of columns in all rows");
350 
351  // Append to the matrix:
352  if ( Derived::RowsAtCompileTime==Eigen::Dynamic || Derived::ColsAtCompileTime==Eigen::Dynamic )
353  {
354  if (rows()<static_cast<int>(nRows+1) || cols()<static_cast<int>(i))
355  {
356  const size_t extra_rows = std::max(static_cast<size_t>(1), nRows >> 1 );
358  }
359  }
360  else if (Derived::RowsAtCompileTime!=Eigen::Dynamic && int(nRows)>=Derived::RowsAtCompileTime)
361  throw std::runtime_error("loadFromTextFile: Read more rows than the capacity of the fixed sized matrix.");
362 
363  for (size_t q=0;q<i;q++)
364  coeffRef(nRows,q) = Scalar(fil[q]);
365 
366  nRows++;
367  } // end if fgets
368  } // end while not feof
369 
370  // Final resize to the real size (in case we allocated space in advance):
371  if ( Derived::RowsAtCompileTime==Eigen::Dynamic || Derived::ColsAtCompileTime==Eigen::Dynamic )
373 
374  // Report error as exception
375  if (!nRows) throw std::runtime_error("loadFromTextFile: Error loading from text file");
376 }
377 
378 
379 #endif // guard define
EIGEN_STRONG_INLINE void eigenVectorsSymmetricVec(MATRIX1 &eVecs, VECTOR1 &eVals) const
[For symmetric matrices only] Compute the eigenvectors and eigenvalues (in no particular order)...
EIGEN_STRONG_INLINE iterator end()
Definition: eigen_plugins.h:27
engineering format &#39;e&#39;
Definition: math_frwds.h:66
void saveToTextFile(const std::string &file, mrpt::math::TMatrixTextFileFormat fileFormat=mrpt::math::MATRIX_FORMAT_ENG, bool appendMRPTHeader=false, const std::string &userHeader=std::string()) const
Save matrix to a text file, compatible with MATLAB text format (see also the methods of matrix classe...
int BASE_IMPEXP void BASE_IMPEXP fclose(FILE *f)
An OS-independent version of fscanf.
EIGEN_STRONG_INLINE void eigenVectorsSymmetric(MATRIX1 &eVecs, MATRIX2 &eVals) const
[For symmetric matrices only] Compute the eigenvectors and eigenvalues (in no particular order)...
static void doit(Eigen::Matrix< S, 1, 1, Opt, MaxR, MaxC > &mat, size_t, size_t new_cols)
std::string BASE_IMPEXP MRPT_getVersion()
Returns a string describing the MRPT version.
static void doit(Eigen::Matrix< S, R, 1, Opt, MaxR, MaxC > &mat, size_t new_rows, size_t)
TMatrixTextFileFormat
Definition: math_frwds.h:64
std::string inMatlabFormat(const size_t decimal_digits=6) const
Dump matrix in matlab format.
intergers &#39;i&#39;
Definition: math_frwds.h:68
void loadFromTextFile(const std::string &file)
Load matrix from a text file, compatible with MATLAB text format.
Internal resize which compiles to nothing on fixed-size matrices.
Definition: math_frwds.h:50
static void doit(Eigen::Matrix< S, R, C, Opt, MaxR, MaxC > &mat, size_t new_rows, size_t new_cols)
EIGEN_STRONG_INLINE void eigenVectors(MATRIX1 &eVecs, MATRIX2 &eVals) const
[For square matrices only] Compute the eigenvectors and eigenvalues (sorted), both returned as matric...
bool fromMatlabStringFormat(const std::string &s, std::ostream *dump_errors_here=NULL)
Read a matrix from a string in Matlab-like format, for example "[1 0 2; 0 4 -1]" The string must star...
static void doit(Eigen::Matrix< S, 1, C, Opt, MaxR, MaxC > &mat, size_t, size_t new_cols)
int BASE_IMPEXP fprintf(FILE *fil, const char *format,...) MRPT_NO_THROWS MRPT_printf_format_check(2
An OS-independent version of fprintf.
FILE BASE_IMPEXP * fopen(const char *fileName, const char *mode) MRPT_NO_THROWS
An OS-independent version of fopen.
EIGEN_STRONG_INLINE void eigenVectorsVec(MATRIX1 &eVecs, VECTOR1 &eVals) const
[For square matrices only] Compute the eigenvectors and eigenvalues (sorted), eigenvectors are the co...
fixed floating point &#39;f&#39;
Definition: math_frwds.h:67



Page generated by Doxygen 1.8.11 for MRPT 1.4.0 SVN:Unversioned directory at Mon Jul 4 10:31:07 UTC 2016