Main MRPT website > C++ reference for MRPT 1.9.9
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-2018, 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>
22 struct MatOrVecResizer
23 {
24  template <typename S, int Opt, int MaxR, int MaxC>
25  static inline void doit(
26  Eigen::Matrix<S, R, C, Opt, MaxR, MaxC>& mat, size_t new_rows,
27  size_t new_cols)
28  {
29  ::mrpt::math::detail::TAuxResizer<
30  Eigen::Matrix<S, R, C, Opt, MaxR, MaxC>,
31  Eigen::Matrix<S, R, C, Opt, MaxR, MaxC>::SizeAtCompileTime>::
32  internal_resize(mat, new_rows, new_cols);
33  }
34 };
35 // Specialization for column matrices:
36 template <int R>
37 struct MatOrVecResizer<R, 1>
38 {
39  template <typename S, int Opt, int MaxR, int MaxC>
40  static inline void doit(
41  Eigen::Matrix<S, R, 1, Opt, MaxR, MaxC>& mat, size_t new_rows, size_t)
42  {
43  ::mrpt::math::detail::TAuxResizer<
44  Eigen::Matrix<S, R, 1, Opt, MaxR, MaxC>,
45  Eigen::Matrix<S, R, 1, Opt, MaxR, MaxC>::SizeAtCompileTime>::
46  internal_resize(mat, new_rows);
47  }
48 };
49 // Specialization for row matrices:
50 template <int C>
51 struct MatOrVecResizer<1, C>
52 {
53  template <typename S, int Opt, int MaxR, int MaxC>
54  static inline void doit(
55  Eigen::Matrix<S, 1, C, Opt, MaxR, MaxC>& mat, size_t, size_t new_cols)
56  {
57  ::mrpt::math::detail::TAuxResizer<
58  Eigen::Matrix<S, 1, C, Opt, MaxR, MaxC>,
59  Eigen::Matrix<S, 1, C, Opt, MaxR, MaxC>::SizeAtCompileTime>::
60  internal_resize(mat, new_cols);
61  }
62 };
63 template <>
64 struct MatOrVecResizer<1, 1>
65 {
66  template <typename S, int Opt, int MaxR, int MaxC>
67  static inline void doit(
68  Eigen::Matrix<S, 1, 1, Opt, MaxR, MaxC>& mat, size_t, size_t new_cols)
69  {
70  ::mrpt::math::detail::TAuxResizer<
71  Eigen::Matrix<S, 1, 1, Opt, MaxR, MaxC>,
72  Eigen::Matrix<S, 1, 1, Opt, MaxR, MaxC>::SizeAtCompileTime>::
73  internal_resize(mat, new_cols);
74  }
75 };
76 }
77 
78 /** Compute the eigenvectors and eigenvalues, both returned as matrices:
79  * eigenvectors are the columns, and eigenvalues. \return false on error.
80  */
81 template <class Derived>
82 template <class MATRIX1, class MATRIX2>
83 EIGEN_STRONG_INLINE bool Eigen::MatrixBase<Derived>::eigenVectors(
84  MATRIX1& eVecs, MATRIX2& eVals) const
85 {
86  Matrix<Scalar, Dynamic, 1> evals;
87  if (!eigenVectorsVec(eVecs, evals)) return false;
88  eVals.resize(evals.size(), evals.size());
89  eVals.setZero();
90  eVals.diagonal() = evals;
91  return true;
92 }
93 
94 /** Compute the eigenvectors and eigenvalues, both returned as matrices:
95  * eigenvectors are the columns, and eigenvalues
96  */
97 template <class Derived>
98 template <class MATRIX1, class VECTOR1>
99 EIGEN_STRONG_INLINE bool Eigen::MatrixBase<Derived>::eigenVectorsVec(
100  MATRIX1& eVecs, VECTOR1& eVals) const
101 {
102  Eigen::EigenSolver<Derived> es(*this, true);
103  if (es.info() != Eigen::Success) return false;
104  eVecs =
105  es.eigenvectors().real(); // Keep only the real part of complex matrix
106  eVals =
107  es.eigenvalues().real(); // Keep only the real part of complex matrix
108 
109  // Sort by ascending eigenvalues:
110  std::vector<std::pair<Scalar, Index>> D;
111  D.reserve(eVals.size());
112  for (Index i = 0; i < eVals.size(); i++)
113  D.push_back(std::pair<Scalar, Index>(eVals.coeff(i, 0), i));
114  std::sort(D.begin(), D.end());
115  MATRIX1 sortedEigs;
116  sortedEigs.resizeLike(eVecs);
117  for (int i = 0; i < eVals.size(); i++)
118  {
119  eVals.coeffRef(i, 0) = D[i].first;
120  sortedEigs.col(i) = eVecs.col(D[i].second);
121  }
122  eVecs = sortedEigs;
123  return true;
124 }
125 
126 /** Compute the eigenvectors and eigenvalues, both returned as matrices:
127  * eigenvectors are the columns, and eigenvalues
128  */
129 template <class Derived>
130 template <class MATRIX1, class MATRIX2>
131 EIGEN_STRONG_INLINE void Eigen::MatrixBase<Derived>::eigenVectorsSymmetric(
132  MATRIX1& eVecs, MATRIX2& eVals) const
133 {
134  Matrix<Scalar, Dynamic, 1> evals;
135  eigenVectorsSymmetricVec(eVecs, evals);
136  eVals.resize(evals.size(), evals.size());
137  eVals.setZero();
138  eVals.diagonal() = evals;
139 }
140 
141 /** Compute the eigenvectors and eigenvalues, both returned as matrices:
142  * eigenvectors are the columns, and eigenvalues
143  */
144 template <class Derived>
145 template <class MATRIX1, class VECTOR1>
146 EIGEN_STRONG_INLINE void Eigen::MatrixBase<Derived>::eigenVectorsSymmetricVec(
147  MATRIX1& eVecs, VECTOR1& eVals) const
148 {
149  // This solver returns the eigenvectors already sorted.
150  Eigen::SelfAdjointEigenSolver<Derived> eigensolver(*this);
151  eVecs = eigensolver.eigenvectors();
152  eVals = eigensolver.eigenvalues();
153 }
154 
155 template <class Derived>
156 bool Eigen::MatrixBase<Derived>::fromMatlabStringFormat(
157  const std::string& s, std::ostream* dump_errors_here)
158 {
159  // Start with a (0,0) matrix:
160  if (Derived::RowsAtCompileTime == Eigen::Dynamic) (*this) = Derived();
161 
162  // Look for starting "[".
163  size_t ini = s.find_first_not_of(" \t\r\n");
164  if (ini == std::string::npos || s[ini] != '[')
165  {
166  return false;
167  }
168 
169  size_t end = s.find_last_not_of(" \t\r\n");
170  if (end == std::string::npos || s[end] != ']') return false;
171 
172  if (ini > end) return false;
173 
174  std::vector<Scalar> lstElements;
175 
176  size_t i = ini + 1;
177  size_t nRow = 0;
178 
179  while (i < end)
180  {
181  // Extract one row:
182  size_t end_row = s.find_first_of(";]", i);
183  if (end_row == std::string::npos)
184  {
185  return false;
186  }
187 
188  // We have one row in s[ i : (end_row-1) ]
189  std::stringstream ss(s.substr(i, end_row - i));
190  lstElements.clear();
191  try
192  {
193  while (!ss.eof())
194  {
195  Scalar val;
196  ss >> val;
197  if (ss.bad() || ss.fail()) break;
198  lstElements.push_back(val);
199  }
200  }
201  catch (...)
202  {
203  } // end of line
204 
205  // Empty row? Only for the first row, then this is an empty matrix:
206  if (lstElements.empty())
207  {
208  if (nRow > 0)
209  return false;
210  else
211  {
212  // Else, this may be an empty matrix... if there is no next row,
213  // we'll return with a (0,0) matrix
214  if (Derived::RowsAtCompileTime == Eigen::Dynamic)
215  (*this) = Derived();
216  }
217  }
218  else
219  {
220  const size_t N = lstElements.size();
221 
222  // Check valid width: All rows must have the same width
223  if ((nRow > 0 && size_t(cols()) != N) ||
224  (nRow == 0 && Derived::ColsAtCompileTime != Eigen::Dynamic &&
225  Derived::ColsAtCompileTime != int(N)))
226  {
227  if (dump_errors_here)
228  (*dump_errors_here) << "[fromMatlabStringFormat] Row "
229  << nRow + 1
230  << " has invalid number of columns.\n";
231  return false;
232  }
233 
234  // Append to the matrix:
235  if (Derived::RowsAtCompileTime == Eigen::Dynamic ||
236  Derived::ColsAtCompileTime == Eigen::Dynamic)
237  internal_mrpt::MatOrVecResizer<
238  Derived::RowsAtCompileTime,
239  Derived::ColsAtCompileTime>::doit(derived(), nRow + 1, N);
240  else if (
241  Derived::RowsAtCompileTime != Eigen::Dynamic &&
242  int(nRow) >= Derived::RowsAtCompileTime)
243  {
244  if (dump_errors_here)
245  (*dump_errors_here) << "[fromMatlabStringFormat] Read more "
246  "rows than the capacity of the "
247  "fixed sized matrix.\n";
248  return false;
249  }
250 
251  for (size_t q = 0; q < N; q++) coeffRef(nRow, q) = lstElements[q];
252 
253  // Go for the next row:
254  nRow++;
255  }
256 
257  i = end_row + 1;
258  }
259  // For fixed sized matrices, check size:
260  if (Derived::RowsAtCompileTime != Eigen::Dynamic &&
261  int(nRow) != Derived::RowsAtCompileTime)
262  {
263  if (dump_errors_here)
264  (*dump_errors_here) << "[fromMatlabStringFormat] Read less rows "
265  "than the capacity of the fixed sized "
266  "matrix.\n";
267  return false;
268  }
269  return true; // Ok
270 }
271 
272 template <class Derived>
273 std::string Eigen::MatrixBase<Derived>::inMatlabFormat(
274  const size_t decimal_digits) const
275 {
276  std::stringstream s;
277  s << "[" << std::scientific;
278  s.precision(decimal_digits);
279  for (Index i = 0; i < rows(); i++)
280  {
281  for (Index j = 0; j < cols(); j++) s << coeff(i, j) << " ";
282  if (i < rows() - 1) s << ";";
283  }
284  s << "]";
285  return s.str();
286 }
287 
288 template <class Derived>
289 void Eigen::MatrixBase<Derived>::saveToTextFile(
290  const std::string& file, mrpt::math::TMatrixTextFileFormat fileFormat,
291  bool appendMRPTHeader, const std::string& userHeader) const
292 {
293 #if defined(_MSC_VER) && \
294  (_MSC_VER >= 1400) // Use a secure version in Visual Studio 2005+
295  FILE* f;
296  if (0 != ::fopen_s(&f, file.c_str(), "wt")) f = nullptr;
297 #else
298  FILE* f = ::fopen(file.c_str(), "wt");
299 #endif
300  if (!f)
301  throw std::runtime_error(
302  std::string("saveToTextFile: Error opening file ") + file +
303  std::string("' for writing a matrix as text."));
304 
305  if (!userHeader.empty()) fprintf(f, "%s", userHeader.c_str());
306 
307  if (appendMRPTHeader)
308  {
309  time_t rawtime;
310  ::time(&rawtime);
311 #if defined(_MSC_VER) && \
312  (_MSC_VER >= 1400) // Use a secure version in Visual Studio 2005+
313  struct tm timeinfo_data;
314  struct tm* timeinfo;
315  if (0 != ::localtime_s(&timeinfo_data, &rawtime))
316  timeinfo = nullptr;
317  else
318  timeinfo = &timeinfo_data;
319 #else
320  struct tm* timeinfo = ::localtime(&rawtime);
321 #endif
322 
323 #if defined(_MSC_VER) && \
324  (_MSC_VER >= 1400) // Use a secure version in Visual Studio 2005+
325  char strTimeBuf[100];
326  if (0 != asctime_s(strTimeBuf, sizeof(strTimeBuf), timeinfo))
327  strTimeBuf[0] = '\0';
328  char* strTime = &strTimeBuf[0];
329 #else
330  char* strTime = asctime(timeinfo);
331 #endif
332  fprintf(
333  f,
334  "%% File generated with %s at "
335  "%s\n%%------------------------------------------------------------"
336  "-----\n",
337  mrpt::system::MRPT_getVersion().c_str(), strTime);
338  }
339 
340  for (Index i = 0; i < rows(); i++)
341  {
342  for (Index j = 0; j < cols(); j++)
343  {
344  switch (fileFormat)
345  {
346  case mrpt::math::MATRIX_FORMAT_ENG:
347  ::fprintf(f, "%.16e", static_cast<double>(coeff(i, j)));
348  break;
349  case mrpt::math::MATRIX_FORMAT_FIXED:
350  ::fprintf(f, "%.16f", static_cast<double>(coeff(i, j)));
351  break;
352  case mrpt::math::MATRIX_FORMAT_INT:
353  ::fprintf(f, "%i", static_cast<int>(coeff(i, j)));
354  break;
355  default:
356  throw std::runtime_error(
357  "Unsupported value for the parameter 'fileFormat'!");
358  };
359  // Separating blank space
360  if (j < (cols() - 1)) ::fprintf(f, " ");
361  }
362  ::fprintf(f, "\n");
363  }
364  ::fclose(f);
365 }
366 
367 template <class Derived>
368 void Eigen::MatrixBase<Derived>::loadFromTextFile(const std::string& file)
369 {
370  std::ifstream f(file.c_str());
371  if (f.fail())
372  throw std::runtime_error(
373  std::string("loadFromTextFile: can't open file:") + file);
374  loadFromTextFile(f);
375 }
376 
377 template <class Derived>
378 void Eigen::MatrixBase<Derived>::loadFromTextFile(std::istream& f)
379 {
380  // This matrix is NROWS x NCOLS
381  std::string str;
382  std::vector<double> fil(512);
383  size_t nRows = 0;
384  while (!f.eof() && !f.fail())
385  {
386  std::getline(f, str);
387  if (str.size() && str[0] != '#' && str[0] != '%')
388  {
389  // Parse row to floats:
390  const char* ptr = str.c_str();
391  char* ptrEnd = nullptr;
392  size_t i = 0;
393  // Process each number in this row:
394  while (ptr[0] && ptr != ptrEnd)
395  {
396  // Find next number: (non white-space character):
397  while (ptr[0] &&
398  (ptr[0] == ' ' || ptr[0] == ',' || ptr[0] == '\t' ||
399  ptr[0] == '\r' || ptr[0] == '\n'))
400  ptr++;
401  if (fil.size() <= i) fil.resize(fil.size() + (fil.size() >> 1));
402  // Convert to "double":
403  fil[i] = strtod(ptr, &ptrEnd);
404  // A valid conversion has been done?
405  if (ptr != ptrEnd)
406  {
407  i++; // Yes
408  ptr = ptrEnd;
409  ptrEnd = nullptr;
410  }
411  }; // end while procesing this row
412 
413  // "i": # of columns:
414  if ((Derived::ColsAtCompileTime != Eigen::Dynamic &&
415  Index(i) != Derived::ColsAtCompileTime))
416  throw std::runtime_error(
417  "loadFromTextFile: The matrix in the text file does not "
418  "match fixed matrix size");
419  if (Derived::ColsAtCompileTime == Eigen::Dynamic && nRows > 0 &&
420  Index(i) != cols())
421  throw std::runtime_error(
422  "loadFromTextFile: The matrix in the text file does not "
423  "have the same number of columns in all rows");
424 
425  // Append to the matrix:
426  if (Derived::RowsAtCompileTime == Eigen::Dynamic ||
427  Derived::ColsAtCompileTime == Eigen::Dynamic)
428  {
429  if (rows() < static_cast<int>(nRows + 1) ||
430  cols() < static_cast<int>(i))
431  {
432  const size_t extra_rows =
433  std::max(static_cast<size_t>(1), nRows >> 1);
434  internal_mrpt::MatOrVecResizer<Derived::RowsAtCompileTime,
435  Derived::ColsAtCompileTime>::
436  doit(derived(), nRows + extra_rows, i);
437  }
438  }
439  else if (
440  Derived::RowsAtCompileTime != Eigen::Dynamic &&
441  int(nRows) >= Derived::RowsAtCompileTime)
442  throw std::runtime_error(
443  "loadFromTextFile: Read more rows than the capacity of the "
444  "fixed sized matrix.");
445 
446  for (size_t q = 0; q < i; q++) coeffRef(nRows, q) = Scalar(fil[q]);
447 
448  nRows++;
449  } // end if fgets
450  } // end while not feof
451 
452  // Final resize to the real size (in case we allocated space in advance):
453  if (Derived::RowsAtCompileTime == Eigen::Dynamic ||
454  Derived::ColsAtCompileTime == Eigen::Dynamic)
455  internal_mrpt::MatOrVecResizer<
456  Derived::RowsAtCompileTime,
457  Derived::ColsAtCompileTime>::doit(derived(), nRows, cols());
458 
459  // Report error as exception
460  if (!nRows)
461  throw std::runtime_error(
462  "loadFromTextFile: Error loading from text file");
463 }
464 
465 #endif // guard define
mrpt::math::MATRIX_FORMAT_INT
@ MATRIX_FORMAT_INT
intergers 'i'
Definition: math_frwds.h:67
mrpt::system::os::fclose
int void fclose(FILE *f)
An OS-independent version of fclose.
Definition: os.cpp:273
eigenVectorsSymmetricVec
EIGEN_STRONG_INLINE void eigenVectorsSymmetricVec(MATRIX1 &eVecs, VECTOR1 &eVals) const
[For symmetric matrices only] Compute the eigenvectors and eigenvalues (in no particular order),...
internal_mrpt::MatOrVecResizer< 1, 1 >::doit
static void doit(Eigen::Matrix< S, 1, 1, Opt, MaxR, MaxC > &mat, size_t, size_t new_cols)
Definition: eigen_plugins_impl.h:67
mrpt::math::TMatrixTextFileFormat
TMatrixTextFileFormat
Definition: math_frwds.h:60
q
GLdouble GLdouble GLdouble GLdouble q
Definition: glext.h:3721
s
GLdouble s
Definition: glext.h:3676
Eigen::SelfAdjointEigenSolver
Definition: RandomGenerators.h:24
end
GLuint GLuint end
Definition: glext.h:3528
mrpt::math::MATRIX_FORMAT_FIXED
@ MATRIX_FORMAT_FIXED
fixed floating point 'f'
Definition: math_frwds.h:65
eigenVectorsSymmetric
EIGEN_STRONG_INLINE void eigenVectorsSymmetric(MATRIX1 &eVecs, MATRIX2 &eVals) const
[For symmetric matrices only] Compute the eigenvectors and eigenvalues (in no particular order),...
inMatlabFormat
std::string inMatlabFormat(const size_t decimal_digits=6) const
Dump matrix in matlab format.
R
const float R
Definition: CKinematicChain.cpp:138
eigenVectors
EIGEN_STRONG_INLINE bool eigenVectors(MATRIX1 &eVecs, MATRIX2 &eVals) const
[For square matrices only] Compute the eigenvectors and eigenvalues (sorted), both returned as matric...
mrpt::math::MATRIX_FORMAT_ENG
@ MATRIX_FORMAT_ENG
engineering format 'e'
Definition: math_frwds.h:63
Scalar
double Scalar
Definition: KmUtils.h:44
internal_mrpt::MatOrVecResizer< R, 1 >::doit
static void doit(Eigen::Matrix< S, R, 1, Opt, MaxR, MaxC > &mat, size_t new_rows, size_t)
Definition: eigen_plugins_impl.h:40
saveToTextFile
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...
internal_mrpt
Definition: eigen_plugins_impl.h:18
fromMatlabStringFormat
bool fromMatlabStringFormat(const std::string &s, std::ostream *dump_errors_here=nullptr)
Read a matrix from a string in Matlab-like format, for example "[1 0 2; 0 4 -1]" The string must star...
eigenVectorsVec
EIGEN_STRONG_INLINE bool eigenVectorsVec(MATRIX1 &eVecs, VECTOR1 &eVals) const
[For square matrices only] Compute the eigenvectors and eigenvalues (sorted), eigenvectors are the co...
val
int val
Definition: mrpt_jpeglib.h:955
internal_mrpt::MatOrVecResizer::doit
static void doit(Eigen::Matrix< S, R, C, Opt, MaxR, MaxC > &mat, size_t new_rows, size_t new_cols)
Definition: eigen_plugins_impl.h:25
mrpt::system::os::fprintf
int fprintf(FILE *fil, const char *format,...) noexcept MRPT_printf_format_check(2
An OS-independent version of fprintf.
Definition: os.cpp:406
mrpt::math::detail::TAuxResizer
Internal resize which compiles to nothing on fixed-size matrices.
Definition: math_frwds.h:39
internal_mrpt::MatOrVecResizer< 1, C >::doit
static void doit(Eigen::Matrix< S, 1, C, Opt, MaxR, MaxC > &mat, size_t, size_t new_cols)
Definition: eigen_plugins_impl.h:54
loadFromTextFile
void loadFromTextFile(const std::string &file)
Load matrix from a text file, compatible with MATLAB text format.
mrpt::system::os::fopen
FILE * fopen(const char *fileName, const char *mode) noexcept
An OS-independent version of fopen.
Definition: os.cpp:255
string
GLsizei const GLchar ** string
Definition: glext.h:4101
internal_mrpt::MatOrVecResizer
Definition: eigen_plugins_impl.h:22
mrpt::system::MRPT_getVersion
std::string MRPT_getVersion()
Returns a string describing the MRPT version.
Definition: os.cpp:185


Page generated by Doxygen 1.8.17 for MRPT 1.9.9 Git: ad3a9d8ae Tue May 1 23:10:22 2018 -0700 at mié 12 jul 2023 10:03:34 CEST