MRPT  1.9.9
MatrixVectorBase_impl.h
Go to the documentation of this file.
1 /* +------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | https://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2019, Individual contributors, see AUTHORS file |
6  | See: https://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See: https://www.mrpt.org/License |
8  +------------------------------------------------------------------------+ */
9 #pragma once
10 
11 #include <mrpt/core/exceptions.h>
13 #include <cstdint>
14 #include <cstdio> // fopen(),...
15 #include <ctime> // time(),...
16 #include <fstream> // ifstream
17 #include <sstream> // stringstream
18 #include <stdexcept>
19 #include <vector>
20 
21 namespace mrpt::math
22 {
23 template <typename Scalar, class Derived>
25  const std::string& s, mrpt::optional_ref<std::ostream> dump_errors_here)
26 {
27  // Start with a (0,0) matrix:
28  if (Derived::RowsAtCompileTime == Eigen::Dynamic) mvbDerived() = Derived();
29 
30  // Look for starting "[".
31  size_t ini = s.find_first_not_of(" \t\r\n");
32  if (ini == std::string::npos || s[ini] != '[')
33  {
34  return false;
35  }
36 
37  size_t end = s.find_last_not_of(" \t\r\n");
38  if (end == std::string::npos || s[end] != ']') return false;
39 
40  if (ini > end) return false;
41 
42  std::vector<Scalar> lstElements;
43 
44  size_t i = ini + 1;
45  size_t nRow = 0;
46 
47  while (i < end)
48  {
49  // Extract one row:
50  size_t end_row = s.find_first_of(";]", i);
51  if (end_row == std::string::npos)
52  {
53  return false;
54  }
55 
56  // We have one row in s[ i : (end_row-1) ]
57  std::stringstream ss(s.substr(i, end_row - i));
58  lstElements.clear();
59  try
60  {
61  while (!ss.eof())
62  {
63  Scalar val;
64  ss >> val;
65  if (ss.bad() || ss.fail()) break;
66  lstElements.push_back(val);
67  }
68  }
69  catch (...)
70  {
71  } // end of line
72 
73  // Empty row? Only for the first row, then this is an empty matrix:
74  if (lstElements.empty())
75  {
76  if (nRow > 0)
77  return false;
78  else
79  {
80  // Else, this may be an empty matrix... if there is no next row,
81  // we'll return with a (0,0) matrix
82  if (Derived::RowsAtCompileTime == Eigen::Dynamic)
83  mvbDerived() = Derived();
84  }
85  }
86  else
87  {
88  const size_t N = lstElements.size();
89 
90  // Check valid width: All rows must have the same width
91  if ((nRow > 0 && size_t(mvbDerived().cols()) != N) ||
92  (nRow == 0 && Derived::ColsAtCompileTime != Eigen::Dynamic &&
93  Derived::ColsAtCompileTime != int(N)))
94  {
95  if (dump_errors_here)
96  dump_errors_here->get()
97  << "[fromMatlabStringFormat] Row " << nRow + 1
98  << " has invalid number of columns.\n";
99  return false;
100  }
101 
102  // Append to the matrix:
103  if (Derived::RowsAtCompileTime == Eigen::Dynamic ||
104  Derived::ColsAtCompileTime == Eigen::Dynamic)
105  mvbDerived().resize(nRow + 1, N);
106  else if (
107  Derived::RowsAtCompileTime != Eigen::Dynamic &&
108  int(nRow) >= Derived::RowsAtCompileTime)
109  {
110  if (dump_errors_here)
111  dump_errors_here->get()
112  << "[fromMatlabStringFormat] Read more "
113  "rows than the capacity of the "
114  "fixed sized matrix.\n";
115  return false;
116  }
117  for (size_t q = 0; q < N; q++)
118  mvbDerived()(nRow, q) = lstElements[q];
119  // Go for the next row:
120  nRow++;
121  }
122  i = end_row + 1;
123  }
124  // For fixed sized matrices, check size:
125  if (Derived::RowsAtCompileTime != Eigen::Dynamic &&
126  int(nRow) != Derived::RowsAtCompileTime)
127  {
128  if (dump_errors_here)
129  dump_errors_here->get()
130  << "[fromMatlabStringFormat] Read less rows "
131  "than the capacity of the fixed sized "
132  "matrix.\n";
133  return false;
134  }
135  return true; // Ok
136 }
137 
138 template <typename Scalar, class Derived>
140  const std::size_t decimal_digits) const
141 {
142  using Index = typename Derived::Index;
143  std::stringstream s;
144  s << "[" << std::scientific;
145  s.precision(decimal_digits);
146  for (Index i = 0; i < mvbDerived().rows(); i++)
147  {
148  for (Index j = 0; j < mvbDerived().cols(); j++)
149  s << mvbDerived().coeff(i, j) << " ";
150  if (i < mvbDerived().rows() - 1) s << ";";
151  }
152  s << "]";
153  return s.str();
154 }
155 
156 template <typename Scalar, class Derived>
158  const std::string& file, mrpt::math::TMatrixTextFileFormat fileFormat,
159  bool appendMRPTHeader, const std::string& userHeader) const
160 {
161  using Index = typename Derived::Index;
162  // Use a secure version in Visual Studio 2005+
163 #if defined(_MSC_VER) && (_MSC_VER >= 1400)
164  FILE* f;
165  if (0 != ::fopen_s(&f, file.c_str(), "wt")) f = nullptr;
166 #else
167  FILE* f = ::fopen(file.c_str(), "wt");
168 #endif
169  if (!f)
170  throw std::runtime_error(
171  std::string("saveToTextFile: Error opening file ") + file +
172  std::string("' for writing a matrix as text."));
173 
174  if (!userHeader.empty()) fprintf(f, "%s", userHeader.c_str());
175 
176  if (appendMRPTHeader)
177  {
178  time_t rawtime;
179  ::time(&rawtime);
180  // Use a secure version in Visual Studio 2005+
181 #if defined(_MSC_VER) && (_MSC_VER >= 1400)
182  struct tm timeinfo_data;
183  struct tm* timeinfo;
184  if (0 != ::localtime_s(&timeinfo_data, &rawtime))
185  timeinfo = nullptr;
186  else
187  timeinfo = &timeinfo_data;
188 #else
189  struct tm* timeinfo = ::localtime(&rawtime);
190 #endif
191 
192 #if defined(_MSC_VER) && (_MSC_VER >= 1400)
193  char strTimeBuf[100];
194  if (0 != asctime_s(strTimeBuf, sizeof(strTimeBuf), timeinfo))
195  strTimeBuf[0] = '\0';
196  char* strTime = &strTimeBuf[0];
197 #else
198  char* strTime = asctime(timeinfo);
199 #endif
200  fprintf(
201  f,
202  "%% File generated with mrpt-math at %s\n"
203  "%%------------------------------------\n",
204  strTime);
205  }
206 
207  const auto& m = mvbDerived();
208  for (Index i = 0; i < m.rows(); i++)
209  {
210  for (Index j = 0; j < m.cols(); j++)
211  {
212  switch (fileFormat)
213  {
215  ::fprintf(f, "%.16e", static_cast<double>(m(i, j)));
216  break;
218  ::fprintf(f, "%.16f", static_cast<double>(m(i, j)));
219  break;
221  ::fprintf(f, "%i", static_cast<int>(m(i, j)));
222  break;
223  default:
224  throw std::runtime_error(
225  "Unsupported value for the parameter 'fileFormat'!");
226  };
227  // Separating blank space
228  if (j < (mvbDerived().cols() - 1)) ::fprintf(f, " ");
229  }
230  ::fprintf(f, "\n");
231  }
232  ::fclose(f);
233 }
234 
235 template <typename Scalar, class Derived>
237 {
238  using Index = typename Derived::Index;
239  std::string str;
240  std::vector<double> fil(512);
241  size_t nRows = 0;
242  while (!f.eof() && !f.fail())
243  {
244  std::getline(f, str);
245  if (str.size() && str[0] != '#' && str[0] != '%')
246  {
247  // Parse row to floats:
248  const char* ptr = str.c_str();
249  char* ptrEnd = nullptr;
250  size_t i = 0;
251  // Process each number in this row:
252  while (ptr[0] && ptr != ptrEnd)
253  {
254  // Find next number: (non white-space character):
255  while (ptr[0] &&
256  (ptr[0] == ' ' || ptr[0] == ',' || ptr[0] == '\t' ||
257  ptr[0] == '\r' || ptr[0] == '\n'))
258  ptr++;
259  if (fil.size() <= i) fil.resize(fil.size() + (fil.size() >> 1));
260  // Convert to "double":
261  fil[i] = strtod(ptr, &ptrEnd);
262  // A valid conversion has been done?
263  if (ptr != ptrEnd)
264  {
265  i++; // Yes
266  ptr = ptrEnd;
267  ptrEnd = nullptr;
268  }
269  }; // end while procesing this row
270 
271  if (!i && nRows == 0)
272  throw std::runtime_error("loadFromTextFile: Empty first line!");
273 
274  // "i": # of columns:
275  if ((Derived::ColsAtCompileTime != Eigen::Dynamic &&
276  Index(i) != Derived::ColsAtCompileTime))
277  throw std::runtime_error(
278  "loadFromTextFile: The matrix in the text file does not "
279  "match fixed matrix size");
280  if (Derived::ColsAtCompileTime == Eigen::Dynamic && nRows > 0 &&
281  Index(i) != mvbDerived().cols())
282  throw std::runtime_error(
283  "loadFromTextFile: The matrix in the text file does not "
284  "have the same number of columns in all rows");
285 
286  // Append to the matrix:
287  if (Derived::RowsAtCompileTime == Eigen::Dynamic ||
288  Derived::ColsAtCompileTime == Eigen::Dynamic)
289  {
290  if (mvbDerived().rows() < static_cast<int>(nRows + 1) ||
291  mvbDerived().cols() < static_cast<int>(i))
292  {
293  const size_t extra_rows =
294  std::max(static_cast<size_t>(1), nRows >> 1);
295  mvbDerived().resize(nRows + extra_rows, i);
296  }
297  }
298  else if (
299  Derived::RowsAtCompileTime != Eigen::Dynamic &&
300  int(nRows) >= Derived::RowsAtCompileTime)
301  throw std::runtime_error(
302  "loadFromTextFile: Read more rows than the capacity of the "
303  "fixed sized matrix.");
304 
305  for (size_t q = 0; q < i; q++)
306  mvbDerived()(nRows, q) = Scalar(fil[q]);
307 
308  nRows++;
309  } // end if fgets
310  } // end while not feof
311 
312  // Final resize to the real size (in case we allocated space in advance):
313  if (Derived::RowsAtCompileTime == Eigen::Dynamic ||
314  Derived::ColsAtCompileTime == Eigen::Dynamic)
315  mvbDerived().resize(nRows, mvbDerived().cols());
316 
317  // Report error as exception
318  if (!nRows)
319  throw std::runtime_error(
320  "loadFromTextFile: Error loading from text file");
321 }
322 
323 template <typename Scalar, class Derived>
325  const std::string& file)
326 {
327  std::ifstream f(file.c_str());
328  if (f.fail())
329  throw std::runtime_error(
330  std::string("loadFromTextFile: can't open file:") + file);
331  loadFromTextFile(f);
332 }
333 
334 template <typename Scalar, class Derived>
336 {
337  std::stringstream ss;
338  ss << mvbDerived().asEigen();
339  return ss.str();
340 }
341 
342 template <typename Scalar, class Derived>
344 {
345  return mvbDerived().asEigen().array().sum();
346 }
347 
348 template <typename Scalar, class Derived>
350 {
351  return mvbDerived().asEigen().array().abs().sum();
352 }
353 
354 template <typename Scalar, class Derived>
356 {
357  return mvbDerived().asEigen().minCoeff();
358 }
359 
360 template <typename Scalar, class Derived>
362 {
363  return mvbDerived().asEigen().maxCoeff();
364 }
365 
366 template <typename Scalar, class Derived>
368 {
369  if constexpr (Derived::ColsAtCompileTime == 1)
370  {
371  typename Derived::Index idx;
372  auto r = mvbDerived().asEigen().minCoeff(&idx);
373  outIdx = static_cast<std::size_t>(idx);
374  return r;
375  }
376  else
377  throw std::runtime_error(
378  "minCoeff(idx): Signature only valid for column vectors");
379 }
380 
381 template <typename Scalar, class Derived>
383 {
384  if constexpr (Derived::ColsAtCompileTime == 1)
385  {
386  typename Derived::Index idx;
387  auto r = mvbDerived().asEigen().maxCoeff(&idx);
388  outIdx = static_cast<std::size_t>(idx);
389  return r;
390  }
391  else
392  throw std::runtime_error(
393  "minCoeff(idx): Signature only valid for column vectors");
394 }
395 template <typename Scalar, class Derived>
397  std::size_t& rowIdx, std::size_t& colIdx) const
398 {
399  typename Derived::Index row, col;
400  auto r = mvbDerived().asEigen().minCoeff(&row, &col);
401  rowIdx = static_cast<std::size_t>(row);
402  colIdx = static_cast<std::size_t>(col);
403  return r;
404 }
405 
406 template <typename Scalar, class Derived>
408  std::size_t& rowIdx, std::size_t& colIdx) const
409 {
410  typename Derived::Index row, col;
411  auto r = mvbDerived().asEigen().maxCoeff(&row, &col);
412  rowIdx = static_cast<std::size_t>(row);
413  colIdx = static_cast<std::size_t>(col);
414  return r;
415 }
416 
417 template <typename Scalar, class Derived>
419 {
420  mvbDerived().asEigen().array() += s;
421 }
422 
423 template <typename Scalar, class Derived>
425 {
426  mvbDerived().asEigen().array() -= s;
427 }
428 
429 template <typename Scalar, class Derived>
431 {
432  mvbDerived().asEigen().array() *= s;
433 }
434 
435 template <typename Scalar, class Derived>
437  const CMatrixDynamic<Scalar>& v)
438 {
439  return CMatrixDynamic<Scalar>(
440  (mvbDerived().asEigen() * v.asEigen()).eval());
441 }
442 
443 template <typename Scalar, class Derived>
444 Derived MatrixVectorBase<Scalar, Derived>::impl_op_add(const Derived& m2) const
445 {
446  Derived ret(mvbDerived().rows(), mvbDerived().cols());
447  ret.asEigen() = mvbDerived().asEigen() + m2.asEigen();
448  return ret;
449 }
450 template <typename Scalar, class Derived>
452 {
453  mvbDerived().asEigen() += m2.asEigen();
454 }
455 template <typename Scalar, class Derived>
456 Derived MatrixVectorBase<Scalar, Derived>::impl_op_subs(const Derived& m2) const
457 {
458  Derived ret(mvbDerived().rows(), mvbDerived().cols());
459  ret.asEigen() = mvbDerived().asEigen() - m2.asEigen();
460  return ret;
461 }
462 template <typename Scalar, class Derived>
464 {
465  mvbDerived().asEigen() -= m2.asEigen();
466 }
467 template <typename Scalar, class Derived>
468 Derived MatrixVectorBase<Scalar, Derived>::operator*(const Derived& m2) const
469 {
470  ASSERTMSG_(
471  mvbDerived().cols() == mvbDerived().rows(),
472  "Operator* implemented only for square matrices. Use `A.asEigen() * "
473  "B.asEigen()` for general matrix products.");
474  Derived ret(mvbDerived().rows(), mvbDerived().rows());
475  if constexpr (Derived::RowsAtCompileTime == Derived::ColsAtCompileTime)
476  {
477  ret.asEigen() = mvbDerived().asEigen() * m2.asEigen();
478  }
479  return ret;
480 }
481 
482 template <typename Scalar, class Derived>
485 {
486  mvbDerived() = A.asEigen() * b.asEigen();
487 }
488 
489 template <typename Scalar, class Derived>
492 {
493  mvbDerived() = A.asEigen().transpose() * b.asEigen();
494 }
495 
496 template <typename Scalar, class Derived>
498 {
499  return mvbDerived().asEigen().template lpNorm<Eigen::Infinity>();
500 }
501 
502 template <typename Scalar, class Derived>
504 {
505  return mvbDerived().asEigen().norm();
506 }
507 
508 template <typename Scalar, class Derived>
510  const CVectorDynamic<Scalar>& v) const
511 {
512  if constexpr (Derived::ColsAtCompileTime == 1)
513  {
514  return mvbDerived().asEigen().dot(v.mvbDerived().asEigen());
515  }
516  else
517  {
518  ASSERTMSG_(false, "dot(): Implemented for column vectors only.");
519  }
520 }
521 template <typename Scalar, class Derived>
524 {
525  if constexpr (Derived::ColsAtCompileTime == 1)
526  {
527  return mvbDerived().asEigen().dot(v.mvbDerived().asEigen());
528  }
529  else
530  {
531  ASSERTMSG_(false, "dot(): Implemented for column vectors only.");
532  }
533 }
534 
535 } // namespace mrpt::math
Scalar maxCoeff() const
Maximum value in the matrix/vector.
Scalar norm_inf() const
Compute the norm-infinite of a vector ($f[ ||{v}||_ $f]), ie the maximum absolute value of the elemen...
void impl_op_selfsubs(const Derived &m2)
auto operator*(const MatrixVectorBase< S2, D2 > &m2) const
double Scalar
Definition: KmUtils.h:43
std::string asString() const
Returns a string representation of the vector/matrix, using Eigen&#39;s default settings.
Template for column vectors of dynamic size, compatible with Eigen.
int void fclose(FILE *f)
An OS-independent version of fclose.
Definition: os.cpp:275
Scalar norm() const
Compute the L2 norm of a vector/array/matrix (the Euclidean distance to the origin, taking all the elements as a single vector).
GLdouble GLdouble GLdouble GLdouble q
Definition: glext.h:3727
engineering format &#39;e&#39;
std::optional< std::reference_wrapper< T > > optional_ref
Shorter name for std::optional<std::reference_wrapper<T>>
Definition: optional_ref.h:20
GLdouble s
Definition: glext.h:3682
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
Saves the vector/matrix to a file compatible with MATLAB/Octave text format.
This base provides a set of functions for maths stuff.
Scalar dot(const CVectorDynamic< Scalar > &v) const
dot product of this \cdot v
Derived impl_op_subs(const Derived &m2) const
GLuint GLuint end
Definition: glext.h:3532
int val
Definition: mrpt_jpeglib.h:957
GLubyte GLubyte b
Definition: glext.h:6372
void matProductOf_Ab(const CMatrixDynamic< Scalar > &A, const CVectorDynamic< Scalar > &b)
this = A * b , with A and b a dynamic matrix & vector
void operator+=(const MatrixVectorBase< S2, D2 > &m2)
#define ASSERTMSG_(f, __ERROR_MSG)
Defines an assertion mechanism.
Definition: exceptions.h:108
bool fromMatlabStringFormat(const std::string &s, mrpt::optional_ref< std::ostream > dump_errors_here=std::nullopt)
Reads a matrix from a string in Matlab-like format, for example: "[1 0 2; 0 4 -1]" The string must st...
GLsizei const GLchar ** string
Definition: glext.h:4116
int fprintf(FILE *fil, const char *format,...) noexcept MRPT_printf_format_check(2
An OS-independent version of fprintf.
Definition: os.cpp:410
Scalar sum_abs() const
Sum of the absolute value of all elements in matrix/vector.
fixed floating point &#39;f&#39;
const GLdouble * v
Definition: glext.h:3684
void operator-=(const MatrixVectorBase< S2, D2 > &m2)
GLdouble GLdouble GLdouble r
Definition: glext.h:3711
Scalar sum() const
Sum of all elements in matrix/vector.
Derived impl_op_add(const Derived &m2) const
GLenum GLenum GLvoid * row
Definition: glext.h:3580
void impl_op_selfadd(const Derived &m2)
std::string inMatlabFormat(const std::size_t decimal_digits=6) const
Exports the matrix as a string compatible with Matlab/Octave.
Scalar minCoeff() const
Minimum value in the matrix/vector.
FILE * fopen(const char *fileName, const char *mode) noexcept
An OS-independent version of fopen.
Definition: os.cpp:257
Base CRTP class for all MRPT vectors and matrices.
void matProductOf_Atb(const CMatrixDynamic< Scalar > &A, const CVectorDynamic< Scalar > &b)
this = AT * b , with A and b a dynamic matrix & vector
This template class provides the basic functionality for a general 2D any-size, resizable container o...
intergers &#39;i&#39;
void loadFromTextFile(std::istream &f)
Loads a vector/matrix from a text file, compatible with MATLAB text format.



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: 3316834cb Wed Aug 21 11:42:56 2019 +0200 at miƩ ago 21 11:50:11 CEST 2019