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
13#include <cstdarg>
14#include <cstdio>
16
17namespace 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. */
41
42 /** Returns true if the number is NaN. */
44
45 /** Returns true if the number is non infinity. */
47
48 /** Returns true if the number is non infinity. */
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
Column vector, like Eigen::MatrixX*, but automatically initialized to zeros since construction.
Definition types_math.h:65
This CStream derived class allow using a file as a read/write binary stream, creating it if the file ...
Definition CFileStream.h:43
EIGEN_STRONG_INLINE double mean() const
Computes the mean of the entire matrix.
bool BASE_IMPEXP isNaN(float f) MRPT_NO_THROWS
Returns true if the number is NaN.
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 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.
std::vector< T > make_vector(const T val1,...)
A versatile template to build vectors on-the-fly in a style close to MATLAB's v=[a b c d ....
void normalize(const VEC1 &v, VEC2 &out_v)
Normalize a vector, such as its norm is the unity.
uint64_t BASE_IMPEXP factorial64(unsigned int n)
Computes the factorial of an integer number and returns it as a 64-bit integer number.
std::vector< T > sequenceStdVec(T first, size_t length)
Generates a sequence of values [first,first+STEP,first+2*STEP,...].
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...
T round2up(T val)
Round up to the nearest power of two of a given number.
bool BASE_IMPEXP isFinite(float f) MRPT_NO_THROWS
Returns true if the number is non infinity.
void BASE_IMPEXP medianFilter(const std::vector< double > &inV, std::vector< double > &outV, const int &winSize, const int &numberOfSigmas=2)
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...
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...
#define MRPT_COMPILE_TIME_ASSERT(f)
#define THROW_EXCEPTION(msg)
#define MRPT_NO_THROWS
Used after member declarations.
Definition mrpt_macros.h:81
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.



Page generated by Doxygen 1.9.7 for MRPT 1.4.0 SVN: at Tue Jun 27 17:12:06 UTC 2023