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


Page generated by Doxygen 1.8.3 for MRPT 0.9.6 SVN: at Fri Feb 15 22:05:02 EST 2013