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-2020, 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().resize(0, 0);
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().setZero(0, 0);
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  {
106  mvbDerived().resize(nRow + 1, N);
107  }
108  else if (
109  Derived::RowsAtCompileTime != Eigen::Dynamic &&
110  int(nRow) >= Derived::RowsAtCompileTime)
111  {
112  if (dump_errors_here)
113  dump_errors_here->get()
114  << "[fromMatlabStringFormat] Read more "
115  "rows than the capacity of the "
116  "fixed sized matrix.\n";
117  return false;
118  }
119  for (size_t q = 0; q < N; q++)
120  mvbDerived()(nRow, q) = lstElements[q];
121  // Go for the next row:
122  nRow++;
123  }
124  i = end_row + 1;
125  }
126  // For fixed sized matrices, check size:
127  if (Derived::RowsAtCompileTime != Eigen::Dynamic &&
128  int(nRow) != Derived::RowsAtCompileTime)
129  {
130  if (dump_errors_here)
131  dump_errors_here->get()
132  << "[fromMatlabStringFormat] Read less rows "
133  "than the capacity of the fixed sized "
134  "matrix.\n";
135  return false;
136  }
137  return true; // Ok
138 }
139 
140 template <typename Scalar, class Derived>
142  const std::size_t decimal_digits) const
143 {
144  using Index = typename Derived::Index;
145  std::stringstream s;
146  s << "[" << std::scientific;
147  s.precision(decimal_digits);
148  for (Index i = 0; i < mvbDerived().rows(); i++)
149  {
150  for (Index j = 0; j < mvbDerived().cols(); j++)
151  s << mvbDerived().coeff(i, j) << " ";
152  if (i < mvbDerived().rows() - 1) s << ";";
153  }
154  s << "]";
155  return s.str();
156 }
157 
158 template <typename Scalar, class Derived>
160  const std::string& file, mrpt::math::TMatrixTextFileFormat fileFormat,
161  bool appendMRPTHeader, const std::string& userHeader) const
162 {
163  using Index = typename Derived::Index;
164  // Use a secure version in Visual Studio 2005+
165 #if defined(_MSC_VER) && (_MSC_VER >= 1400)
166  FILE* f;
167  if (0 != ::fopen_s(&f, file.c_str(), "wt")) f = nullptr;
168 #else
169  FILE* f = ::fopen(file.c_str(), "wt");
170 #endif
171  if (!f)
172  throw std::runtime_error(
173  std::string("saveToTextFile: Error opening file ") + file +
174  std::string("' for writing a matrix as text."));
175 
176  if (!userHeader.empty()) fprintf(f, "%s", userHeader.c_str());
177 
178  if (appendMRPTHeader)
179  {
180  time_t rawtime;
181  ::time(&rawtime);
182  // Use a secure version in Visual Studio 2005+
183 #if defined(_MSC_VER) && (_MSC_VER >= 1400)
184  struct tm timeinfo_data;
185  struct tm* timeinfo;
186  if (0 != ::localtime_s(&timeinfo_data, &rawtime))
187  timeinfo = nullptr;
188  else
189  timeinfo = &timeinfo_data;
190 #else
191  struct tm* timeinfo = ::localtime(&rawtime);
192 #endif
193 
194 #if defined(_MSC_VER) && (_MSC_VER >= 1400)
195  char strTimeBuf[100];
196  if (0 != asctime_s(strTimeBuf, sizeof(strTimeBuf), timeinfo))
197  strTimeBuf[0] = '\0';
198  char* strTime = &strTimeBuf[0];
199 #else
200  char* strTime = asctime(timeinfo);
201 #endif
202  fprintf(
203  f,
204  "%% File generated with mrpt-math at %s\n"
205  "%%------------------------------------\n",
206  strTime);
207  }
208 
209  const auto& m = mvbDerived();
210  for (Index i = 0; i < m.rows(); i++)
211  {
212  for (Index j = 0; j < m.cols(); j++)
213  {
214  switch (fileFormat)
215  {
217  ::fprintf(f, "%.16e", static_cast<double>(m(i, j)));
218  break;
220  ::fprintf(f, "%.16f", static_cast<double>(m(i, j)));
221  break;
223  ::fprintf(f, "%i", static_cast<int>(m(i, j)));
224  break;
225  default:
226  throw std::runtime_error(
227  "Unsupported value for the parameter 'fileFormat'!");
228  };
229  // Separating blank space
230  if (j < (mvbDerived().cols() - 1)) ::fprintf(f, " ");
231  }
232  ::fprintf(f, "\n");
233  }
234  ::fclose(f);
235 }
236 
237 template <typename Scalar, class Derived>
239 {
240  using Index = typename Derived::Index;
241  std::string str;
242  std::vector<double> fil(512);
243  size_t nRows = 0;
244  while (!f.eof() && !f.fail())
245  {
246  std::getline(f, str);
247  if (str.size() && str[0] != '#' && str[0] != '%')
248  {
249  // Parse row to floats:
250  const char* ptr = str.c_str();
251  char* ptrEnd = nullptr;
252  size_t i = 0;
253  // Process each number in this row:
254  while (ptr[0] && ptr != ptrEnd)
255  {
256  // Find next number: (non white-space character):
257  while (ptr[0] &&
258  (ptr[0] == ' ' || ptr[0] == ',' || ptr[0] == '\t' ||
259  ptr[0] == '\r' || ptr[0] == '\n'))
260  ptr++;
261  if (fil.size() <= i) fil.resize(fil.size() + (fil.size() >> 1));
262  // Convert to "double":
263  fil[i] = strtod(ptr, &ptrEnd);
264  // A valid conversion has been done?
265  if (ptr != ptrEnd)
266  {
267  i++; // Yes
268  ptr = ptrEnd;
269  ptrEnd = nullptr;
270  }
271  }; // end while procesing this row
272 
273  if (!i && nRows == 0)
274  throw std::runtime_error("loadFromTextFile: Empty first line!");
275 
276  // "i": # of columns:
277  if ((Derived::ColsAtCompileTime != Eigen::Dynamic &&
278  Index(i) != Derived::ColsAtCompileTime))
279  throw std::runtime_error(
280  "loadFromTextFile: The matrix in the text file does not "
281  "match fixed matrix size");
282  if (Derived::ColsAtCompileTime == Eigen::Dynamic && nRows > 0 &&
283  Index(i) != mvbDerived().cols())
284  throw std::runtime_error(
285  "loadFromTextFile: The matrix in the text file does not "
286  "have the same number of columns in all rows");
287 
288  // Append to the matrix:
289  if (Derived::RowsAtCompileTime == Eigen::Dynamic ||
290  Derived::ColsAtCompileTime == Eigen::Dynamic)
291  {
292  if (mvbDerived().rows() < static_cast<int>(nRows + 1) ||
293  mvbDerived().cols() < static_cast<int>(i))
294  {
295  const size_t extra_rows =
296  std::max(static_cast<size_t>(1), nRows >> 1);
297  mvbDerived().resize(nRows + extra_rows, i);
298  }
299  }
300  else if (
301  Derived::RowsAtCompileTime != Eigen::Dynamic &&
302  int(nRows) >= Derived::RowsAtCompileTime)
303  throw std::runtime_error(
304  "loadFromTextFile: Read more rows than the capacity of the "
305  "fixed sized matrix.");
306 
307  for (size_t q = 0; q < i; q++)
308  mvbDerived()(nRows, q) = Scalar(fil[q]);
309 
310  nRows++;
311  } // end if fgets
312  } // end while not feof
313 
314  // Final resize to the real size (in case we allocated space in advance):
315  if (Derived::RowsAtCompileTime == Eigen::Dynamic ||
316  Derived::ColsAtCompileTime == Eigen::Dynamic)
317  mvbDerived().resize(nRows, mvbDerived().cols());
318 
319  // Report error as exception
320  if (!nRows)
321  throw std::runtime_error(
322  "loadFromTextFile: Error loading from text file");
323 }
324 
325 template <typename Scalar, class Derived>
327  const std::string& file)
328 {
329  std::ifstream f(file.c_str());
330  if (f.fail())
331  throw std::runtime_error(
332  std::string("loadFromTextFile: can't open file:") + file);
333  loadFromTextFile(f);
334 }
335 
336 template <typename Scalar, class Derived>
338 {
339  std::stringstream ss;
340  ss << mvbDerived().asEigen();
341  return ss.str();
342 }
343 
344 template <typename Scalar, class Derived>
346 {
347  return mvbDerived().asEigen().array().sum();
348 }
349 
350 template <typename Scalar, class Derived>
352 {
353  return mvbDerived().asEigen().array().abs().sum();
354 }
355 
356 template <typename Scalar, class Derived>
358 {
359  return mvbDerived().asEigen().minCoeff();
360 }
361 
362 template <typename Scalar, class Derived>
364 {
365  return mvbDerived().asEigen().maxCoeff();
366 }
367 
368 template <typename Scalar, class Derived>
370 {
371  if constexpr (Derived::ColsAtCompileTime == 1)
372  {
373  typename Derived::Index idx;
374  auto r = mvbDerived().asEigen().minCoeff(&idx);
375  outIdx = static_cast<std::size_t>(idx);
376  return r;
377  }
378  else
379  throw std::runtime_error(
380  "minCoeff(idx): Signature only valid for column vectors");
381 }
382 
383 template <typename Scalar, class Derived>
385 {
386  if constexpr (Derived::ColsAtCompileTime == 1)
387  {
388  typename Derived::Index idx;
389  auto r = mvbDerived().asEigen().maxCoeff(&idx);
390  outIdx = static_cast<std::size_t>(idx);
391  return r;
392  }
393  else
394  throw std::runtime_error(
395  "minCoeff(idx): Signature only valid for column vectors");
396 }
397 template <typename Scalar, class Derived>
399  std::size_t& rowIdx, std::size_t& colIdx) const
400 {
401  typename Derived::Index row, col;
402  auto r = mvbDerived().asEigen().minCoeff(&row, &col);
403  rowIdx = static_cast<std::size_t>(row);
404  colIdx = static_cast<std::size_t>(col);
405  return r;
406 }
407 
408 template <typename Scalar, class Derived>
410  std::size_t& rowIdx, std::size_t& colIdx) const
411 {
412  typename Derived::Index row, col;
413  auto r = mvbDerived().asEigen().maxCoeff(&row, &col);
414  rowIdx = static_cast<std::size_t>(row);
415  colIdx = static_cast<std::size_t>(col);
416  return r;
417 }
418 
419 template <typename Scalar, class Derived>
421 {
422  mvbDerived().asEigen().array() += s;
423 }
424 
425 template <typename Scalar, class Derived>
427 {
428  mvbDerived().asEigen().array() -= s;
429 }
430 
431 template <typename Scalar, class Derived>
433 {
434  mvbDerived().asEigen().array() *= s;
435 }
436 
437 template <typename Scalar, class Derived>
439  const CMatrixDynamic<Scalar>& v)
440 {
441  return CMatrixDynamic<Scalar>(
442  (mvbDerived().asEigen() * v.asEigen()).eval());
443 }
444 
445 template <typename Scalar, class Derived>
446 Derived MatrixVectorBase<Scalar, Derived>::impl_op_add(const Derived& m2) const
447 {
448  Derived ret(mvbDerived().rows(), mvbDerived().cols());
449  ret.asEigen() = mvbDerived().asEigen() + m2.asEigen();
450  return ret;
451 }
452 template <typename Scalar, class Derived>
454 {
455  mvbDerived().asEigen() += m2.asEigen();
456 }
457 template <typename Scalar, class Derived>
458 Derived MatrixVectorBase<Scalar, Derived>::impl_op_subs(const Derived& m2) const
459 {
460  Derived ret(mvbDerived().rows(), mvbDerived().cols());
461  ret.asEigen() = mvbDerived().asEigen() - m2.asEigen();
462  return ret;
463 }
464 template <typename Scalar, class Derived>
466 {
467  mvbDerived().asEigen() -= m2.asEigen();
468 }
469 template <typename Scalar, class Derived>
470 Derived MatrixVectorBase<Scalar, Derived>::operator*(const Derived& m2) const
471 {
472  ASSERTMSG_(
473  mvbDerived().cols() == mvbDerived().rows(),
474  "Operator* implemented only for square matrices. Use `A.asEigen() * "
475  "B.asEigen()` for general matrix products.");
476  Derived ret(mvbDerived().rows(), mvbDerived().rows());
477  if constexpr (Derived::RowsAtCompileTime == Derived::ColsAtCompileTime)
478  {
479  ret.asEigen() = mvbDerived().asEigen() * m2.asEigen();
480  }
481  return ret;
482 }
483 
484 template <typename Scalar, class Derived>
487 {
488  mvbDerived() = A.asEigen() * b.asEigen();
489 }
490 
491 template <typename Scalar, class Derived>
494 {
495  mvbDerived() = A.asEigen().transpose() * b.asEigen();
496 }
497 
498 template <typename Scalar, class Derived>
500 {
501  return mvbDerived().asEigen().template lpNorm<Eigen::Infinity>();
502 }
503 
504 template <typename Scalar, class Derived>
506 {
507  return mvbDerived().asEigen().norm();
508 }
509 
510 template <typename Scalar, class Derived>
512  const CVectorDynamic<Scalar>& v) const
513 {
514  if constexpr (Derived::ColsAtCompileTime == 1)
515  {
516  return mvbDerived().asEigen().dot(v.mvbDerived().asEigen());
517  }
518  else
519  {
520  ASSERTMSG_(false, "dot(): Implemented for column vectors only.");
521  }
522 }
523 template <typename Scalar, class Derived>
525  const MatrixVectorBase<Scalar, Derived>& v) const
526 {
527  if constexpr (Derived::ColsAtCompileTime == 1)
528  {
529  return mvbDerived().asEigen().dot(v.mvbDerived().asEigen());
530  }
531  else
532  {
533  ASSERTMSG_(false, "dot(): Implemented for column vectors only.");
534  }
535 }
536 
537 } // 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).
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
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
int val
Definition: mrpt_jpeglib.h:957
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...
int fprintf(FILE *fil, const char *format,...) noexcept MRPT_printf_format_check(2
An OS-independent version of fprintf.
Definition: os.cpp:408
Scalar sum_abs() const
Sum of the absolute value of all elements in matrix/vector.
fixed floating point &#39;f&#39;
const_iterator end() const
Definition: ts_hash_map.h:246
void operator-=(const MatrixVectorBase< S2, D2 > &m2)
Scalar sum() const
Sum of all elements in matrix/vector.
Derived impl_op_add(const Derived &m2) const
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.
EIGEN_MAP asEigen()
Get as an Eigen-compatible Eigen::Map object.
void matProductOf_Atb(const CMatrixDynamic< Scalar > &A, const CVectorDynamic< Scalar > &b)
this = AT * b , with A and b a dynamic matrix & vector
EIGEN_MAP asEigen()
Get as an Eigen-compatible Eigen::Map object.
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: c7a3bec24 Sun Mar 29 18:33:13 2020 +0200 at dom mar 29 18:50:38 CEST 2020