Main MRPT website > C++ reference for 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  | 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 #pragma once
10 
11 #include <cstdarg>
12 #include <cstdio>
13 #include <cmath>
14 #include <iosfwd>
15 #include <vector>
16 #include <limits> // numeric_limits
17 #include <mrpt/core/exceptions.h>
18 #include <mrpt/math/eigen_frwds.h>
19 
20 namespace mrpt
21 {
22 /** This base provides a set of functions for maths stuff.
23  * \ingroup mrpt_math_grp
24  */
25 namespace math
26 {
27 /**\brief Compare 2 floats and determine whether they are equal
28  * \return True if equal, false otherwise
29  * \param a Fist num
30  * \param b Second num
31  * \param epsilon Difference below which a, b are considered equal
32  */
33 template <class T1, class T2>
34 bool approximatelyEqual(T1 a, T1 b, T2 epsilon)
35 {
36  return fabs(a - b) <= ((fabs(a) > fabs(b) ? fabs(b) : fabs(a)) * epsilon);
37 }
38 
39 /**\brief Compare 2 floats and determine whether they are equal
40  * \return True if equal, false otherwise
41  * \param a Fist num
42  * \param b Second num
43  */
44 template <class T>
46 {
47  return approximatelyEqual(a, b, std::numeric_limits<T>::epsilon());
48 }
49 
50 /**\brief Absolute difference between two numbers.
51  *
52  */
53 template <class T>
54 T absDiff(const T& lhs, const T& rhs)
55 {
56  return lhs > rhs ? lhs - rhs : rhs - lhs;
57 }
58 
59 /** \addtogroup container_ops_grp
60  * @{ */
61 
62 /** Loads one row of a text file as a numerical std::vector.
63  * \return false on EOF or invalid format.
64  * The body of the function is implemented in MATH.cpp
65  */
66 bool loadVector(std::istream& f, std::vector<int>& d);
67 
68 /** Loads one row of a text file as a numerical std::vector.
69  * \return false on EOF or invalid format.
70  * The body of the function is implemented in MATH.cpp
71  */
72 bool loadVector(std::istream& f, std::vector<double>& d);
73 
74 void medianFilter(
75  const std::vector<double>& inV, std::vector<double>& outV,
76  const int& winSize, const int& numberOfSigmas = 2);
77 
78 /** Generates an equidistant sequence of numbers given the first one, the last
79  one and the desired number of points.
80  \sa sequence */
81 template <typename T, typename VECTOR>
82 void linspace(T first, T last, size_t count, VECTOR& out_vector)
83 {
84  if (count < 2)
85  {
86  out_vector.assign(count, last);
87  return;
88  }
89  else
90  {
91  out_vector.resize(count);
92  const T incr = (last - first) / T(count - 1);
93  T c = first;
94  for (size_t i = 0; i < count; i++, c += incr) out_vector[i] = c;
95  }
96 }
97 
98 /** Generates a sequence of values [first,first+STEP,first+2*STEP,...] \sa
99  * linspace, sequence */
100 template <class T, T STEP>
101 inline std::vector<T> sequenceStdVec(T first, size_t length)
102 {
103  std::vector<T> ret(length);
104  if (!length) return ret;
105  size_t i = 0;
106  while (length--)
107  {
108  ret[i++] = first;
109  first += STEP;
110  }
111  return ret;
112 }
113 
114 /** Normalize a vector, such as its norm is the unity.
115  * If the vector has a null norm, the output is a null vector.
116  */
117 template <class VEC1, class VEC2>
118 void normalize(const VEC1& v, VEC2& out_v)
119 {
120  typename VEC1::Scalar total = 0;
121  const size_t N = v.size();
122  for (size_t i = 0; i < N; i++) total += square(v[i]);
123  total = std::sqrt(total);
124  if (total)
125  {
126  out_v = v * (1.0 / total);
127  }
128  else
129  out_v.assign(v.size(), 0);
130 }
131 
132 /** Extract a column from a vector of vectors, and store it in another vector.
133  * - Input data can be: std::vector<mrpt::math::CVectorDouble>,
134  * std::deque<std::list<double> >, std::list<CArrayDouble<5> >, etc. etc.
135  * - Output is the sequence: data[0][idx],data[1][idx],data[2][idx], etc..
136  *
137  * For the sake of generality, this function does NOT check the limits in
138  * the number of column, unless it's implemented in the [] operator of each of
139  * the "rows".
140  */
141 template <class VECTOR_OF_VECTORS, class VECTORLIKE>
143  const size_t colIndex, const VECTOR_OF_VECTORS& data,
144  VECTORLIKE& out_column)
145 {
146  const size_t N = data.size();
147  out_column.resize(N);
148  for (size_t i = 0; i < N; i++) out_column[i] = data[i][colIndex];
149 }
150 
151 /** Computes the factorial of an integer number and returns it as a 64-bit
152  * integer number.
153  */
154 uint64_t factorial64(unsigned int n);
155 
156 /** Computes the factorial of an integer number and returns it as a double value
157  * (internally it uses logarithms for avoiding overflow).
158  */
159 double factorial(unsigned int n);
160 
161 /** Generates a string with the MATLAB commands required to plot an confidence
162  * interval (ellipse) for a 2D Gaussian ('float' version)..
163  * \param cov22 The 2x2 covariance matrix
164  * \param mean The 2-length vector with the mean
165  * \param stdCount How many "quantiles" to get into the area of the ellipse:
166  * 2: 95%, 3:99.97%,...
167  * \param style A matlab style string, for colors, line styles,...
168  * \param nEllipsePoints The number of points in the ellipse to generate
169  * \ingroup stats_grp
170  */
172  const CMatrixFloat& cov22, const CVectorFloat& mean, const float& stdCount,
173  const std::string& style = std::string("b"),
174  const size_t& nEllipsePoints = 30);
175 
176 /** Generates a string with the MATLAB commands required to plot an confidence
177  * interval (ellipse) for a 2D Gaussian ('double' version).
178  * \param cov22 The 2x2 covariance matrix
179  * \param mean The 2-length vector with the mean
180  * \param stdCount How many "quantiles" to get into the area of the ellipse:
181  * 2: 95%, 3:99.97%,...
182  * \param style A matlab style string, for colors, line styles,...
183  * \param nEllipsePoints The number of points in the ellipse to generate
184  * \ingroup stats_grp
185  */
187  const CMatrixDouble& cov22, const CVectorDouble& mean,
188  const float& stdCount, const std::string& style = std::string("b"),
189  const size_t& nEllipsePoints = 30);
190 
191 /** Assignment operator for initializing a std::vector from a C array (The
192  *vector will be automatically set to the correct size).
193  * \code
194  * CVectorDouble v;
195  * const double numbers[] = { 1,2,3,5,6,7,8,9,10 };
196  * loadVector( v, numbers );
197  * \endcode
198  * \note This operator performs the appropiate type castings, if required.
199  */
200 template <typename EIGEN_VECTOR, typename At, size_t N>
201 EIGEN_VECTOR& loadVector(EIGEN_VECTOR& v, At (&theArray)[N])
202 {
203  MRPT_COMPILE_TIME_ASSERT(N != 0);
204  v.derived().resize(N);
205  for (size_t i = 0; i < N; i++)
206  (v.derived())[i] =
207  static_cast<typename EIGEN_VECTOR::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  MRPT_COMPILE_TIME_ASSERT(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
GLuint GLuint GLsizei count
Definition: glext.h:3528
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:354
double Scalar
Definition: KmUtils.h:44
void normalize(const VEC1 &v, VEC2 &out_v)
Normalize a vector, such as its norm is the unity.
int void fclose(FILE *f)
An OS-independent version of fclose.
Definition: os.cpp:273
GLint * first
Definition: glext.h:3827
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:5074
dynamic_vector< float > CVectorFloat
Column vector, like Eigen::MatrixXf, but automatically initialized to zeros since construction...
Definition: eigen_frwds.h:45
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:68
const GLubyte * c
Definition: glext.h:6313
CMatrixTemplateNumeric< double > CMatrixDouble
Declares a matrix of double numbers (non serializable).
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
GLubyte GLubyte b
Definition: glext.h:6279
CMatrixTemplateNumeric< float > CMatrixFloat
Declares a matrix of float numbers (non serializable).
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:4101
void medianFilter(const std::vector< double > &inV, std::vector< double > &outV, const int &winSize, const int &numberOfSigmas=2)
Definition: math.cpp:460
int fprintf(FILE *fil, const char *format,...) noexcept MRPT_printf_format_check(2
An OS-independent version of fprintf.
Definition: os.cpp:406
unsigned __int64 uint64_t
Definition: rptypes.h:50
const GLdouble * v
Definition: glext.h:3678
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
GLenum GLenum GLvoid * row
Definition: glext.h:3576
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:4064
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:255
T absDiff(const T &lhs, const T &rhs)
Absolute difference between two numbers.
dynamic_vector< double > CVectorDouble
Column vector, like Eigen::MatrixXd, but automatically initialized to zeros since construction...
Definition: eigen_frwds.h:46
#define MRPT_COMPILE_TIME_ASSERT(expression)
Definition: exceptions.h:127
GLsizei GLsizei GLenum GLenum const GLvoid * data
Definition: glext.h:3546
GLubyte GLubyte GLubyte a
Definition: glext.h:6279
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,...].



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: ad3a9d8ae Tue May 1 23:10:22 2018 -0700 at lun oct 28 00:14:14 CET 2019