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



Page generated by Doxygen 1.8.14 for MRPT 1.5.7 Git: 5902e14cc Wed Apr 24 15:04:01 2019 +0200 at lun oct 28 01:39:17 CET 2019