Main MRPT website > C++ reference for MRPT 1.4.0
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-2016, 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 
17 namespace mrpt
18 {
19  /** This base provides a set of functions for maths stuff. \ingroup mrpt_base_grp
20  */
21  namespace math
22  {
23  /** \addtogroup container_ops_grp
24  * @{ */
25 
26  /** Loads one row of a text file as a numerical std::vector.
27  * \return false on EOF or invalid format.
28  * The body of the function is implemented in MATH.cpp
29  */
30  bool BASE_IMPEXP loadVector( utils::CFileStream &f, std::vector<int> &d);
31 
32  /** Loads one row of a text file as a numerical std::vector.
33  * \return false on EOF or invalid format.
34  * The body of the function is implemented in MATH.cpp
35  */
36  bool BASE_IMPEXP loadVector( utils::CFileStream &f, std::vector<double> &d);
37 
38 
39  /** Returns true if the number is NaN. */
40  bool BASE_IMPEXP isNaN(float f) MRPT_NO_THROWS;
41 
42  /** Returns true if the number is NaN. */
43  bool BASE_IMPEXP isNaN(double f) MRPT_NO_THROWS;
44 
45  /** Returns true if the number is non infinity. */
46  bool BASE_IMPEXP isFinite(float f) MRPT_NO_THROWS;
47 
48  /** Returns true if the number is non infinity. */
49  bool BASE_IMPEXP isFinite(double f) MRPT_NO_THROWS;
50 
51  void BASE_IMPEXP medianFilter( const std::vector<double> &inV, std::vector<double> &outV, const int &winSize, const int &numberOfSigmas = 2 );
52 
53 #ifdef HAVE_LONG_DOUBLE
54  /** Returns true if the number is NaN. */
55  bool BASE_IMPEXP isNaN(long double f) MRPT_NO_THROWS;
56 
57  /** Returns true if the number is non infinity. */
58  bool BASE_IMPEXP isFinite(long double f) MRPT_NO_THROWS;
59 #endif
60 
61  /** Generates an equidistant sequence of numbers given the first one, the last one and the desired number of points.
62  \sa sequence */
63  template<typename T,typename VECTOR>
64  void linspace(T first,T last, size_t count, VECTOR &out_vector)
65  {
66  if (count<2)
67  {
68  out_vector.assign(count,last);
69  return;
70  }
71  else
72  {
73  out_vector.resize(count);
74  const T incr = (last-first)/T(count-1);
75  T c = first;
76  for (size_t i=0;i<count;i++,c+=incr)
77  out_vector[i] = c;
78  }
79  }
80 
81  /** Generates a sequence of values [first,first+STEP,first+2*STEP,...] \sa linspace, sequence */
82  template<class T,T STEP>
83  inline std::vector<T> sequenceStdVec(T first,size_t length)
84  {
85  std::vector<T> ret(length);
86  if (!length) return ret;
87  size_t i=0;
88  while (length--) { ret[i++]=first; first+=STEP; }
89  return ret;
90  }
91 
92  /** Normalize a vector, such as its norm is the unity.
93  * If the vector has a null norm, the output is a null vector.
94  */
95  template<class VEC1,class VEC2>
96  void normalize(const VEC1 &v, VEC2 &out_v)
97  {
98  typename VEC1::Scalar total=0;
99  const size_t N = v.size();
100  for (size_t i=0;i<N;i++)
101  total += square(v[i]);
102  total = std::sqrt(total);
103  if (total)
104  {
105  out_v = v * (1.0/total);
106  }
107  else out_v.assign(v.size(),0);
108  }
109 
110  /** Extract a column from a vector of vectors, and store it in another vector.
111  * - Input data can be: std::vector<mrpt::math::CVectorDouble>, std::deque<std::list<double> >, std::list<CArrayDouble<5> >, etc. etc.
112  * - Output is the sequence: data[0][idx],data[1][idx],data[2][idx], etc..
113  *
114  * 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".
115  */
116  template <class VECTOR_OF_VECTORS, class VECTORLIKE>
117  inline void extractColumnFromVectorOfVectors(const size_t colIndex, const VECTOR_OF_VECTORS &data, VECTORLIKE &out_column)
118  {
119  const size_t N = data.size();
120  out_column.resize(N);
121  for (size_t i=0;i<N;i++)
122  out_column[i]=data[i][colIndex];
123  }
124 
125  /** Computes the factorial of an integer number and returns it as a 64-bit integer number.
126  */
127  uint64_t BASE_IMPEXP factorial64(unsigned int n);
128 
129  /** Computes the factorial of an integer number and returns it as a double value (internally it uses logarithms for avoiding overflow).
130  */
131  double BASE_IMPEXP factorial(unsigned int n);
132 
133  /** Round up to the nearest power of two of a given number
134  */
135  template <class T>
136  T round2up(T val)
137  {
138  T n = 1;
139  while (n < val)
140  {
141  n <<= 1;
142  if (n<=1)
143  THROW_EXCEPTION("Overflow!");
144  }
145  return n;
146  }
147 
148  /** Generates a string with the MATLAB commands required to plot an confidence interval (ellipse) for a 2D Gaussian ('float' version)..
149  * \param cov22 The 2x2 covariance matrix
150  * \param mean The 2-length vector with the mean
151  * \param stdCount How many "quantiles" to get into the area of the ellipse: 2: 95%, 3:99.97%,...
152  * \param style A matlab style string, for colors, line styles,...
153  * \param nEllipsePoints The number of points in the ellipse to generate
154  * \ingroup stats_grp
155  */
157  const CMatrixFloat &cov22,
158  const CVectorFloat &mean,
159  const float &stdCount,
160  const std::string &style = std::string("b"),
161  const size_t &nEllipsePoints = 30 );
162 
163  /** Generates a string with the MATLAB commands required to plot an confidence interval (ellipse) for a 2D Gaussian ('double' version).
164  * \param cov22 The 2x2 covariance matrix
165  * \param mean The 2-length vector with the mean
166  * \param stdCount How many "quantiles" to get into the area of the ellipse: 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 CMatrixDouble &cov22,
173  const CVectorDouble &mean,
174  const float &stdCount,
175  const std::string &style = std::string("b"),
176  const size_t &nEllipsePoints = 30 );
177 
178 
179 
180  /** Assignment operator for initializing a std::vector from a C array (The vector will be automatically set to the correct size).
181  * \code
182  * CVectorDouble v;
183  * const double numbers[] = { 1,2,3,5,6,7,8,9,10 };
184  * loadVector( v, numbers );
185  * \endcode
186  * \note This operator performs the appropiate type castings, if required.
187  */
188  template <typename EIGEN_VECTOR, typename At, size_t N>
189  EIGEN_VECTOR& loadVector(EIGEN_VECTOR &v, At (&theArray)[N] )
190  {
192  v.derived().resize(N);
193  for (size_t i=0; i < N; i++)
194  (v.derived())[i] = static_cast<typename EIGEN_VECTOR::Scalar>(theArray[i]);
195  return v;
196  }
197  //! \overload
198  template <typename T, typename At, size_t N>
199  std::vector<T>& loadVector( std::vector<T> &v, At (&theArray)[N] )
200  {
202  v.resize(N);
203  for (size_t i=0; i < N; i++)
204  v[i] = static_cast<T>(theArray[i]);
205  return v;
206  }
207 
208  /** A versatile template to build vectors on-the-fly in a style close to MATLAB's v=[a b c d ...]
209  * The first argument of the template is the vector length, and the second the type of the numbers.
210  * Some examples:
211  *
212  * \code
213  * std::vector<double> = make_vector<4,double>(1.0,3.0,4.0,5.0);
214  * std::vector<float> = make_vector<2,float>(-8.12, 3e4);
215  * \endcode
216  */
217  template <size_t N, typename T>
218  std::vector<T> make_vector(const T val1, ...)
219  {
221  std::vector<T> ret;
222  ret.reserve(N);
223 
224  ret.push_back(val1);
225 
226  va_list args;
227  va_start(args,val1);
228  for (size_t i=1;i<N;i++)
229  ret.push_back( va_arg(args,T) );
230 
231  va_end(args);
232  return ret;
233  }
234 
235  /** @} */ // end of grouping container_ops_grp
236 
237 
238 
239 
240  /** \defgroup mrpt_math_io Custom I/O for math containers
241  * \ingroup mrpt_base_grp */
242  /** \addtogroup mrpt_math_io
243  * @{ */
244 
245  /** Saves to a plain-text file the nonzero entries of a Eigen sparse matrix, represented as a vector of triplets.
246  * Output format is one line per entry with the format: "i j val", i:row, j:col, val:value.
247  * \tparam TRIPLET should be Eigen::Triplet<T>
248  */
249  template <class TRIPLET>
250  bool saveEigenSparseTripletsToFile(const std::string &sFile, std::vector<TRIPLET> &tri)
251  {
252 #if defined(_MSC_VER) && (_MSC_VER>=1400) // Use a secure version in Visual Studio 2005+
253  FILE *f;
254  if (0!=::fopen_s(&f,sFile.c_str(),"wt")) f= NULL;
255 #else
256  FILE *f= ::fopen(sFile.c_str(),"wt");
257 #endif
258 
259  if (!f) return false;
260 
261  for (size_t i=0;i<tri.size();i++)
262  fprintf(f,"%u %u %e\n", 1+tri[i].row(), 1+tri[i].col(), tri[i].value() );
263 
264  fclose(f);
265  return true;
266  }
267 
268  /** @} */ // End of mrpt_math_io
269 
270 
271  } // End of MATH namespace
272 
273 } // End of namespace
274 
275 #endif
bool BASE_IMPEXP isFinite(float f) MRPT_NO_THROWS
Returns true if the number is non infinity.
uint64_t BASE_IMPEXP factorial64(unsigned int n)
Computes the factorial of an integer number and returns it as a 64-bit integer number.
void normalize(const VEC1 &v, VEC2 &out_v)
Normalize a vector, such as its norm is the unity.
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...
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)
int BASE_IMPEXP void BASE_IMPEXP fclose(FILE *f)
An OS-independent version of fscanf.
#define MRPT_NO_THROWS
Used after member declarations.
Column vector, like Eigen::MatrixX*, but automatically initialized to zeros since construction...
Definition: eigen_frwds.h:35
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 ...
T round2up(T val)
Round up to the nearest power of two of a given number.
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...
#define MRPT_COMPILE_TIME_ASSERT(f)
T square(const T x)
Inline function for the square of a number.
Definition: bits.h:113
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
bool BASE_IMPEXP isNaN(float f) MRPT_NO_THROWS
Returns true if the number is NaN.
bool BASE_IMPEXP loadVector(utils::CFileStream &f, std::vector< int > &d)
Loads one row of a text file as a numerical std::vector.
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.
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)
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...
int BASE_IMPEXP fprintf(FILE *fil, const char *format,...) MRPT_NO_THROWS MRPT_printf_format_check(2
An OS-independent version of fprintf.
FILE BASE_IMPEXP * fopen(const char *fileName, const char *mode) MRPT_NO_THROWS
An OS-independent version of fopen.
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.13 for MRPT 1.4.0 SVN: at Fri Mar 17 07:27:15 UTC 2017