Main MRPT website > C++ reference
MRPT logo
base/include/mrpt/math/utils.h
Go to the documentation of this file.
1 /* +---------------------------------------------------------------------------+
2  | The Mobile Robot Programming Toolkit (MRPT) |
3  | |
4  | http://www.mrpt.org/ |
5  | |
6  | Copyright (c) 2005-2013, Individual contributors, see AUTHORS file |
7  | Copyright (c) 2005-2013, MAPIR group, University of Malaga |
8  | Copyright (c) 2012-2013, University of Almeria |
9  | All rights reserved. |
10  | |
11  | Redistribution and use in source and binary forms, with or without |
12  | modification, are permitted provided that the following conditions are |
13  | met: |
14  | * Redistributions of source code must retain the above copyright |
15  | notice, this list of conditions and the following disclaimer. |
16  | * Redistributions in binary form must reproduce the above copyright |
17  | notice, this list of conditions and the following disclaimer in the |
18  | documentation and/or other materials provided with the distribution. |
19  | * Neither the name of the copyright holders nor the |
20  | names of its contributors may be used to endorse or promote products |
21  | derived from this software without specific prior written permission.|
22  | |
23  | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS |
24  | 'AS IS' AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED |
25  | TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR|
26  | PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDERS BE LIABLE |
27  | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL|
28  | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR|
29  | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) |
30  | HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, |
31  | STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN |
32  | ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE |
33  | POSSIBILITY OF SUCH DAMAGE. |
34  +---------------------------------------------------------------------------+ */
35 #ifndef MRPT_MATH_H
36 #define MRPT_MATH_H
37 
38 #include <mrpt/utils/utils_defs.h>
41 #include <mrpt/math/CHistogram.h>
42 
43 #include <mrpt/math/ops_vectors.h>
44 #include <mrpt/math/ops_matrices.h>
45 
46 #include <numeric>
47 #include <cmath>
48 
49 /*---------------------------------------------------------------
50  Namespace
51  ---------------------------------------------------------------*/
52 namespace mrpt
53 {
54  /** This base provides a set of functions for maths stuff. \ingroup mrpt_base_grp
55  */
56  namespace math
57  {
58  using namespace mrpt::utils;
59 
60  /** \addtogroup container_ops_grp
61  * @{ */
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<int> &d);
68 
69  /** Loads one row of a text file as a numerical std::vector.
70  * \return false on EOF or invalid format.
71  * The body of the function is implemented in MATH.cpp
72  */
73  bool BASE_IMPEXP loadVector( utils::CFileStream &f, std::vector<double> &d);
74 
75 
76  /** Returns true if the number is NaN. */
77  bool BASE_IMPEXP isNaN(float f) MRPT_NO_THROWS;
78 
79  /** Returns true if the number is NaN. */
80  bool BASE_IMPEXP isNaN(double f) MRPT_NO_THROWS;
81 
82  /** Returns true if the number is non infinity. */
83  bool BASE_IMPEXP isFinite(float f) MRPT_NO_THROWS;
84 
85  /** Returns true if the number is non infinity. */
86  bool BASE_IMPEXP isFinite(double f) MRPT_NO_THROWS;
87 
88  void BASE_IMPEXP medianFilter( const std::vector<double> &inV, std::vector<double> &outV, const int &winSize, const int &numberOfSigmas = 2 );
89 
90 #ifdef HAVE_LONG_DOUBLE
91  /** Returns true if the number is NaN. */
92  bool BASE_IMPEXP isNaN(long double f) MRPT_NO_THROWS;
93 
94  /** Returns true if the number is non infinity. */
95  bool BASE_IMPEXP isFinite(long double f) MRPT_NO_THROWS;
96 #endif
97 
98  /** Generates an equidistant sequence of numbers given the first one, the last one and the desired number of points.
99  \sa sequence */
100  template<typename T,typename VECTOR>
101  void linspace(T first,T last, size_t count, VECTOR &out_vector)
102  {
103  if (count<2)
104  {
105  out_vector.assign(count,last);
106  return;
107  }
108  else
109  {
110  out_vector.resize(count);
111  const T incr = (last-first)/T(count-1);
112  T c = first;
113  for (size_t i=0;i<count;i++,c+=incr)
114  out_vector[i] = c;
115  }
116  }
117 
118  /** Generates an equidistant sequence of numbers given the first one, the last one and the desired number of points.
119  \sa sequence */
120  template<class T>
121  inline Eigen::Matrix<T,Eigen::Dynamic,1> linspace(T first,T last, size_t count)
122  {
123  Eigen::Matrix<T,Eigen::Dynamic,1> ret;
124  mrpt::math::linspace(first,last,count,ret);
125  return ret;
126  }
127 
128  /** Generates a sequence of values [first,first+STEP,first+2*STEP,...] \sa linspace, sequenceStdVec */
129  template<class T,T STEP>
130  inline Eigen::Matrix<T,Eigen::Dynamic,1> sequence(T first,size_t length)
131  {
132  Eigen::Matrix<T,Eigen::Dynamic,1> ret(length);
133  if (!length) return ret;
134  size_t i=0;
135  while (length--) { ret[i++]=first; first+=STEP; }
136  return ret;
137  }
138 
139  /** Generates a sequence of values [first,first+STEP,first+2*STEP,...] \sa linspace, sequence */
140  template<class T,T STEP>
141  inline std::vector<T> sequenceStdVec(T first,size_t length)
142  {
143  std::vector<T> ret(length);
144  if (!length) return ret;
145  size_t i=0;
146  while (length--) { ret[i++]=first; first+=STEP; }
147  return ret;
148  }
149 
150  /** Generates a vector of all ones of the given length. */
151  template<class T> inline Eigen::Matrix<T,Eigen::Dynamic,1> ones(size_t count)
152  {
153  Eigen::Matrix<T,Eigen::Dynamic,1> v(count);
154  v.setOnes();
155  return v;
156  }
157 
158  /** Generates a vector of all zeros of the given length. */
159  template<class T> inline Eigen::Matrix<T,Eigen::Dynamic,1> zeros(size_t count)
160  {
161  Eigen::Matrix<T,Eigen::Dynamic,1> v(count);
162  v.setZero();
163  return v;
164  }
165 
166 
167  /** Modifies the given angle to translate it into the [0,2pi[ range.
168  * \note Take care of not instancing this template for integer numbers, since it only works for float, double and long double.
169  * \sa wrapToPi, wrapTo2Pi, unwrap2PiSequence
170  */
171  template <class T>
172  inline void wrapTo2PiInPlace(T &a)
173  {
174  bool was_neg = a<0;
175  a = fmod(a, static_cast<T>(M_2PI) );
176  if (was_neg) a+=static_cast<T>(M_2PI);
177  }
178 
179  /** Modifies the given angle to translate it into the [0,2pi[ range.
180  * \note Take care of not instancing this template for integer numbers, since it only works for float, double and long double.
181  * \sa wrapToPi, wrapTo2Pi, unwrap2PiSequence
182  */
183  template <class T>
184  inline T wrapTo2Pi(T a)
185  {
186  wrapTo2PiInPlace(a);
187  return a;
188  }
189 
190  /** Modifies the given angle to translate it into the ]-pi,pi] range.
191  * \note Take care of not instancing this template for integer numbers, since it only works for float, double and long double.
192  * \sa wrapTo2Pi, wrapToPiInPlace, unwrap2PiSequence
193  */
194  template <class T>
195  inline T wrapToPi(T a)
196  {
197  return wrapTo2Pi( a + static_cast<T>(M_PI) )-static_cast<T>(M_PI);
198  }
199 
200  /** Modifies the given angle to translate it into the ]-pi,pi] range.
201  * \note Take care of not instancing this template for integer numbers, since it only works for float, double and long double.
202  * \sa wrapToPi,wrapTo2Pi, unwrap2PiSequence
203  */
204  template <class T>
205  inline void wrapToPiInPlace(T &a)
206  {
207  a = wrapToPi(a);
208  }
209 
210 
211  /** Normalize a vector, such as its norm is the unity.
212  * If the vector has a null norm, the output is a null vector.
213  */
214  template<class VEC1,class VEC2>
215  void normalize(const VEC1 &v, VEC2 &out_v)
216  {
217  typename VEC1::Scalar total=0;
218  const size_t N = v.size();
219  for (size_t i=0;i<N;i++)
220  total += square(v[i]);
221  total = std::sqrt(total);
222  if (total)
223  {
224  out_v = v * (1.0/total);
225  }
226  else out_v.assign(v.size(),0);
227  }
228 
229  /** Computes covariances and mean of any vector of containers, given optional weights for the different samples.
230  * \param elements Any kind of vector of vectors/arrays, eg. std::vector<vector_double>, with all the input samples, each sample in a "row".
231  * \param covariances Output estimated covariance; it can be a fixed/dynamic matrix or a matrixview.
232  * \param means Output estimated mean; it can be vector_double/CArrayDouble, etc...
233  * \param weights_mean If !=NULL, it must point to a vector of size()==number of elements, with normalized weights to take into account for the mean.
234  * \param weights_cov If !=NULL, it must point to a vector of size()==number of elements, with normalized weights to take into account for the covariance.
235  * \param elem_do_wrap2pi If !=NULL; it must point to an array of "bool" of size()==dimension of each element, stating if it's needed to do a wrap to [-pi,pi] to each dimension.
236  * \sa This method is used in mrpt::math::unscented_transform_gaussian
237  * \ingroup stats_grp
238  */
239  template<class VECTOR_OF_VECTORS, class MATRIXLIKE,class VECTORLIKE,class VECTORLIKE2,class VECTORLIKE3>
240  inline void covariancesAndMeanWeighted( // Done inline to speed-up the special case expanded in covariancesAndMean() below.
241  const VECTOR_OF_VECTORS &elements,
242  MATRIXLIKE &covariances,
243  VECTORLIKE &means,
244  const VECTORLIKE2 *weights_mean,
245  const VECTORLIKE3 *weights_cov,
246  const bool *elem_do_wrap2pi = NULL
247  )
248  {
249  ASSERTMSG_(elements.size()!=0,"No samples provided, so there is no way to deduce the output size.")
250  typedef typename MATRIXLIKE::Scalar T;
251  const size_t DIM = elements[0].size();
252  means.resize(DIM);
253  covariances.setSize(DIM,DIM);
254  const size_t nElms=elements.size();
255  const T NORM=1.0/nElms;
256  if (weights_mean) { ASSERTDEB_(size_t(weights_mean->size())==size_t(nElms)) }
257  // The mean goes first:
258  for (size_t i=0;i<DIM;i++)
259  {
260  T accum = 0;
261  if (!elem_do_wrap2pi || !elem_do_wrap2pi[i])
262  { // i'th dimension is a "normal", real number:
263  if (weights_mean)
264  {
265  for (size_t j=0;j<nElms;j++)
266  accum+= (*weights_mean)[j] * elements[j][i];
267  }
268  else
269  {
270  for (size_t j=0;j<nElms;j++) accum+=elements[j][i];
271  accum*=NORM;
272  }
273  }
274  else
275  { // i'th dimension is a circle in [-pi,pi]: we need a little trick here:
276  double accum_L=0,accum_R=0;
277  double Waccum_L=0,Waccum_R=0;
278  for (size_t j=0;j<nElms;j++)
279  {
280  double ang = elements[j][i];
281  const double w = weights_mean!=NULL ? (*weights_mean)[j] : NORM;
282  if (fabs( ang )>0.5*M_PI)
283  { // LEFT HALF: 0,2pi
284  if (ang<0) ang = (M_2PI + ang);
285  accum_L += ang * w;
286  Waccum_L += w;
287  }
288  else
289  { // RIGHT HALF: -pi,pi
290  accum_R += ang * w;
291  Waccum_R += w;
292  }
293  }
294  if (Waccum_L>0) accum_L /= Waccum_L; // [0,2pi]
295  if (Waccum_R>0) accum_R /= Waccum_R; // [-pi,pi]
296  if (accum_L>M_PI) accum_L -= M_2PI; // Left side to [-pi,pi] again:
297  accum = (accum_L* Waccum_L + accum_R * Waccum_R ); // The overall result:
298  }
299  means[i]=accum;
300  }
301  // Now the covariance:
302  for (size_t i=0;i<DIM;i++)
303  for (size_t j=0;j<=i;j++) // Only 1/2 of the matrix
304  {
305  typename MATRIXLIKE::Scalar elem=0;
306  if (weights_cov)
307  {
308  ASSERTDEB_(size_t(weights_cov->size())==size_t(nElms))
309  for (size_t k=0;k<nElms;k++)
310  {
311  const T Ai = (elements[k][i]-means[i]);
312  const T Aj = (elements[k][j]-means[j]);
313  if (!elem_do_wrap2pi || !elem_do_wrap2pi[i])
314  elem+= (*weights_cov)[k] * Ai * Aj;
315  else elem+= (*weights_cov)[k] * mrpt::math::wrapToPi(Ai) * mrpt::math::wrapToPi(Aj);
316  }
317  }
318  else
319  {
320  for (size_t k=0;k<nElms;k++)
321  {
322  const T Ai = (elements[k][i]-means[i]);
323  const T Aj = (elements[k][j]-means[j]);
324  if (!elem_do_wrap2pi || !elem_do_wrap2pi[i])
325  elem+= Ai * Aj;
326  else elem+= mrpt::math::wrapToPi(Ai) * mrpt::math::wrapToPi(Aj);
327  }
328  elem*=NORM;
329  }
330  covariances.get_unsafe(i,j) = elem;
331  if (i!=j) covariances.get_unsafe(j,i)=elem;
332  }
333  }
334 
335  /** Computes covariances and mean of any vector of containers.
336  * \param elements Any kind of vector of vectors/arrays, eg. std::vector<vector_double>, with all the input samples, each sample in a "row".
337  * \param covariances Output estimated covariance; it can be a fixed/dynamic matrix or a matrixview.
338  * \param means Output estimated mean; it can be vector_double/CArrayDouble, etc...
339  * \param elem_do_wrap2pi If !=NULL; it must point to an array of "bool" of size()==dimension of each element, stating if it's needed to do a wrap to [-pi,pi] to each dimension.
340  * \ingroup stats_grp
341  */
342  template<class VECTOR_OF_VECTORS, class MATRIXLIKE,class VECTORLIKE>
343  void covariancesAndMean(const VECTOR_OF_VECTORS &elements,MATRIXLIKE &covariances,VECTORLIKE &means, const bool *elem_do_wrap2pi = NULL)
344  { // The function below is inline-expanded here:
345  covariancesAndMeanWeighted<VECTOR_OF_VECTORS,MATRIXLIKE,VECTORLIKE,vector_double,vector_double>(elements,covariances,means,NULL,NULL,elem_do_wrap2pi);
346  }
347 
348 
349  /** Computes the weighted histogram for a vector of values and their corresponding weights.
350  * \param values [IN] The N values
351  * \param weights [IN] The weights for the corresponding N values (don't need to be normalized)
352  * \param binWidth [IN] The desired width of the bins
353  * \param out_binCenters [OUT] The centers of the M bins generated to cover from the minimum to the maximum value of "values" with the given "binWidth"
354  * \param out_binValues [OUT] The ratio of values at each given bin, such as the whole vector sums up the unity.
355  * \sa weightedHistogramLog
356  */
357  template<class VECTORLIKE1,class VECTORLIKE2>
359  const VECTORLIKE1 &values,
360  const VECTORLIKE1 &weights,
361  float binWidth,
362  VECTORLIKE2 &out_binCenters,
363  VECTORLIKE2 &out_binValues )
364  {
365  MRPT_START
366  typedef typename VECTORLIKE1::Scalar TNum;
367 
368  ASSERT_( values.size() == weights.size() );
369  ASSERT_( binWidth > 0 );
370  TNum minBin = minimum( values );
371  unsigned int nBins = static_cast<unsigned>(ceil((maximum( values )-minBin) / binWidth));
372 
373  // Generate bin center and border values:
374  out_binCenters.resize(nBins);
375  out_binValues.clear(); out_binValues.resize(nBins,0);
376  TNum halfBin = TNum(0.5)*binWidth;;
377  VECTORLIKE2 binBorders(nBins+1,minBin-halfBin);
378  for (unsigned int i=0;i<nBins;i++)
379  {
380  binBorders[i+1] = binBorders[i]+binWidth;
381  out_binCenters[i] = binBorders[i]+halfBin;
382  }
383 
384  // Compute the histogram:
385  TNum totalSum = 0;
386  typename VECTORLIKE1::const_iterator itVal, itW;
387  for (itVal = values.begin(), itW = weights.begin(); itVal!=values.end(); ++itVal, ++itW )
388  {
389  int idx = round(((*itVal)-minBin)/binWidth);
390  if (idx>=int(nBins)) idx=nBins-1;
391  ASSERTDEB_(idx>=0);
392  out_binValues[idx] += *itW;
393  totalSum+= *itW;
394  }
395 
396  if (totalSum)
397  out_binValues /= totalSum;
398 
399 
400  MRPT_END
401  }
402 
403  /** Computes the weighted histogram for a vector of values and their corresponding log-weights.
404  * \param values [IN] The N values
405  * \param weights [IN] The log-weights for the corresponding N values (don't need to be normalized)
406  * \param binWidth [IN] The desired width of the bins
407  * \param out_binCenters [OUT] The centers of the M bins generated to cover from the minimum to the maximum value of "values" with the given "binWidth"
408  * \param out_binValues [OUT] The ratio of values at each given bin, such as the whole vector sums up the unity.
409  * \sa weightedHistogram
410  */
411  template<class VECTORLIKE1,class VECTORLIKE2>
413  const VECTORLIKE1 &values,
414  const VECTORLIKE1 &log_weights,
415  float binWidth,
416  VECTORLIKE2 &out_binCenters,
417  VECTORLIKE2 &out_binValues )
418  {
419  MRPT_START
420  typedef typename VECTORLIKE1::Scalar TNum;
421 
422  ASSERT_( values.size() == log_weights.size() );
423  ASSERT_( binWidth > 0 );
424  TNum minBin = minimum( values );
425  unsigned int nBins = static_cast<unsigned>(ceil((maximum( values )-minBin) / binWidth));
426 
427  // Generate bin center and border values:
428  out_binCenters.resize(nBins);
429  out_binValues.clear(); out_binValues.resize(nBins,0);
430  TNum halfBin = TNum(0.5)*binWidth;;
431  VECTORLIKE2 binBorders(nBins+1,minBin-halfBin);
432  for (unsigned int i=0;i<nBins;i++)
433  {
434  binBorders[i+1] = binBorders[i]+binWidth;
435  out_binCenters[i] = binBorders[i]+halfBin;
436  }
437 
438  // Compute the histogram:
439  const TNum max_log_weight = maximum(log_weights);
440  TNum totalSum = 0;
441  typename VECTORLIKE1::const_iterator itVal, itW;
442  for (itVal = values.begin(), itW = log_weights.begin(); itVal!=values.end(); ++itVal, ++itW )
443  {
444  int idx = round(((*itVal)-minBin)/binWidth);
445  if (idx>=int(nBins)) idx=nBins-1;
446  ASSERTDEB_(idx>=0);
447  const TNum w = exp(*itW-max_log_weight);
448  out_binValues[idx] += w;
449  totalSum+= w;
450  }
451 
452  if (totalSum)
453  out_binValues /= totalSum;
454 
455  MRPT_END
456  }
457 
458 
459  /** Extract a column from a vector of vectors, and store it in another vector.
460  * - Input data can be: std::vector<vector_double>, std::deque<std::list<double> >, std::list<CArrayDouble<5> >, etc. etc.
461  * - Output is the sequence: data[0][idx],data[1][idx],data[2][idx], etc..
462  *
463  * 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".
464  */
465  template <class VECTOR_OF_VECTORS, class VECTORLIKE>
466  inline void extractColumnFromVectorOfVectors(const size_t colIndex, const VECTOR_OF_VECTORS &data, VECTORLIKE &out_column)
467  {
468  const size_t N = data.size();
469  out_column.resize(N);
470  for (size_t i=0;i<N;i++)
471  out_column[i]=data[i][colIndex];
472  }
473 
474  /** Computes the factorial of an integer number and returns it as a 64-bit integer number.
475  */
476  uint64_t BASE_IMPEXP factorial64(unsigned int n);
477 
478  /** Computes the factorial of an integer number and returns it as a double value (internally it uses logarithms for avoiding overflow).
479  */
480  double BASE_IMPEXP factorial(unsigned int n);
481 
482  /** Round up to the nearest power of two of a given number
483  */
484  template <class T>
485  T round2up(T val)
486  {
487  T n = 1;
488  while (n < val)
489  {
490  n <<= 1;
491  if (n<=1)
492  THROW_EXCEPTION("Overflow!");
493  }
494  return n;
495  }
496 
497  /** Round a decimal number up to the given 10'th power (eg, to 1000,100,10, and also fractions)
498  * power10 means round up to: 1 -> 10, 2 -> 100, 3 -> 1000, ... -1 -> 0.1, -2 -> 0.01, ...
499  */
500  template <class T>
501  T round_10power(T val, int power10)
502  {
503  long double F = ::pow((long double)10.0,-(long double)power10);
504  long int t = round_long( val * F );
505  return T(t/F);
506  }
507 
508  /** Calculate the correlation between two matrices
509  * (by AJOGD @ JAN-2007)
510  */
511  template<class T>
513  {
514  if ((a1.getColCount()!=a2.getColCount())|(a1.getRowCount()!=a2.getRowCount()))
515  THROW_EXCEPTION("Correlation Error!, images with no same size");
516 
517  int i,j;
518  T x1,x2;
519  T syy=0, sxy=0, sxx=0, m1=0, m2=0 ,n=a1.getRowCount()*a2.getColCount();
520 
521  //find the means
522  for (i=0;i<a1.getRowCount();i++)
523  {
524  for (j=0;j<a1.getColCount();j++)
525  {
526  m1 += a1(i,j);
527  m2 += a2(i,j);
528  }
529  }
530  m1 /= n;
531  m2 /= n;
532 
533  for (i=0;i<a1.getRowCount();i++)
534  {
535  for (j=0;j<a1.getColCount();j++)
536  {
537  x1 = a1(i,j) - m1;
538  x2 = a2(i,j) - m2;
539  sxx += x1*x1;
540  syy += x2*x2;
541  sxy += x1*x2;
542  }
543  }
544 
545  return sxy / sqrt(sxx * syy);
546  }
547 
548  /** A numerically-stable method to compute average likelihood values with strongly different ranges (unweighted likelihoods: compute the arithmetic mean).
549  * This method implements this equation:
550  *
551  * \f[ return = - \log N + \log \sum_{i=1}^N e^{ll_i-ll_{max}} + ll_{max} \f]
552  *
553  * See also the <a href="http://www.mrpt.org/Averaging_Log-Likelihood_Values:Numerical_Stability">tutorial page</a>.
554  * \ingroup stats_grp
555  */
556  double BASE_IMPEXP averageLogLikelihood( const vector_double &logLikelihoods );
557 
558  /** Computes the average of a sequence of angles in radians taking into account the correct wrapping in the range \f$ ]-\pi,\pi [ \f$, for example, the mean of (2,-2) is \f$ \pi \f$, not 0.
559  * \ingroup stats_grp
560  */
561  double BASE_IMPEXP averageWrap2Pi(const vector_double &angles );
562 
563  /** A numerically-stable method to average likelihood values with strongly different ranges (weighted likelihoods).
564  * This method implements this equation:
565  *
566  * \f[ return = \log \left( \frac{1}{\sum_i e^{lw_i}} \sum_i e^{lw_i} e^{ll_i} \right) \f]
567  *
568  * See also the <a href="http://www.mrpt.org/Averaging_Log-Likelihood_Values:Numerical_Stability">tutorial page</a>.
569  * \ingroup stats_grp
570  */
572  const vector_double &logWeights,
573  const vector_double &logLikelihoods );
574 
575  /** Generates a string with the MATLAB commands required to plot an confidence interval (ellipse) for a 2D Gaussian ('float' version)..
576  * \param cov22 The 2x2 covariance matrix
577  * \param mean The 2-length vector with the mean
578  * \param stdCount How many "quantiles" to get into the area of the ellipse: 2: 95%, 3:99.97%,...
579  * \param style A matlab style string, for colors, line styles,...
580  * \param nEllipsePoints The number of points in the ellipse to generate
581  * \ingroup stats_grp
582  */
584  const CMatrixFloat &cov22,
585  const vector_float &mean,
586  const float &stdCount,
587  const std::string &style = std::string("b"),
588  const size_t &nEllipsePoints = 30 );
589 
590  /** Generates a string with the MATLAB commands required to plot an confidence interval (ellipse) for a 2D Gaussian ('double' version).
591  * \param cov22 The 2x2 covariance matrix
592  * \param mean The 2-length vector with the mean
593  * \param stdCount How many "quantiles" to get into the area of the ellipse: 2: 95%, 3:99.97%,...
594  * \param style A matlab style string, for colors, line styles,...
595  * \param nEllipsePoints The number of points in the ellipse to generate
596  * \ingroup stats_grp
597  */
599  const CMatrixDouble &cov22,
600  const vector_double &mean,
601  const float &stdCount,
602  const std::string &style = std::string("b"),
603  const size_t &nEllipsePoints = 30 );
604 
605 
606  /** Efficiently compute the inverse of a 4x4 homogeneous matrix by only transposing the rotation 3x3 part and solving the translation with dot products.
607  * This is a generic template which works with:
608  * MATRIXLIKE: CMatrixTemplateNumeric, CMatrixFixedNumeric
609  */
610  template <class MATRIXLIKE1,class MATRIXLIKE2>
611  void homogeneousMatrixInverse(const MATRIXLIKE1 &M, MATRIXLIKE2 &out_inverse_M)
612  {
613  MRPT_START
614  ASSERT_( M.isSquare() && size(M,1)==4);
615 
616  /* Instead of performing a generic 4x4 matrix inversion, we only need to
617  transpose the rotation part, then replace the translation part by
618  three dot products. See, for example:
619  https://graphics.stanford.edu/courses/cs248-98-fall/Final/q4.html
620 
621  [ux vx wx tx] -1 [ux uy uz -dot(u,t)]
622  [uy vy wy ty] [vx vy vz -dot(v,t)]
623  [uz vz wz tz] = [wx wy wz -dot(w,t)]
624  [ 0 0 0 1] [ 0 0 0 1 ]
625  */
626 
627  out_inverse_M.setSize(4,4);
628 
629  // 3x3 rotation part:
630  out_inverse_M.set_unsafe(0,0, M.get_unsafe(0,0));
631  out_inverse_M.set_unsafe(0,1, M.get_unsafe(1,0));
632  out_inverse_M.set_unsafe(0,2, M.get_unsafe(2,0));
633 
634  out_inverse_M.set_unsafe(1,0, M.get_unsafe(0,1));
635  out_inverse_M.set_unsafe(1,1, M.get_unsafe(1,1));
636  out_inverse_M.set_unsafe(1,2, M.get_unsafe(2,1));
637 
638  out_inverse_M.set_unsafe(2,0, M.get_unsafe(0,2));
639  out_inverse_M.set_unsafe(2,1, M.get_unsafe(1,2));
640  out_inverse_M.set_unsafe(2,2, M.get_unsafe(2,2));
641 
642  const double tx = -M.get_unsafe(0,3);
643  const double ty = -M.get_unsafe(1,3);
644  const double tz = -M.get_unsafe(2,3);
645 
646  const double tx_ = tx*M.get_unsafe(0,0)+ty*M.get_unsafe(1,0)+tz*M.get_unsafe(2,0);
647  const double ty_ = tx*M.get_unsafe(0,1)+ty*M.get_unsafe(1,1)+tz*M.get_unsafe(2,1);
648  const double tz_ = tx*M.get_unsafe(0,2)+ty*M.get_unsafe(1,2)+tz*M.get_unsafe(2,2);
649 
650  out_inverse_M.set_unsafe(0,3, tx_ );
651  out_inverse_M.set_unsafe(1,3, ty_ );
652  out_inverse_M.set_unsafe(2,3, tz_ );
653 
654  out_inverse_M.set_unsafe(3,0, 0);
655  out_inverse_M.set_unsafe(3,1, 0);
656  out_inverse_M.set_unsafe(3,2, 0);
657  out_inverse_M.set_unsafe(3,3, 1);
658 
659  MRPT_END
660  }
661  //! \overload
662  template <class IN_ROTMATRIX,class IN_XYZ, class OUT_ROTMATRIX, class OUT_XYZ>
664  const IN_ROTMATRIX & in_R,
665  const IN_XYZ & in_xyz,
666  OUT_ROTMATRIX & out_R,
667  OUT_XYZ & out_xyz
668  )
669  {
670  MRPT_START
671  ASSERT_( in_R.isSquare() && size(in_R,1)==3 && in_xyz.size()==3)
672  out_R.setSize(3,3);
673  out_xyz.resize(3);
674 
675  // translation part:
676  typedef typename IN_ROTMATRIX::Scalar T;
677  const T tx = -in_xyz[0];
678  const T ty = -in_xyz[1];
679  const T tz = -in_xyz[2];
680 
681  out_xyz[0] = tx*in_R.get_unsafe(0,0)+ty*in_R.get_unsafe(1,0)+tz*in_R.get_unsafe(2,0);
682  out_xyz[1] = tx*in_R.get_unsafe(0,1)+ty*in_R.get_unsafe(1,1)+tz*in_R.get_unsafe(2,1);
683  out_xyz[2] = tx*in_R.get_unsafe(0,2)+ty*in_R.get_unsafe(1,2)+tz*in_R.get_unsafe(2,2);
684 
685  // 3x3 rotation part: transpose
686  out_R = in_R.adjoint();
687 
688  MRPT_END
689  }
690  //! \overload
691  template <class MATRIXLIKE>
692  inline void homogeneousMatrixInverse(MATRIXLIKE &M)
693  {
694  ASSERTDEB_( M.isSquare() && size(M,1)==4);
695  // translation:
696  const double tx = -M(0,3);
697  const double ty = -M(1,3);
698  const double tz = -M(2,3);
699  M(0,3) = tx*M(0,0)+ty*M(1,0)+tz*M(2,0);
700  M(1,3) = tx*M(0,1)+ty*M(1,1)+tz*M(2,1);
701  M(2,3) = tx*M(0,2)+ty*M(1,2)+tz*M(2,2);
702  // 3x3 rotation part:
703  std::swap( M(1,0),M(0,1) );
704  std::swap( M(2,0),M(0,2) );
705  std::swap( M(1,2),M(2,1) );
706  }
707 
708 
709  /** Estimate the Jacobian of a multi-dimensional function around a point "x", using finite differences of a given size in each input dimension.
710  * The template argument USERPARAM is for the data can be passed to the functor.
711  * If it is not required, set to "int" or any other basic type.
712  *
713  * This is a generic template which works with:
714  * VECTORLIKE: vector_float, vector_double, CArrayNumeric<>, double [N], ...
715  * MATRIXLIKE: CMatrixTemplateNumeric, CMatrixFixedNumeric
716  */
717  template <class VECTORLIKE,class VECTORLIKE2, class VECTORLIKE3, class MATRIXLIKE, class USERPARAM >
719  const VECTORLIKE &x,
720  void (*functor) (const VECTORLIKE &x,const USERPARAM &y, VECTORLIKE3 &out),
721  const VECTORLIKE2 &increments,
722  const USERPARAM &userParam,
723  MATRIXLIKE &out_Jacobian )
724  {
725  MRPT_START
726  ASSERT_(x.size()>0 && increments.size() == x.size());
727 
728  size_t m = 0; // will determine automatically on the first call to "f":
729  const size_t n = x.size();
730 
731  for (size_t j=0;j<n;j++) { ASSERT_( increments[j]>0 ) } // Who knows...
732 
733  VECTORLIKE3 f_minus, f_plus;
734  VECTORLIKE x_mod(x);
735 
736  // Evaluate the function "i" with increments in the "j" input x variable:
737  for (size_t j=0;j<n;j++)
738  {
739  // Create the modified "x" vector:
740  x_mod[j]=x[j]+increments[j];
741  functor(x_mod,userParam, f_plus);
742 
743  x_mod[j]=x[j]-increments[j];
744  functor(x_mod,userParam, f_minus);
745 
746  x_mod[j]=x[j]; // Leave as original
747  const double Ax_2_inv = 0.5/increments[j];
748 
749  // The first time?
750  if (j==0)
751  {
752  m = f_plus.size();
753  out_Jacobian.setSize(m,n);
754  }
755 
756  for (size_t i=0;i<m;i++)
757  out_Jacobian.get_unsafe(i,j) = Ax_2_inv* (f_plus[i]-f_minus[i]);
758 
759  } // end for j
760 
761  MRPT_END
762  }
763 
764  /** Assignment operator for initializing a std::vector from a C array (The vector will be automatically set to the correct size).
765  * \code
766  * vector_double v;
767  * const double numbers[] = { 1,2,3,5,6,7,8,9,10 };
768  * loadVector( v, numbers );
769  * \endcode
770  * \note This operator performs the appropiate type castings, if required.
771  */
772  template <typename T, typename At, size_t N>
773  std::vector<T>& loadVector( std::vector<T> &v, At (&theArray)[N] )
774  {
776  v.resize(N);
777  for (size_t i=0; i < N; i++)
778  v[i] = static_cast<T>(theArray[i]);
779  return v;
780  }
781  //! \overload
782  template <typename Derived, typename At, size_t N>
783  Eigen::EigenBase<Derived>& loadVector( Eigen::EigenBase<Derived> &v, At (&theArray)[N] )
784  {
786  v.derived().resize(N);
787  for (size_t i=0; i < N; i++)
788  (v.derived())[i] = static_cast<typename Derived::Scalar>(theArray[i]);
789  return v;
790  }
791 
792  /** Modify a sequence of angle values such as no consecutive values have a jump larger than PI in absolute value.
793  * \sa wrapToPi
794  */
796 
797  /** A versatile template to build vectors on-the-fly in a style close to MATLAB's v=[a b c d ...]
798  * The first argument of the template is the vector length, and the second the type of the numbers.
799  * Some examples:
800  *
801  * \code
802  * vector_double = make_vector<4,double>(1.0,3.0,4.0,5.0);
803  * vector_float = make_vector<2,float>(-8.12, 3e4);
804  * \endcode
805  */
806  template <size_t N, typename T>
807  std::vector<T> make_vector(const T val1, ...)
808  {
810  std::vector<T> ret;
811  ret.reserve(N);
812 
813  ret.push_back(val1);
814 
815  va_list args;
816  va_start(args,val1);
817  for (size_t i=0;i<N-1;i++)
818  ret.push_back( va_arg(args,T) );
819 
820  va_end(args);
821  return ret;
822  }
823 
824  /** @} */ // end of grouping container_ops_grp
825 
826  /** \addtogroup stats_grp
827  * @{
828  */
829 
830  /** @name Probability density distributions (pdf) distance metrics
831  @{ */
832 
833  /** Computes the squared mahalanobis distance of a vector X given the mean MU and the covariance *inverse* COV_inv
834  * \f[ d^2 = (X-MU)^\top \Sigma^{-1} (X-MU) \f]
835  */
836  template<class VECTORLIKE1,class VECTORLIKE2,class MAT>
837  typename MAT::Scalar mahalanobisDistance2(
838  const VECTORLIKE1 &X,
839  const VECTORLIKE2 &MU,
840  const MAT &COV )
841  {
842  MRPT_START
843  #if defined(_DEBUG) || (MRPT_ALWAYS_CHECKS_DEBUG_MATRICES)
844  ASSERT_( !X.empty() );
845  ASSERT_( X.size()==MU.size() );
846  ASSERT_( X.size()==size(COV,1) && COV.isSquare() );
847  #endif
848  const size_t N = X.size();
850  for (size_t i=0;i<N;i++) X_MU[i]=X[i]-MU[i];
851  return multiply_HCHt_scalar(X_MU, COV.inv() );
852  MRPT_END
853  }
854 
855 
856  /** Computes the mahalanobis distance of a vector X given the mean MU and the covariance *inverse* COV_inv
857  * \f[ d = \sqrt{ (X-MU)^\top \Sigma^{-1} (X-MU) } \f]
858  */
859  template<class VECTORLIKE1,class VECTORLIKE2,class MAT>
860  inline typename VECTORLIKE1::Scalar mahalanobisDistance(
861  const VECTORLIKE1 &X,
862  const VECTORLIKE2 &MU,
863  const MAT &COV )
864  {
865  return std::sqrt( mahalanobisDistance2(X,MU,COV) );
866  }
867 
868 
869  /** Computes the squared mahalanobis distance between two *non-independent* Gaussians, given the two covariance matrices and the vector with the difference of their means.
870  * \f[ d^2 = \Delta_\mu^\top (\Sigma_1 + \Sigma_2 - 2 \Sigma_12 )^{-1} \Delta_\mu \f]
871  */
872  template<class VECTORLIKE,class MAT1,class MAT2,class MAT3>
873  typename MAT1::Scalar
875  const VECTORLIKE &mean_diffs,
876  const MAT1 &COV1,
877  const MAT2 &COV2,
878  const MAT3 &CROSS_COV12 )
879  {
880  MRPT_START
881  #if defined(_DEBUG) || (MRPT_ALWAYS_CHECKS_DEBUG_MATRICES)
882  ASSERT_( !mean_diffs.empty() );
883  ASSERT_( mean_diffs.size()==size(COV1,1));
884  ASSERT_( COV1.isSquare() && COV2.isSquare() );
885  ASSERT_( size(COV1,1)==size(COV2,1));
886  #endif
887  const size_t N = size(COV1,1);
888  MAT1 COV = COV1;
889  COV+=COV2;
890  COV.substract_An(CROSS_COV12,2);
891  MAT1 COV_inv;
892  COV.inv_fast(COV_inv);
893  return multiply_HCHt_scalar(mean_diffs,COV_inv);
894  MRPT_END
895  }
896 
897  /** Computes the mahalanobis distance between two *non-independent* Gaussians (or independent if CROSS_COV12=NULL), given the two covariance matrices and the vector with the difference of their means.
898  * \f[ d = \sqrt{ \Delta_\mu^\top (\Sigma_1 + \Sigma_2 - 2 \Sigma_12 )^{-1} \Delta_\mu } \f]
899  */
900  template<class VECTORLIKE,class MAT1,class MAT2,class MAT3> inline typename VECTORLIKE::Scalar
902  const VECTORLIKE &mean_diffs,
903  const MAT1 &COV1,
904  const MAT2 &COV2,
905  const MAT3 &CROSS_COV12 )
906  {
907  return std::sqrt( mahalanobisDistance( mean_diffs, COV1,COV2,CROSS_COV12 ));
908  }
909 
910  /** Computes the squared mahalanobis distance between a point and a Gaussian, given the covariance matrix and the vector with the difference between the mean and the point.
911  * \f[ d^2 = \Delta_\mu^\top \Sigma^{-1} \Delta_\mu \f]
912  */
913  template<class VECTORLIKE,class MATRIXLIKE>
914  inline typename MATRIXLIKE::Scalar
915  mahalanobisDistance2(const VECTORLIKE &delta_mu,const MATRIXLIKE &cov)
916  {
917  ASSERTDEB_(cov.isSquare())
918  ASSERTDEB_(cov.getColCount()==delta_mu.size())
919  return multiply_HCHt_scalar(delta_mu,cov.inverse());
920  }
921 
922  /** Computes the mahalanobis distance between a point and a Gaussian, given the covariance matrix and the vector with the difference between the mean and the point.
923  * \f[ d^2 = \sqrt( \Delta_\mu^\top \Sigma^{-1} \Delta_\mu ) \f]
924  */
925  template<class VECTORLIKE,class MATRIXLIKE>
926  inline typename MATRIXLIKE::Scalar
927  mahalanobisDistance(const VECTORLIKE &delta_mu,const MATRIXLIKE &cov)
928  {
929  return std::sqrt(mahalanobisDistance2(delta_mu,cov));
930  }
931 
932  /** Computes the integral of the product of two Gaussians, with means separated by "mean_diffs" and covariances "COV1" and "COV2".
933  * \f[ D = \frac{1}{(2 \pi)^{0.5 N} \sqrt{} } \exp( \Delta_\mu^\top (\Sigma_1 + \Sigma_2 - 2 \Sigma_12)^{-1} \Delta_\mu) \f]
934  */
935  template <typename T>
937  const std::vector<T> &mean_diffs,
938  const CMatrixTemplateNumeric<T> &COV1,
939  const CMatrixTemplateNumeric<T> &COV2
940  )
941  {
942  const size_t vector_dim = mean_diffs.size();
943  ASSERT_(vector_dim>=1)
944 
945  CMatrixTemplateNumeric<T> C = COV1;
946  C+= COV2; // Sum of covs:
947  const T cov_det = C.det();
949  C.inv_fast(C_inv);
950 
951  return std::pow( M_2PI, -0.5*vector_dim ) * (1.0/std::sqrt( cov_det ))
952  * exp( -0.5 * mean_diffs.multiply_HCHt_scalar(C_inv) );
953  }
954 
955  /** Computes the integral of the product of two Gaussians, with means separated by "mean_diffs" and covariances "COV1" and "COV2".
956  * \f[ D = \frac{1}{(2 \pi)^{0.5 N} \sqrt{} } \exp( \Delta_\mu^\top (\Sigma_1 + \Sigma_2)^{-1} \Delta_\mu) \f]
957  */
958  template <typename T, size_t DIM>
960  const std::vector<T> &mean_diffs,
961  const CMatrixFixedNumeric<T,DIM,DIM> &COV1,
963  )
964  {
965  ASSERT_(mean_diffs.size()==DIM);
966 
968  C+= COV2; // Sum of covs:
969  const T cov_det = C.det();
971  C.inv_fast(C_inv);
972 
973  return std::pow( M_2PI, -0.5*DIM ) * (1.0/std::sqrt( cov_det ))
974  * exp( -0.5 * mean_diffs.multiply_HCHt_scalar(C_inv) );
975  }
976 
977  /** Computes both, the integral of the product of two Gaussians and their square Mahalanobis distance.
978  * \sa productIntegralTwoGaussians, mahalanobisDistance2
979  */
980  template <typename T, class VECLIKE,class MATLIKE1, class MATLIKE2>
982  const VECLIKE &mean_diffs,
983  const MATLIKE1 &COV1,
984  const MATLIKE2 &COV2,
985  T &maha2_out,
986  T &intprod_out,
987  const MATLIKE1 *CROSS_COV12=NULL
988  )
989  {
990  const size_t vector_dim = mean_diffs.size();
991  ASSERT_(vector_dim>=1)
992 
993  MATLIKE1 C = COV1;
994  C+= COV2; // Sum of covs:
995  if (CROSS_COV12) { C-=*CROSS_COV12; C-=*CROSS_COV12; }
996  const T cov_det = C.det();
997  MATLIKE1 C_inv;
998  C.inv_fast(C_inv);
999 
1000  maha2_out = mean_diffs.multiply_HCHt_scalar(C_inv);
1001  intprod_out = std::pow( M_2PI, -0.5*vector_dim ) * (1.0/std::sqrt( cov_det ))*exp(-0.5*maha2_out);
1002  }
1003 
1004  /** Computes both, the logarithm of the PDF and the square Mahalanobis distance between a point (given by its difference wrt the mean) and a Gaussian.
1005  * \sa productIntegralTwoGaussians, mahalanobisDistance2, normalPDF, mahalanobisDistance2AndPDF
1006  */
1007  template <typename T, class VECLIKE,class MATRIXLIKE>
1009  const VECLIKE &diff_mean,
1010  const MATRIXLIKE &cov,
1011  T &maha2_out,
1012  T &log_pdf_out)
1013  {
1014  MRPT_START
1015  ASSERTDEB_(cov.isSquare())
1016  ASSERTDEB_(size_t(cov.getColCount())==size_t(diff_mean.size()))
1017  MATRIXLIKE C_inv;
1018  cov.inv(C_inv);
1019  maha2_out = multiply_HCHt_scalar(diff_mean,C_inv);
1020  log_pdf_out = static_cast<typename MATRIXLIKE::Scalar>(-0.5)* (
1021  maha2_out+
1022  static_cast<typename MATRIXLIKE::Scalar>(cov.getColCount())*::log(static_cast<typename MATRIXLIKE::Scalar>(M_2PI))+
1023  ::log(cov.det())
1024  );
1025  MRPT_END
1026  }
1027 
1028  /** Computes both, the PDF and the square Mahalanobis distance between a point (given by its difference wrt the mean) and a Gaussian.
1029  * \sa productIntegralTwoGaussians, mahalanobisDistance2, normalPDF
1030  */
1031  template <typename T, class VECLIKE,class MATRIXLIKE>
1033  const VECLIKE &diff_mean,
1034  const MATRIXLIKE &cov,
1035  T &maha2_out,
1036  T &pdf_out)
1037  {
1038  mahalanobisDistance2AndLogPDF(diff_mean,cov,maha2_out,pdf_out);
1039  pdf_out = std::exp(pdf_out); // log to linear
1040  }
1041 
1042  /** @} */
1043  /** @} */ // end of grouping statistics
1044 
1045  /** @addtogroup interpolation_grp Interpolation, least-squares fit, splines
1046  * \ingroup mrpt_base_grp
1047  * @{ */
1048 
1049  /** Interpolate a data sequence "ys" ranging from "x0" to "x1" (equally spaced), to obtain the approximation of the sequence at the point "x".
1050  * If the point "x" is out of the range [x0,x1], the closest extreme "ys" value is returned.
1051  * \sa spline, interpolate2points
1052  */
1053  template <class T,class VECTOR>
1055  const T &x,
1056  const VECTOR &ys,
1057  const T &x0,
1058  const T &x1 )
1059  {
1060  MRPT_START
1061  ASSERT_(x1>x0); ASSERT_(!ys.empty());
1062  const size_t N = ys.size();
1063  if (x<=x0) return ys[0];
1064  if (x>=x1) return ys[N-1];
1065  const T Ax = (x1-x0)/T(N);
1066  const size_t i = int( (x-x0)/Ax );
1067  if (i>=N-1) return ys[N-1];
1068  const T Ay = ys[i+1]-ys[i];
1069  return ys[i] + (x-(x0+i*Ax))*Ay/Ax;
1070  MRPT_END
1071  }
1072 
1073  /** Linear interpolation/extrapolation: evaluates at "x" the line (x0,y0)-(x1,y1).
1074  * If wrap2pi is true, output is wrapped to ]-pi,pi] (It is assumed that input "y" values already are in the correct range).
1075  * \sa spline, interpolate, leastSquareLinearFit
1076  */
1077  double BASE_IMPEXP interpolate2points(const double x, const double x0, const double y0, const double x1, const double y1, bool wrap2pi = false);
1078 
1079  /** Interpolates the value of a function in a point "t" given 4 SORTED points where "t" is between the two middle points
1080  * If wrap2pi is true, output "y" values are wrapped to ]-pi,pi] (It is assumed that input "y" values already are in the correct range).
1081  * \sa leastSquareLinearFit
1082  */
1083  double BASE_IMPEXP spline(const double t, const vector_double &x, const vector_double &y, bool wrap2pi = false);
1084 
1085  /** Interpolates or extrapolates using a least-square linear fit of the set of values "x" and "y", evaluated at a single point "t".
1086  * The vectors x and y must have size >=2, and all values of "x" must be different.
1087  * If wrap2pi is true, output "y" values are wrapped to ]-pi,pi] (It is assumed that input "y" values already are in the correct range).
1088  * \sa spline
1089  * \sa getRegressionLine, getRegressionPlane
1090  */
1091  template <typename NUMTYPE,class VECTORLIKE>
1092  NUMTYPE leastSquareLinearFit(const NUMTYPE t, const VECTORLIKE &x, const VECTORLIKE &y, bool wrap2pi = false)
1093  {
1094  MRPT_START
1095 
1096  // http://en.wikipedia.org/wiki/Linear_least_squares
1097  ASSERT_(x.size()==y.size());
1098  ASSERT_(x.size()>1);
1099 
1100  const size_t N = x.size();
1101 
1102  typedef typename VECTORLIKE::Scalar NUM;
1103 
1104  // X= [1 columns of ones, x' ]
1105  const NUM x_min = x.minimum();
1107  for (size_t i=0;i<N;i++)
1108  {
1109  Xt.set_unsafe(0,i, 1);
1110  Xt.set_unsafe(1,i, x[i]-x_min);
1111  }
1112 
1114  XtX.multiply_AAt(Xt);
1115 
1117  XtX.inv_fast(XtXinv);
1118 
1119  CMatrixTemplateNumeric<NUM> XtXinvXt; // B = inv(X' * X)*X' (pseudoinverse)
1120  XtXinvXt.multiply(XtXinv,Xt);
1121 
1122  VECTORLIKE B;
1123  XtXinvXt.multiply_Ab(y,B);
1124 
1125  ASSERT_(B.size()==2)
1126 
1127  NUM ret = B[0] + B[1]*(t-x_min);
1128 
1129  // wrap?
1130  if (!wrap2pi)
1131  return ret;
1132  else return mrpt::math::wrapToPi(ret);
1133 
1134  MRPT_END
1135  }
1136 
1137  /** Interpolates or extrapolates using a least-square linear fit of the set of values "x" and "y", evaluated at a sequence of points "ts" and returned at "outs".
1138  * If wrap2pi is true, output "y" values are wrapped to ]-pi,pi] (It is assumed that input "y" values already are in the correct range).
1139  * \sa spline, getRegressionLine, getRegressionPlane
1140  */
1141  template <class VECTORLIKE1,class VECTORLIKE2,class VECTORLIKE3>
1143  const VECTORLIKE1 &ts,
1144  VECTORLIKE2 &outs,
1145  const VECTORLIKE3 &x,
1146  const VECTORLIKE3 &y,
1147  bool wrap2pi = false)
1148  {
1149  MRPT_START
1150 
1151  // http://en.wikipedia.org/wiki/Linear_least_squares
1152  ASSERT_(x.size()==y.size());
1153  ASSERT_(x.size()>1);
1154 
1155  const size_t N = x.size();
1156 
1157  // X= [1 columns of ones, x' ]
1158  typedef typename VECTORLIKE3::Scalar NUM;
1159  const NUM x_min = x.minimum();
1161  for (size_t i=0;i<N;i++)
1162  {
1163  Xt.set_unsafe(0,i, 1);
1164  Xt.set_unsafe(1,i, x[i]-x_min);
1165  }
1166 
1168  XtX.multiply_AAt(Xt);
1169 
1171  XtX.inv_fast(XtXinv);
1172 
1173  CMatrixTemplateNumeric<NUM> XtXinvXt; // B = inv(X' * X)*X' (pseudoinverse)
1174  XtXinvXt.multiply(XtXinv,Xt);
1175 
1176  VECTORLIKE3 B;
1177  XtXinvXt.multiply_Ab(y,B);
1178 
1179  ASSERT_(B.size()==2)
1180 
1181  const size_t tsN = size_t(ts.size());
1182  outs.resize(tsN);
1183  if (!wrap2pi)
1184  for (size_t k=0;k<tsN;k++)
1185  outs[k] = B[0] + B[1]*(ts[k]-x_min);
1186  else
1187  for (size_t k=0;k<tsN;k++)
1188  outs[k] = mrpt::math::wrapToPi( B[0] + B[1]*(ts[k]-x_min) );
1189  MRPT_END
1190  }
1191 
1192  /** @} */ // end grouping interpolation_grp
1193 
1194 
1195  /** \defgroup mrpt_math_io Custom I/O for math containers
1196  * \ingroup mrpt_base_grp */
1197  /** \addtogroup mrpt_math_io
1198  * @{ */
1199 
1200  /** Saves to a plain-text file the nonzero entries of a Eigen sparse matrix, represented as a vector of triplets.
1201  * Output format is one line per entry with the format: "i j val", i:row, j:col, val:value.
1202  * \tparam TRIPLET should be Eigen::Triplet<T>
1203  */
1204  template <class TRIPLET>
1205  bool saveEigenSparseTripletsToFile(const std::string &sFile, std::vector<TRIPLET> &tri)
1206  {
1207 #if defined(_MSC_VER) && (_MSC_VER>=1400) // Use a secure version in Visual Studio 2005+
1208  FILE *f;
1209  if (0!=::fopen_s(&f,sFile.c_str(),"wt")) f= NULL;
1210 #else
1211  FILE *f= ::fopen(sFile.c_str(),"wt");
1212 #endif
1213 
1214  if (!f) return false;
1215 
1216  for (size_t i=0;i<tri.size();i++)
1217  fprintf(f,"%u %u %e\n", 1+tri[i].row(), 1+tri[i].col(), tri[i].value() );
1218 
1219  fclose(f);
1220  return true;
1221  }
1222 
1223  /** @} */ // End of mrpt_math_io
1224 
1225 
1226  } // End of MATH namespace
1227 
1228 } // End of namespace
1229 
1230 #endif
void mahalanobisDistance2AndPDF(const VECLIKE &diff_mean, const MATRIXLIKE &cov, T &maha2_out, T &pdf_out)
Computes both, the PDF and the square Mahalanobis distance between a point (given by its difference w...
bool BASE_IMPEXP isFinite(float f) MRPT_NO_THROWS
Returns true if the number is non infinity.
double BASE_IMPEXP spline(const double t, const vector_double &x, const vector_double &y, bool wrap2pi=false)
Interpolates the value of a function in a point "t" given 4 SORTED points where "t" is between the tw...
Classes for serialization, sockets, ini-file manipulation, streams, list of properties-values, timewatch, extensions to STL.
Definition: zip.h:42
Eigen::Matrix< T, Eigen::Dynamic, 1 > zeros(size_t count)
Generates a vector of all zeros of the given length.
void unwrap2PiSequence(vector_double &x)
Modify a sequence of angle values such as no consecutive values have a jump larger than PI in absolut...
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.
int round(const T value)
Returns the closer integer (int) to x.
Definition: bits.h:126
The base class of MRPT vectors, actually, Eigen column matrices of dynamic size with specialized cons...
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.
T interpolate(const T &x, const VECTOR &ys, const T &x0, const T &x1)
Interpolate a data sequence "ys" ranging from "x0" to "x1" (equally spaced), to obtain the approximat...
#define MRPT_NO_THROWS
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 ...
This file implements miscelaneous matrix and matrix/vector operations, plus internal functions in mrp...
EIGEN_STRONG_INLINE const AdjointReturnType t() const
Transpose.
MAT_C::Scalar multiply_HCHt_scalar(const VECTOR_H &H, const MAT_C &C)
r (a scalar) = H * C * H^t (with a vector H and a symmetric matrix C)
Definition: ops_matrices.h:167
Eigen::Matrix< T, Eigen::Dynamic, 1 > sequence(T first, size_t length)
Generates a sequence of values [first,first+STEP,first+2*STEP,...].
double BASE_IMPEXP averageWrap2Pi(const vector_double &angles)
Computes the average of a sequence of angles in radians taking into account the correct wrapping in t...
const Scalar * const_iterator
Definition: eigen_plugins.h:50
void homogeneousMatrixInverse(const MATRIXLIKE1 &M, MATRIXLIKE2 &out_inverse_M)
Efficiently compute the inverse of a 4x4 homogeneous matrix by only transposing the rotation 3x3 part...
#define M_2PI
Definition: mrpt_macros.h:412
long round_long(const T value)
Returns the closer integer (long) to x.
Definition: bits.h:148
NUMTYPE leastSquareLinearFit(const NUMTYPE t, const VECTORLIKE &x, const VECTORLIKE &y, bool wrap2pi=false)
Interpolates or extrapolates using a least-square linear fit of the set of values "x" and "y"...
CONTAINER::Scalar minimum(const CONTAINER &v)
void wrapToPiInPlace(T &a)
Modifies the given angle to translate it into the ]-pi,pi] range.
A numeric matrix of compile-time fixed size.
T round2up(T val)
Round up to the nearest power of two of a given number.
The base class of MRPT vectors, actually, Eigen column matrices of dynamic size with specialized cons...
void mahalanobisDistance2AndLogPDF(const VECLIKE &diff_mean, const MATRIXLIKE &cov, T &maha2_out, T &log_pdf_out)
Computes both, the logarithm of the PDF and the square Mahalanobis distance between a point (given by...
#define MRPT_END
Eigen::Matrix< T, Eigen::Dynamic, 1 > ones(size_t count)
Generates a vector of all ones of the given length.
Eigen::Matrix< typename MATRIX::Scalar, MATRIX::ColsAtCompileTime, MATRIX::ColsAtCompileTime > cov(const MATRIX &v)
Computes the covariance matrix from a list of samples in an NxM matrix, where each row is a sample...
Definition: ops_matrices.h:240
void productIntegralAndMahalanobisTwoGaussians(const VECLIKE &mean_diffs, const MATLIKE1 &COV1, const MATLIKE2 &COV2, T &maha2_out, T &intprod_out, const MATLIKE1 *CROSS_COV12=NULL)
Computes both, the integral of the product of two Gaussians and their square Mahalanobis distance...
T wrapTo2Pi(T a)
Modifies the given angle to translate it into the [0,2pi[ range.
double correlate_matrix(const CMatrixTemplateNumeric< T > &a1, const CMatrixTemplateNumeric< T > &a2)
Calculate the correlation between two matrices (by AJOGD @ JAN-2007)
CONTAINER::Scalar maximum(const CONTAINER &v)
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 wrapToPi(T a)
Modifies the given angle to translate it into the ]-pi,pi] range.
MAT::Scalar mahalanobisDistance2(const VECTORLIKE1 &X, const VECTORLIKE2 &MU, const MAT &COV)
Computes the squared mahalanobis distance of a vector X given the mean MU and the covariance inverse ...
void estimateJacobian(const VECTORLIKE &x, void(*functor)(const VECTORLIKE &x, const USERPARAM &y, VECTORLIKE3 &out), const VECTORLIKE2 &increments, const USERPARAM &userParam, MATRIXLIKE &out_Jacobian)
Estimate the Jacobian of a multi-dimensional function around a point "x", using finite differences of...
void wrapTo2PiInPlace(T &a)
Modifies the given angle to translate it into the [0,2pi[ range.
void weightedHistogram(const VECTORLIKE1 &values, const VECTORLIKE1 &weights, float binWidth, VECTORLIKE2 &out_binCenters, VECTORLIKE2 &out_binValues)
Computes the weighted histogram for a vector of values and their corresponding weights.
#define MRPT_COMPILE_TIME_ASSERT(f)
#define MRPT_START
T square(const T x)
Inline function for the square of a number.
Definition: bits.h:184
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
#define ASSERTDEB_(f)
Defines an assertion mechanism - only when compiled in debug.
bool BASE_IMPEXP isNaN(float f) MRPT_NO_THROWS
Returns true if the number is NaN.
double BASE_IMPEXP interpolate2points(const double x, const double x0, const double y0, const double x1, const double y1, bool wrap2pi=false)
Linear interpolation/extrapolation: evaluates at "x" the line (x0,y0)-(x1,y1).
bool BASE_IMPEXP loadVector(utils::CFileStream &f, std::vector< int > &d)
Loads one row of a text file as a numerical std::vector.
size_t size(const MATRIXLIKE &m, int dim)
Definition: bits.h:72
VECTORLIKE1::Scalar mahalanobisDistance(const VECTORLIKE1 &X, const VECTORLIKE2 &MU, const MAT &COV)
Computes the mahalanobis distance of a vector X given the mean MU and the covariance inverse COV_inv ...
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.
void covariancesAndMeanWeighted(const VECTOR_OF_VECTORS &elements, MATRIXLIKE &covariances, VECTORLIKE &means, const VECTORLIKE2 *weights_mean, const VECTORLIKE3 *weights_cov, const bool *elem_do_wrap2pi=NULL)
Computes covariances and mean of any vector of containers, given optional weights for the different s...
T productIntegralTwoGaussians(const std::vector< T > &mean_diffs, const CMatrixTemplateNumeric< T > &COV1, const CMatrixTemplateNumeric< T > &COV2)
Computes the integral of the product of two Gaussians, with means separated by "mean_diffs" and covar...
double mean(const CONTAINER &v)
Computes the mean value of a vector.
void weightedHistogramLog(const VECTORLIKE1 &values, const VECTORLIKE1 &log_weights, float binWidth, VECTORLIKE2 &out_binCenters, VECTORLIKE2 &out_binValues)
Computes the weighted histogram for a vector of values and their corresponding log-weights.
#define ASSERT_(f)
void BASE_IMPEXP medianFilter(const std::vector< double > &inV, std::vector< double > &outV, const int &winSize, const int &numberOfSigmas=2)
T round_10power(T val, int power10)
Round a decimal number up to the given 10&#39;th power (eg, to 1000,100,10, and also fractions) power10 m...
CMatrixTemplateNumeric< double > CMatrixDouble
Declares a matrix of double numbers (non serializable).
void covariancesAndMean(const VECTOR_OF_VECTORS &elements, MATRIXLIKE &covariances, VECTORLIKE &means, const bool *elem_do_wrap2pi=NULL)
Computes covariances and mean of any vector of containers.
double BASE_IMPEXP averageLogLikelihood(const vector_double &logLikelihoods)
A numerically-stable method to compute average likelihood values with strongly different ranges (unwe...
int BASE_IMPEXP fprintf(FILE *fil, const char *format,...) MRPT_NO_THROWS MRPT_printf_format_check(2
An OS-independent version of fprintf.
#define ASSERTMSG_(f, __ERROR_MSG)
#define M_PI
Definition: mrpt_macros.h:408
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,...].
CMatrixTemplateNumeric< float > CMatrixFloat
Declares a matrix of float numbers (non serializable).
std::string BASE_IMPEXP MATLAB_plotCovariance2D(const CMatrixFloat &cov22, const vector_float &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...



Page generated by Doxygen 1.8.14 for MRPT 1.0.2 SVN: at lun oct 28 00:52:41 CET 2019 Hosted on:
SourceForge.net Logo