MRPT  1.9.9
math/include/mrpt/math/utils.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>
14 #include <mrpt/math/math_frwds.h>
15 #include <cmath>
16 #include <cstdarg>
17 #include <cstdio>
18 #include <iosfwd>
19 #include <limits> // numeric_limits
20 #include <vector>
21 
22 namespace mrpt
23 {
24 /** This base provides a set of functions for maths stuff.
25  * \ingroup mrpt_math_grp
26  */
27 namespace math
28 {
29 /**\brief Compare 2 floats and determine whether they are equal
30  * \return True if equal, false otherwise
31  * \param a Fist num
32  * \param b Second num
33  * \param epsilon Difference below which a, b are considered equal
34  */
35 template <class T1, class T2>
36 bool approximatelyEqual(T1 a, T1 b, T2 epsilon)
37 {
38  return fabs(a - b) <= ((fabs(a) > fabs(b) ? fabs(b) : fabs(a)) * epsilon);
39 }
40 
41 /**\brief Compare 2 floats and determine whether they are equal
42  * \return True if equal, false otherwise
43  * \param a Fist num
44  * \param b Second num
45  */
46 template <class T>
47 bool approximatelyEqual(T a, T b)
48 {
49  return approximatelyEqual(a, b, std::numeric_limits<T>::epsilon());
50 }
51 
52 /**\brief Absolute difference between two numbers.
53  *
54  */
55 template <class T>
56 T absDiff(const T& lhs, const T& rhs)
57 {
58  return lhs > rhs ? lhs - rhs : rhs - lhs;
59 }
60 
61 /** \addtogroup container_ops_grp
62  * @{ */
63 
64 /** Loads one row of a text file as a numerical std::vector.
65  * \return false on EOF or invalid format.
66  * The body of the function is implemented in MATH.cpp
67  */
68 bool loadVector(std::istream& f, std::vector<int>& d);
69 
70 /** Loads one row of a text file as a numerical std::vector.
71  * \return false on EOF or invalid format.
72  * The body of the function is implemented in MATH.cpp
73  */
74 bool loadVector(std::istream& f, std::vector<double>& d);
75 
76 void medianFilter(
77  const std::vector<double>& inV, std::vector<double>& outV, int winSize,
78  int numberOfSigmas = 2);
79 
80 /** Generates an equidistant sequence of numbers given the first one, the last
81  one and the desired number of points.
82  \sa sequence */
83 template <typename T, typename VECTOR>
84 void linspace(T first, T last, size_t count, VECTOR& out_vector)
85 {
86  if (count < 2)
87  {
88  out_vector.assign(count, last);
89  return;
90  }
91  else
92  {
93  out_vector.resize(count);
94  const T incr = (last - first) / T(count - 1);
95  T c = first;
96  for (size_t i = 0; i < count; i++, c += incr) out_vector[i] = c;
97  }
98 }
99 
100 /** Generates a sequence of values [first,first+STEP,first+2*STEP,...] \sa
101  * linspace, sequence */
102 template <class T, T STEP>
103 inline std::vector<T> sequenceStdVec(T first, size_t length)
104 {
105  std::vector<T> ret(length);
106  if (!length) return ret;
107  size_t i = 0;
108  while (length--)
109  {
110  ret[i++] = first;
111  first += STEP;
112  }
113  return ret;
114 }
115 
116 /** Normalize a vector, such as its norm is the unity.
117  * If the vector has a null norm, the output is a null vector.
118  */
119 template <class VEC1, class VEC2>
120 void normalize(const VEC1& v, VEC2& out_v)
121 {
122  typename VEC1::Scalar total = 0;
123  const size_t N = v.size();
124  for (size_t i = 0; i < N; i++) total += square(v[i]);
125  total = std::sqrt(total);
126  if (total)
127  {
128  out_v = v;
129  out_v *= (1.0 / total);
130  }
131  else
132  out_v.assign(v.size(), 0);
133 }
134 
135 /** Extract a column from a vector of vectors, and store it in another vector.
136  * - Input data can be: std::vector<mrpt::math::CVectorDouble>,
137  * std::deque<std::list<double> >, std::list<CVectorFixedDouble<5> >, etc. etc.
138  * - Output is the sequence: data[0][idx],data[1][idx],data[2][idx], etc..
139  *
140  * For the sake of generality, this function does NOT check the limits in
141  * the number of column, unless it's implemented in the [] operator of each of
142  * the "rows".
143  */
144 template <class VECTOR_OF_VECTORS, class VECTORLIKE>
146  const size_t colIndex, const VECTOR_OF_VECTORS& data,
147  VECTORLIKE& out_column)
148 {
149  const size_t N = data.size();
150  out_column.resize(N);
151  for (size_t i = 0; i < N; i++) out_column[i] = data[i][colIndex];
152 }
153 
154 /** Computes the factorial of an integer number and returns it as a 64-bit
155  * integer number.
156  */
157 uint64_t factorial64(unsigned int n);
158 
159 /** Computes the factorial of an integer number and returns it as a double value
160  * (internally it uses logarithms for avoiding overflow).
161  */
162 double factorial(unsigned int n);
163 
164 /** Generates a string with the MATLAB commands required to plot an confidence
165  * interval (ellipse) for a 2D Gaussian ('float' version)..
166  * \param cov22 The 2x2 covariance matrix
167  * \param mean The 2-length vector with the mean
168  * \param stdCount How many "quantiles" to get into the area of the ellipse:
169  * 2: 95%, 3:99.97%,...
170  * \param style A matlab style string, for colors, line styles,...
171  * \param nEllipsePoints The number of points in the ellipse to generate
172  * \ingroup stats_grp
173  */
174 std::string MATLAB_plotCovariance2D(
175  const CMatrixFloat& cov22, const CVectorFloat& mean, float stdCount,
176  const std::string& style = std::string("b"), size_t nEllipsePoints = 30);
177 
178 /** Generates a string with the MATLAB commands required to plot an confidence
179  * interval (ellipse) for a 2D Gaussian ('double' version).
180  * \param cov22 The 2x2 covariance matrix
181  * \param mean The 2-length vector with the mean
182  * \param stdCount How many "quantiles" to get into the area of the ellipse:
183  * 2: 95%, 3:99.97%,...
184  * \param style A matlab style string, for colors, line styles,...
185  * \param nEllipsePoints The number of points in the ellipse to generate
186  * \ingroup stats_grp
187  */
188 std::string MATLAB_plotCovariance2D(
189  const CMatrixDouble& cov22, const CVectorDouble& mean, float stdCount,
190  const std::string& style = std::string("b"), size_t nEllipsePoints = 30);
191 
192 /** Assignment operator for initializing a std::vector from a C array (The
193  *vector will be automatically set to the correct size).
194  * \code
195  * CVectorDouble v;
196  * const double numbers[] = { 1,2,3,5,6,7,8,9,10 };
197  * loadVector( v, numbers );
198  * \endcode
199  * \note This operator performs the appropiate type castings, if required.
200  */
201 template <typename VECTOR_T, typename At, size_t N>
202 VECTOR_T& loadVector(VECTOR_T& v, At (&theArray)[N])
203 {
204  static_assert(N != 0, "N!=0");
205  v.resize(N);
206  for (size_t i = 0; i < N; i++)
207  v[i] = static_cast<typename VECTOR_T::Scalar>(theArray[i]);
208  return v;
209 }
210 //! \overload
211 template <typename T, typename At, size_t N>
212 std::vector<T>& loadVector(std::vector<T>& v, At (&theArray)[N])
213 {
214  static_assert(N != 0, "N!=0");
215  v.resize(N);
216  for (size_t i = 0; i < N; i++) v[i] = static_cast<T>(theArray[i]);
217  return v;
218 }
219 
220 /** @} */ // end of grouping container_ops_grp
221 
222 /** \defgroup mrpt_math_io Custom I/O for math containers
223  * \ingroup mrpt_math_grp */
224 /** \addtogroup mrpt_math_io
225  * @{ */
226 
227 /** Saves to a plain-text file the nonzero entries of a Eigen sparse matrix,
228  * represented as a vector of triplets.
229  * Output format is one line per entry with the format: "i j val", i:row,
230  * j:col, val:value.
231  * \tparam TRIPLET should be Eigen::Triplet<T>
232  */
233 template <class TRIPLET>
235  const std::string& sFile, std::vector<TRIPLET>& tri)
236 {
237 #if defined(_MSC_VER) && \
238  (_MSC_VER >= 1400) // Use a secure version in Visual Studio 2005+
239  FILE* f;
240  if (0 != ::fopen_s(&f, sFile.c_str(), "wt")) f = nullptr;
241 #else
242  FILE* f = ::fopen(sFile.c_str(), "wt");
243 #endif
244 
245  if (!f) return false;
246 
247  for (size_t i = 0; i < tri.size(); i++)
248  fprintf(
249  f, "%u %u %e\n", 1 + tri[i].row(), 1 + tri[i].col(),
250  tri[i].value());
251 
252  fclose(f);
253  return true;
254 }
255 
256 /** @} */ // End of mrpt_math_io
257 
258 } // namespace math
259 
260 } // namespace mrpt
double Scalar
Definition: KmUtils.h:43
int void fclose(FILE *f)
An OS-independent version of fclose.
Definition: os.cpp:275
bool saveEigenSparseTripletsToFile(const std::string &sFile, std::vector< TRIPLET > &tri)
Saves to a plain-text file the nonzero entries of a Eigen sparse matrix, represented as a vector of t...
CMatrixDynamic< float > CMatrixFloat
Declares a matrix of float numbers (non serializable).
double factorial(unsigned int n)
Computes the factorial of an integer number and returns it as a double value (internally it uses loga...
Definition: math.cpp:68
CVectorDynamic< double > CVectorDouble
void normalize(CONTAINER &c, Scalar valMin, Scalar valMax)
Scales all elements such as the minimum & maximum values are shifted to the given values...
T square(const T x)
Inline function for the square of a number.
bool loadVector(std::istream &f, std::vector< int > &d)
Loads one row of a text file as a numerical std::vector.
uint64_t factorial64(unsigned int n)
Computes the factorial of an integer number and returns it as a 64-bit integer number.
Definition: math.cpp:56
void linspace(T first, T last, size_t count, VECTOR &out_vector)
Generates an equidistant sequence of numbers given the first one, the last one and the desired number...
CVectorDynamic< float > CVectorFloat
int fprintf(FILE *fil, const char *format,...) noexcept MRPT_printf_format_check(2
An OS-independent version of fprintf.
Definition: os.cpp:410
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
void extractColumnFromVectorOfVectors(const size_t colIndex, const VECTOR_OF_VECTORS &data, VECTORLIKE &out_column)
Extract a column from a vector of vectors, and store it in another vector.
void medianFilter(const std::vector< double > &inV, std::vector< double > &outV, int winSize, int numberOfSigmas=2)
Definition: math.cpp:462
double mean(const CONTAINER &v)
Computes the mean value of a vector.
FILE * fopen(const char *fileName, const char *mode) noexcept
An OS-independent version of fopen.
Definition: os.cpp:257
T absDiff(const T &lhs, const T &rhs)
Absolute difference between two numbers.
std::string MATLAB_plotCovariance2D(const CMatrixFloat &cov22, const CVectorFloat &mean, float stdCount, const std::string &style=std::string("b"), size_t nEllipsePoints=30)
Generates a string with the MATLAB commands required to plot an confidence interval (ellipse) for a 2...
Definition: math.cpp:356
bool approximatelyEqual(T1 a, T1 b, T2 epsilon)
Compare 2 floats and determine whether they are equal.
std::vector< T > sequenceStdVec(T first, size_t length)
Generates a sequence of values [first,first+STEP,first+2*STEP,...].
CMatrixDynamic< double > CMatrixDouble
Declares a matrix of double numbers (non serializable).



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: 24b95e159 Thu Jan 23 01:15:46 2020 +0100 at jue ene 23 01:30:10 CET 2020