Main MRPT website > C++ reference for MRPT 1.5.9
base/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-2017, 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 #ifndef MRPT_MATH_H
10 #define MRPT_MATH_H
11 
12 #include <mrpt/utils/utils_defs.h>
13 #include <cstdarg>
14 #include <cstdio>
15 #include <mrpt/math/eigen_frwds.h>
16 #include <map>
17 
18 namespace mrpt
19 {
20  /** This base provides a set of functions for maths stuff.
21  * \ingroup mrpt_base_grp
22  */
23  namespace math
24  {
25  /**\brief Compare 2 floats and determine whether they are equal
26  * \return True if equal, false otherwise
27  * \param a Fist num
28  * \param b Second num
29  * \param epsilon Difference below which a, b are considered equal
30  */
31  template<class T1, class T2>
32  bool approximatelyEqual(T1 a, T1 b, T2 epsilon) {
33  return fabs(a - b) <= ( (fabs(a) > fabs(b) ? fabs(b) : fabs(a)) * epsilon);
34  }
35 
36  /**\brief Compare 2 floats and determine whether they are equal
37  * \return True if equal, false otherwise
38  * \param a Fist num
39  * \param b Second num
40  */
41  template<class T>
42  bool approximatelyEqual(T a, T b) {
43  return approximatelyEqual(a, b, std::numeric_limits<T>::epsilon());
44  }
45 
46  /**\brief Absolute difference between two numbers.
47  *
48  */
49  template<class T>
50  T absDiff(const T& lhs, const T& rhs) {
51  return lhs>rhs ? lhs - rhs : rhs - lhs;
52  }
53 
54  /** \addtogroup container_ops_grp
55  * @{ */
56 
57  /** Loads one row of a text file as a numerical std::vector.
58  * \return false on EOF or invalid format.
59  * The body of the function is implemented in MATH.cpp
60  */
61  bool BASE_IMPEXP loadVector( utils::CFileStream &f, std::vector<int> &d);
62 
63  /** Loads one row of a text file as a numerical std::vector.
64  * \return false on EOF or invalid format.
65  * The body of the function is implemented in MATH.cpp
66  */
67  bool BASE_IMPEXP loadVector( utils::CFileStream &f, std::vector<double> &d);
68 
69 
70  /** Returns true if the number is NaN. */
71  bool BASE_IMPEXP isNaN(float f) MRPT_NO_THROWS;
72 
73  /** Returns true if the number is NaN. */
74  bool BASE_IMPEXP isNaN(double f) MRPT_NO_THROWS;
75 
76  /** Returns true if the number is non infinity. */
77  bool BASE_IMPEXP isFinite(float f) MRPT_NO_THROWS;
78 
79  /** Returns true if the number is non infinity. */
80  bool BASE_IMPEXP isFinite(double f) MRPT_NO_THROWS;
81 
82  void BASE_IMPEXP medianFilter( const std::vector<double> &inV, std::vector<double> &outV, const int &winSize, const int &numberOfSigmas = 2 );
83 
84 #ifdef HAVE_LONG_DOUBLE
85  /** Returns true if the number is NaN. */
86  bool BASE_IMPEXP isNaN(long double f) MRPT_NO_THROWS;
87 
88  /** Returns true if the number is non infinity. */
89  bool BASE_IMPEXP isFinite(long double f) MRPT_NO_THROWS;
90 #endif
91 
92  /** Generates an equidistant sequence of numbers given the first one, the last one and the desired number of points.
93  \sa sequence */
94  template<typename T,typename VECTOR>
95  void linspace(T first,T last, size_t count, VECTOR &out_vector)
96  {
97  if (count<2)
98  {
99  out_vector.assign(count,last);
100  return;
101  }
102  else
103  {
104  out_vector.resize(count);
105  const T incr = (last-first)/T(count-1);
106  T c = first;
107  for (size_t i=0;i<count;i++,c+=incr)
108  out_vector[i] = c;
109  }
110  }
111 
112  /** Generates a sequence of values [first,first+STEP,first+2*STEP,...] \sa linspace, sequence */
113  template<class T,T STEP>
114  inline std::vector<T> sequenceStdVec(T first,size_t length)
115  {
116  std::vector<T> ret(length);
117  if (!length) return ret;
118  size_t i=0;
119  while (length--) { ret[i++]=first; first+=STEP; }
120  return ret;
121  }
122 
123  /** Normalize a vector, such as its norm is the unity.
124  * If the vector has a null norm, the output is a null vector.
125  */
126  template<class VEC1,class VEC2>
127  void normalize(const VEC1 &v, VEC2 &out_v)
128  {
129  typename VEC1::Scalar total=0;
130  const size_t N = v.size();
131  for (size_t i=0;i<N;i++)
132  total += square(v[i]);
133  total = std::sqrt(total);
134  if (total)
135  {
136  out_v = v * (1.0/total);
137  }
138  else out_v.assign(v.size(),0);
139  }
140 
141  /** Extract a column from a vector of vectors, and store it in another vector.
142  * - Input data can be: std::vector<mrpt::math::CVectorDouble>, std::deque<std::list<double> >, std::list<CArrayDouble<5> >, etc. etc.
143  * - Output is the sequence: data[0][idx],data[1][idx],data[2][idx], etc..
144  *
145  * For the sake of generality, this function does NOT check the limits in the number of column, unless it's implemented in the [] operator of each of the "rows".
146  */
147  template <class VECTOR_OF_VECTORS, class VECTORLIKE>
148  inline void extractColumnFromVectorOfVectors(const size_t colIndex, const VECTOR_OF_VECTORS &data, VECTORLIKE &out_column)
149  {
150  const size_t N = data.size();
151  out_column.resize(N);
152  for (size_t i=0;i<N;i++)
153  out_column[i]=data[i][colIndex];
154  }
155 
156  /** Computes the factorial of an integer number and returns it as a 64-bit integer number.
157  */
158  uint64_t BASE_IMPEXP factorial64(unsigned int n);
159 
160  /** Computes the factorial of an integer number and returns it as a double value (internally it uses logarithms for avoiding overflow).
161  */
162  double BASE_IMPEXP factorial(unsigned int n);
163 
164  /** Round up to the nearest power of two of a given number
165  */
166  template <class T>
168  {
169  T n = 1;
170  while (n < val)
171  {
172  n <<= 1;
173  if (n<=1)
174  THROW_EXCEPTION("Overflow!");
175  }
176  return n;
177  }
178 
179  /** Generates a string with the MATLAB commands required to plot an confidence interval (ellipse) for a 2D Gaussian ('float' 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: 2: 95%, 3:99.97%,...
183  * \param style A matlab style string, for colors, line styles,...
184  * \param nEllipsePoints The number of points in the ellipse to generate
185  * \ingroup stats_grp
186  */
188  const CMatrixFloat &cov22,
189  const CVectorFloat &mean,
190  const float &stdCount,
191  const std::string &style = std::string("b"),
192  const size_t &nEllipsePoints = 30 );
193 
194  /** Generates a string with the MATLAB commands required to plot an confidence interval (ellipse) for a 2D Gaussian ('double' version).
195  * \param cov22 The 2x2 covariance matrix
196  * \param mean The 2-length vector with the mean
197  * \param stdCount How many "quantiles" to get into the area of the ellipse: 2: 95%, 3:99.97%,...
198  * \param style A matlab style string, for colors, line styles,...
199  * \param nEllipsePoints The number of points in the ellipse to generate
200  * \ingroup stats_grp
201  */
203  const CMatrixDouble &cov22,
204  const CVectorDouble &mean,
205  const float &stdCount,
206  const std::string &style = std::string("b"),
207  const size_t &nEllipsePoints = 30 );
208 
209 
210 
211  /** Assignment operator for initializing a std::vector from a C array (The vector will be automatically set to the correct size).
212  * \code
213  * CVectorDouble v;
214  * const double numbers[] = { 1,2,3,5,6,7,8,9,10 };
215  * loadVector( v, numbers );
216  * \endcode
217  * \note This operator performs the appropiate type castings, if required.
218  */
219  template <typename EIGEN_VECTOR, typename At, size_t N>
220  EIGEN_VECTOR& loadVector(EIGEN_VECTOR &v, At (&theArray)[N] )
221  {
223  v.derived().resize(N);
224  for (size_t i=0; i < N; i++)
225  (v.derived())[i] = static_cast<typename EIGEN_VECTOR::Scalar>(theArray[i]);
226  return v;
227  }
228  //! \overload
229  template <typename T, typename At, size_t N>
230  std::vector<T>& loadVector( std::vector<T> &v, At (&theArray)[N] )
231  {
233  v.resize(N);
234  for (size_t i=0; i < N; i++)
235  v[i] = static_cast<T>(theArray[i]);
236  return v;
237  }
238 
239  /** A versatile template to build vectors on-the-fly in a style close to MATLAB's v=[a b c d ...]
240  * The first argument of the template is the vector length, and the second the type of the numbers.
241  * Some examples:
242  *
243  * \code
244  * std::vector<double> = make_vector<4,double>(1.0,3.0,4.0,5.0);
245  * std::vector<float> = make_vector<2,float>(-8.12, 3e4);
246  * \endcode
247  */
248  template <size_t N, typename T>
249  std::vector<T> make_vector(const T val1, ...)
250  {
252  std::vector<T> ret;
253  ret.reserve(N);
254 
255  ret.push_back(val1);
256 
257  va_list args;
258  va_start(args,val1);
259  for (size_t i=1;i<N;i++)
260  ret.push_back( va_arg(args,T) );
261 
262  va_end(args);
263  return ret;
264  }
265 
266  /** @} */ // end of grouping container_ops_grp
267 
268 
269 
270 
271  /** \defgroup mrpt_math_io Custom I/O for math containers
272  * \ingroup mrpt_base_grp */
273  /** \addtogroup mrpt_math_io
274  * @{ */
275 
276  /** Saves to a plain-text file the nonzero entries of a Eigen sparse matrix, represented as a vector of triplets.
277  * Output format is one line per entry with the format: "i j val", i:row, j:col, val:value.
278  * \tparam TRIPLET should be Eigen::Triplet<T>
279  */
280  template <class TRIPLET>
281  bool saveEigenSparseTripletsToFile(const std::string &sFile, std::vector<TRIPLET> &tri)
282  {
283 #if defined(_MSC_VER) && (_MSC_VER>=1400) // Use a secure version in Visual Studio 2005+
284  FILE *f;
285  if (0!=::fopen_s(&f,sFile.c_str(),"wt")) f= NULL;
286 #else
287  FILE *f= ::fopen(sFile.c_str(),"wt");
288 #endif
289 
290  if (!f) return false;
291 
292  for (size_t i=0;i<tri.size();i++)
293  fprintf(f,"%u %u %e\n", 1+tri[i].row(), 1+tri[i].col(), tri[i].value() );
294 
295  fclose(f);
296  return true;
297  }
298 
299  /** @} */ // End of mrpt_math_io
300 
301 
302  } // End of MATH namespace
303 
304 } // End of namespace
305 
306 #endif
GLuint GLuint GLsizei count
Definition: glext.h:3512
FILE BASE_IMPEXP * fopen(const char *fileName, const char *mode) MRPT_NO_THROWS
An OS-independent version of fopen.
Definition: os.cpp:255
std::string BASE_IMPEXP 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:1905
void normalize(const VEC1 &v, VEC2 &out_v)
Normalize a vector, such as its norm is the unity.
int BASE_IMPEXP void BASE_IMPEXP fclose(FILE *f)
An OS-independent version of fclose.
Definition: os.cpp:272
GLint * first
Definition: glext.h:3703
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...
#define THROW_EXCEPTION(msg)
GLenum GLsizei n
Definition: glext.h:4618
This CStream derived class allow using a file as a read/write binary stream, creating it if the file ...
Definition: CFileStream.h:39
#define MRPT_NO_THROWS
C++11 noexcept: Used after member declarations.
std::vector< T > make_vector(const T val1,...)
A versatile template to build vectors on-the-fly in a style close to MATLAB&#39;s v=[a b c d ...
int BASE_IMPEXP fprintf(FILE *fil, const char *format,...) MRPT_NO_THROWS MRPT_printf_format_check(2
An OS-independent version of fprintf.
Definition: os.cpp:412
dynamic_vector< float > CVectorFloat
Column vector, like Eigen::MatrixXf, but automatically initialized to zeros since construction...
Definition: eigen_frwds.h:35
T square(const T x)
Inline function for the square of a number.
Definition: bits.h:52
double BASE_IMPEXP 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:991
bool BASE_IMPEXP isNaN(float f) MRPT_NO_THROWS
Returns true if the number is NaN.
Definition: math.cpp:1679
T round2up(T val)
Round up to the nearest power of two of a given number.
const GLubyte * c
Definition: glext.h:5590
CMatrixTemplateNumeric< double > CMatrixDouble
Declares a matrix of double numbers (non serializable).
uint64_t BASE_IMPEXP factorial64(unsigned int n)
Computes the factorial of an integer number and returns it as a 64-bit integer number.
Definition: math.cpp:978
int val
Definition: mrpt_jpeglib.h:953
GLubyte GLubyte b
Definition: glext.h:5575
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:3919
bool BASE_IMPEXP isFinite(float f) MRPT_NO_THROWS
Returns true if the number is non infinity.
Definition: math.cpp:1705
#define MRPT_COMPILE_TIME_ASSERT(f)
unsigned __int64 uint64_t
Definition: rptypes.h:52
const GLdouble * v
Definition: glext.h:3603
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
bool BASE_IMPEXP loadVector(utils::CFileStream &f, std::vector< int > &d)
Loads one row of a text file as a numerical std::vector.
GLenum GLenum GLvoid * row
Definition: glext.h:3533
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:3900
double mean(const CONTAINER &v)
Computes the mean value of a vector.
void BASE_IMPEXP medianFilter(const std::vector< double > &inV, std::vector< double > &outV, const int &winSize, const int &numberOfSigmas=2)
Definition: math.cpp:2063
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:37
GLsizei GLsizei GLenum GLenum const GLvoid * data
Definition: glext.h:3520
double Scalar
Definition: KmUtils.h:41
GLubyte GLubyte GLubyte a
Definition: glext.h:5575
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.5.9 Git: 690a4699f Wed Apr 15 19:29:53 2020 +0200 at miƩ abr 15 19:30:12 CEST 2020