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 |
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>
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
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,
78  const int& winSize, const 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  */
175  const CMatrixFloat& cov22, const CVectorFloat& mean, const float& stdCount,
176  const std::string& style = std::string("b"),
177  const size_t& nEllipsePoints = 30);
178
179 /** Generates a string with the MATLAB commands required to plot an confidence
180  * interval (ellipse) for a 2D Gaussian ('double' version).
181  * \param cov22 The 2x2 covariance matrix
182  * \param mean The 2-length vector with the mean
183  * \param stdCount How many "quantiles" to get into the area of the ellipse:
184  * 2: 95%, 3:99.97%,...
185  * \param style A matlab style string, for colors, line styles,...
186  * \param nEllipsePoints The number of points in the ellipse to generate
187  * \ingroup stats_grp
188  */
190  const CMatrixDouble& cov22, const CVectorDouble& mean,
191  const float& stdCount, const std::string& style = std::string("b"),
192  const size_t& nEllipsePoints = 30);
193
194 /** Assignment operator for initializing a std::vector from a C array (The
195  *vector will be automatically set to the correct size).
196  * \code
197  * CVectorDouble v;
198  * const double numbers[] = { 1,2,3,5,6,7,8,9,10 };
199  * loadVector( v, numbers );
200  * \endcode
201  * \note This operator performs the appropiate type castings, if required.
202  */
203 template <typename VECTOR_T, typename At, size_t N>
204 VECTOR_T& loadVector(VECTOR_T& v, At (&theArray)[N])
205 {
206  static_assert(N != 0, "N!=0");
207  v.resize(N);
208  for (size_t i = 0; i < N; i++)
209  v[i] = static_cast<typename VECTOR_T::Scalar>(theArray[i]);
210  return v;
211 }
213 template <typename T, typename At, size_t N>
214 std::vector<T>& loadVector(std::vector<T>& v, At (&theArray)[N])
215 {
216  static_assert(N != 0, "N!=0");
217  v.resize(N);
218  for (size_t i = 0; i < N; i++) v[i] = static_cast<T>(theArray[i]);
219  return v;
220 }
221
222 /** @} */ // end of grouping container_ops_grp
223
224 /** \defgroup mrpt_math_io Custom I/O for math containers
225  * \ingroup mrpt_math_grp */
227  * @{ */
228
229 /** Saves to a plain-text file the nonzero entries of a Eigen sparse matrix,
230  * represented as a vector of triplets.
231  * Output format is one line per entry with the format: "i j val", i:row,
232  * j:col, val:value.
233  * \tparam TRIPLET should be Eigen::Triplet<T>
234  */
235 template <class TRIPLET>
237  const std::string& sFile, std::vector<TRIPLET>& tri)
238 {
239 #if defined(_MSC_VER) && \
240  (_MSC_VER >= 1400) // Use a secure version in Visual Studio 2005+
241  FILE* f;
242  if (0 != ::fopen_s(&f, sFile.c_str(), "wt")) f = nullptr;
243 #else
244  FILE* f = ::fopen(sFile.c_str(), "wt");
245 #endif
246
247  if (!f) return false;
248
249  for (size_t i = 0; i < tri.size(); i++)
250  fprintf(
251  f, "%u %u %e\n", 1 + tri[i].row(), 1 + tri[i].col(),
252  tri[i].value());
253
254  fclose(f);
255  return true;
256 }
257
258 /** @} */ // End of mrpt_math_io
259
260 } // namespace math
261
262 } // namespace mrpt
GLuint GLuint GLsizei count
Definition: glext.h:3532
std::string MATLAB_plotCovariance2D(const CMatrixFloat &cov22, const CVectorFloat &mean, const float &stdCount, const std::string &style=std::string("b"), const size_t &nEllipsePoints=30)
Generates a string with the MATLAB commands required to plot an confidence interval (ellipse) for a 2...
Definition: math.cpp:357
double Scalar
Definition: KmUtils.h:43
int void fclose(FILE *f)
An OS-independent version of fclose.
Definition: os.cpp:275
GLint * first
Definition: glext.h:3833
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...
GLenum GLsizei n
Definition: glext.h:5136
CMatrixDynamic< float > CMatrixFloat
Declares a matrix of float numbers (non serializable).
T square(const T x)
Inline function for the square of a number.
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:69
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...
const GLubyte * c
Definition: glext.h:6406
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:57
GLubyte GLubyte b
Definition: glext.h:6372
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...
GLsizei const GLchar ** string
Definition: glext.h:4116
CVectorDynamic< float > CVectorFloat
void medianFilter(const std::vector< double > &inV, std::vector< double > &outV, const int &winSize, const int &numberOfSigmas=2)
Definition: math.cpp:465
int fprintf(FILE *fil, const char *format,...) noexcept MRPT_printf_format_check(2
An OS-independent version of fprintf.
Definition: os.cpp:410
unsigned __int64 uint64_t
Definition: rptypes.h:53
const GLdouble * v
Definition: glext.h:3684
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
GLenum GLenum GLvoid * row
Definition: glext.h:3580
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.
GLuint GLsizei GLsizei * length
Definition: glext.h:4079
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.
GLsizei GLsizei GLenum GLenum const GLvoid * data
Definition: glext.h:3550
GLubyte GLubyte GLubyte a
Definition: glext.h:6372
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: 60a755017 Wed Aug 21 03:10:09 2019 +0200 at miĆ© ago 21 03:20:12 CEST 2019