Main MRPT website > C++ reference
MRPT logo

utils.h

Go to the documentation of this file.
00001 /* +---------------------------------------------------------------------------+
00002    |          The Mobile Robot Programming Toolkit (MRPT) C++ library          |
00003    |                                                                           |
00004    |                   http://mrpt.sourceforge.net/                            |
00005    |                                                                           |
00006    |   Copyright (C) 2005-2011  University of Malaga                           |
00007    |                                                                           |
00008    |    This software was written by the Machine Perception and Intelligent    |
00009    |      Robotics Lab, University of Malaga (Spain).                          |
00010    |    Contact: Jose-Luis Blanco  <jlblanco@ctima.uma.es>                     |
00011    |                                                                           |
00012    |  This file is part of the MRPT project.                                   |
00013    |                                                                           |
00014    |     MRPT is free software: you can redistribute it and/or modify          |
00015    |     it under the terms of the GNU General Public License as published by  |
00016    |     the Free Software Foundation, either version 3 of the License, or     |
00017    |     (at your option) any later version.                                   |
00018    |                                                                           |
00019    |   MRPT is distributed in the hope that it will be useful,                 |
00020    |     but WITHOUT ANY WARRANTY; without even the implied warranty of        |
00021    |     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         |
00022    |     GNU General Public License for more details.                          |
00023    |                                                                           |
00024    |     You should have received a copy of the GNU General Public License     |
00025    |     along with MRPT.  If not, see <http://www.gnu.org/licenses/>.         |
00026    |                                                                           |
00027    +---------------------------------------------------------------------------+ */
00028 #ifndef  MRPT_MATH_H
00029 #define  MRPT_MATH_H
00030 
00031 #include <mrpt/utils/utils_defs.h>
00032 #include <mrpt/math/CMatrixTemplateNumeric.h>
00033 #include <mrpt/math/CMatrixFixedNumeric.h>
00034 #include <mrpt/math/CHistogram.h>
00035 
00036 #include <mrpt/math/ops_vectors.h>
00037 #include <mrpt/math/ops_matrices.h>
00038 
00039 #include <numeric>
00040 #include <cmath>
00041 
00042 /*---------------------------------------------------------------
00043                 Namespace
00044   ---------------------------------------------------------------*/
00045 namespace mrpt
00046 {
00047         /** This base provides a set of functions for maths stuff.
00048          */
00049         namespace math
00050         {
00051             using namespace mrpt::utils;
00052 
00053                 /** Loads one row of a text file as a numerical std::vector.
00054                   * \return false on EOF or invalid format.
00055                   * The body of the function is implemented in MATH.cpp
00056                         */
00057                 bool BASE_IMPEXP loadVector( utils::CFileStream &f, std::vector<int> &d);
00058 
00059                 /** Loads one row of a text file as a numerical std::vector.
00060                   * \return false on EOF or invalid format.
00061                   * The body of the function is implemented in MATH.cpp
00062                         */
00063                 bool BASE_IMPEXP loadVector( utils::CFileStream &f, std::vector<double> &d);
00064 
00065 
00066         /** Returns true if the number is NaN. */
00067         bool BASE_IMPEXP  isNaN(float  f) MRPT_NO_THROWS;
00068 
00069         /** Returns true if the number is NaN. */
00070         bool  BASE_IMPEXP isNaN(double f) MRPT_NO_THROWS;
00071 
00072         /** Returns true if the number is non infinity. */
00073         bool BASE_IMPEXP  isFinite(float f) MRPT_NO_THROWS;
00074 
00075         /** Returns true if the number is non infinity.  */
00076         bool  BASE_IMPEXP isFinite(double f) MRPT_NO_THROWS;
00077 
00078         void BASE_IMPEXP medianFilter( const std::vector<double> &inV, std::vector<double> &outV, const int &winSize, const int &numberOfSigmas = 2 );
00079 
00080 #ifdef HAVE_LONG_DOUBLE
00081                 /** Returns true if the number is NaN. */
00082         bool  BASE_IMPEXP isNaN(long double f) MRPT_NO_THROWS;
00083 
00084         /** Returns true if the number is non infinity. */
00085         bool BASE_IMPEXP  isFinite(long double f) MRPT_NO_THROWS;
00086 #endif
00087 
00088                 /** Generates an equidistant sequence of numbers given the first one, the last one and the desired number of points.
00089                   \sa sequence */
00090                 template<typename T,typename VECTOR>
00091                 void linspace(T first,T last, size_t count, VECTOR &out_vector)
00092                 {
00093                         if (count<2)
00094                         {
00095                                 out_vector.assign(count,last);
00096                                 return;
00097                         }
00098                         else
00099                         {
00100                                 out_vector.resize(count);
00101                                 const T incr = (last-first)/T(count-1);
00102                                 T c = first;
00103                                 for (size_t i=0;i<count;i++,c+=incr)
00104                                         out_vector[i] = static_cast<typename VECTOR::value_type>(c);
00105                         }
00106                 }
00107 
00108                 /** Generates an equidistant sequence of numbers given the first one, the last one and the desired number of points.
00109                   \sa sequence */
00110                 template<class T>
00111                 inline Eigen::Matrix<T,Eigen::Dynamic,1> linspace(T first,T last, size_t count)
00112                 {
00113                         Eigen::Matrix<T,Eigen::Dynamic,1> ret;
00114                         mrpt::math::linspace(first,last,count,ret);
00115                         return ret;
00116                 }
00117 
00118                 /** Generates a sequence of values [first,first+STEP,first+2*STEP,...]   \sa linspace, sequenceStdVec */
00119                 template<class T,T STEP>
00120                 inline Eigen::Matrix<T,Eigen::Dynamic,1> sequence(T first,size_t length)
00121                 {
00122                         Eigen::Matrix<T,Eigen::Dynamic,1> ret(length);
00123                         if (!length) return ret;
00124                         size_t i=0;
00125                         while (length--) { ret[i++]=first; first+=STEP; }
00126                         return ret;
00127                 }
00128 
00129                 /** Generates a sequence of values [first,first+STEP,first+2*STEP,...]   \sa linspace, sequence */
00130                 template<class T,T STEP>
00131                 inline std::vector<T> sequenceStdVec(T first,size_t length)
00132                 {
00133                         std::vector<T> ret(length);
00134                         if (!length) return ret;
00135                         size_t i=0;
00136                         while (length--) { ret[i++]=first; first+=STEP; }
00137                         return ret;
00138                 }
00139 
00140                 /** Generates a vector of all ones of the given length. */
00141                 template<class T> inline Eigen::Matrix<T,Eigen::Dynamic,1> ones(size_t count)
00142                 {
00143                         Eigen::Matrix<T,Eigen::Dynamic,1> v(count);
00144                         v.setOnes();
00145                         return v;
00146                 }
00147 
00148                 /** Generates a vector of all zeros of the given length. */
00149                 template<class T> inline Eigen::Matrix<T,Eigen::Dynamic,1> zeros(size_t count)
00150                 {
00151                         Eigen::Matrix<T,Eigen::Dynamic,1> v(count);
00152                         v.setZero();
00153                         return v;
00154                 }
00155 
00156 
00157                 /** Modifies the given angle to translate it into the [0,2pi[ range.
00158                   * \note Take care of not instancing this template for integer numbers, since it only works for float, double and long double.
00159                   * \sa wrapToPi, wrapTo2Pi, unwrap2PiSequence
00160                   */
00161                 template <class T>
00162                 inline void wrapTo2PiInPlace(T &a)
00163                 {
00164                         bool was_neg = a<0;
00165                         a = fmod(a, static_cast<T>(M_2PI) );
00166                         if (was_neg) a+=static_cast<T>(M_2PI);
00167                 }
00168 
00169                 /** Modifies the given angle to translate it into the [0,2pi[ range.
00170                   * \note Take care of not instancing this template for integer numbers, since it only works for float, double and long double.
00171                   * \sa wrapToPi, wrapTo2Pi, unwrap2PiSequence
00172                   */
00173                 template <class T>
00174                 inline T wrapTo2Pi(T a)
00175                 {
00176                         wrapTo2PiInPlace(a);
00177                         return a;
00178                 }
00179 
00180                 /** Modifies the given angle to translate it into the ]-pi,pi] range.
00181                   * \note Take care of not instancing this template for integer numbers, since it only works for float, double and long double.
00182                   * \sa wrapTo2Pi, wrapToPiInPlace, unwrap2PiSequence
00183                   */
00184                 template <class T>
00185                 inline T wrapToPi(T a)
00186                 {
00187                         return wrapTo2Pi( a + static_cast<T>(M_PI) )-static_cast<T>(M_PI);
00188                 }
00189 
00190                 /** Modifies the given angle to translate it into the ]-pi,pi] range.
00191                   * \note Take care of not instancing this template for integer numbers, since it only works for float, double and long double.
00192                   * \sa wrapToPi,wrapTo2Pi, unwrap2PiSequence
00193                   */
00194                 template <class T>
00195                 inline void wrapToPiInPlace(T &a)
00196                 {
00197                         a = wrapToPi(a);
00198                 }
00199 
00200 
00201                 /** Normalize a vector, such as its norm is the unity.
00202                   *  If the vector has a null norm, the output is a null vector.
00203                   */
00204                 template<class VEC1,class VEC2>
00205                 void normalize(const VEC1 &v, VEC2 &out_v)
00206                 {
00207                         typename VEC1::value_type total=0;
00208                         const size_t N = v.size();
00209                         for (size_t i=0;i<N;i++)
00210                                 total += square(v[i]);
00211                         total = std::sqrt(total);
00212                         if (total)
00213                         {
00214                                 out_v = v * (1.0/total);
00215                         }
00216                         else out_v.assign(v.size(),0);
00217                 }
00218 
00219 
00220                 /** Computes covariances and mean of any vector of containers, given optional weights for the different samples.
00221                   * \param elements Any kind of vector of vectors/arrays, eg. std::vector<vector_double>, with all the input samples, each sample in a "row".
00222                   * \param covariances Output estimated covariance; it can be a fixed/dynamic matrix or a matrixview.
00223                   * \param means Output estimated mean; it can be vector_double/CArrayDouble, etc...
00224                   * \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.
00225                   * \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.
00226                   * \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.
00227                   * \sa This method is used in mrpt::math::unscented_transform_gaussian
00228                   */
00229                 template<class VECTOR_OF_VECTORS, class MATRIXLIKE,class VECTORLIKE,class VECTORLIKE2,class VECTORLIKE3>
00230                 inline void covariancesAndMeanWeighted(   // Done inline to speed-up the special case expanded in covariancesAndMean() below.
00231                         const VECTOR_OF_VECTORS &elements,
00232                         MATRIXLIKE &covariances,
00233                         VECTORLIKE &means,
00234                         const VECTORLIKE2 *weights_mean,
00235                         const VECTORLIKE3 *weights_cov,
00236                         const bool *elem_do_wrap2pi = NULL
00237                         )
00238                 {
00239                         ASSERTMSG_(elements.size()!=0,"No samples provided, so there is no way to deduce the output size.")
00240                         typedef typename VECTORLIKE::value_type T;
00241                         const size_t DIM = elements[0].size();
00242                         means.resize(DIM);
00243                         covariances.setSize(DIM,DIM);
00244                         const size_t nElms=elements.size();
00245                         const T NORM=1.0/nElms;
00246                         if (weights_mean) { ASSERTDEB_(size_t(weights_mean->size())==size_t(nElms)) }
00247                         // The mean goes first:
00248                         for (size_t i=0;i<DIM;i++)
00249                         {
00250                                 T  accum = 0;
00251                                 if (!elem_do_wrap2pi || !elem_do_wrap2pi[i])
00252                                 {       // i'th dimension is a "normal", real number:
00253                                         if (weights_mean)
00254                                         {
00255                                                 for (size_t j=0;j<nElms;j++)
00256                                                         accum+= (*weights_mean)[j] * elements[j][i];
00257                                         }
00258                                         else
00259                                         {
00260                                                 for (size_t j=0;j<nElms;j++) accum+=elements[j][i];
00261                                                 accum*=NORM;
00262                                         }
00263                                 }
00264                                 else
00265                                 {       // i'th dimension is a circle in [-pi,pi]: we need a little trick here:
00266                                         double accum_L=0,accum_R=0;
00267                                         double Waccum_L=0,Waccum_R=0;
00268                                         for (size_t j=0;j<nElms;j++)
00269                                         {
00270                                                 double ang = elements[j][i];
00271                                                 const double w   = weights_mean!=NULL ? (*weights_mean)[j] : NORM;
00272                                                 if (fabs( ang )>0.5*M_PI)
00273                                                 {       // LEFT HALF: 0,2pi
00274                                                         if (ang<0) ang = (M_2PI + ang);
00275                                                         accum_L  += ang * w;
00276                                                         Waccum_L += w;
00277                                                 }
00278                                                 else
00279                                                 {       // RIGHT HALF: -pi,pi
00280                                                         accum_R += ang * w;
00281                                                         Waccum_R += w;
00282                                                 }
00283                                         }
00284                                         if (Waccum_L>0) accum_L /= Waccum_L;  // [0,2pi]
00285                                         if (Waccum_R>0) accum_R /= Waccum_R;  // [-pi,pi]
00286                                         if (accum_L>M_PI) accum_L -= M_2PI;     // Left side to [-pi,pi] again:
00287                                         accum = (accum_L* Waccum_L + accum_R * Waccum_R );      // The overall result:
00288                                 }
00289                                 means[i]=accum;
00290                         }
00291                         // Now the covariance:
00292                         for (size_t i=0;i<DIM;i++)
00293                                 for (size_t j=0;j<=i;j++)       // Only 1/2 of the matrix
00294                                 {
00295                                         typename MATRIXLIKE::value_type elem=0;
00296                                         if (weights_cov)
00297                                         {
00298                                                 ASSERTDEB_(size_t(weights_cov->size())==size_t(nElms))
00299                                                 for (size_t k=0;k<nElms;k++)
00300                                                 {
00301                                                         const T Ai = (elements[k][i]-means[i]);
00302                                                         const T Aj = (elements[k][j]-means[j]);
00303                                                         if (!elem_do_wrap2pi || !elem_do_wrap2pi[i])
00304                                                                         elem+= (*weights_cov)[k] * Ai * Aj;
00305                                                         else    elem+= (*weights_cov)[k] * mrpt::math::wrapToPi(Ai) * mrpt::math::wrapToPi(Aj);
00306                                                 }
00307                                         }
00308                                         else
00309                                         {
00310                                                 for (size_t k=0;k<nElms;k++)
00311                                                 {
00312                                                         const T Ai = (elements[k][i]-means[i]);
00313                                                         const T Aj = (elements[k][j]-means[j]);
00314                                                         if (!elem_do_wrap2pi || !elem_do_wrap2pi[i])
00315                                                                         elem+= Ai * Aj;
00316                                                         else    elem+= mrpt::math::wrapToPi(Ai) * mrpt::math::wrapToPi(Aj);
00317                                                 }
00318                                                 elem*=NORM;
00319                                         }
00320                                         covariances.get_unsafe(i,j) = elem;
00321                                         if (i!=j) covariances.get_unsafe(j,i)=elem;
00322                                 }
00323                 }
00324 
00325                 /** Computes covariances and mean of any vector of containers.
00326                   * \param elements Any kind of vector of vectors/arrays, eg. std::vector<vector_double>, with all the input samples, each sample in a "row".
00327                   * \param covariances Output estimated covariance; it can be a fixed/dynamic matrix or a matrixview.
00328                   * \param means Output estimated mean; it can be vector_double/CArrayDouble, etc...
00329                   * \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.
00330                   */
00331                 template<class VECTOR_OF_VECTORS, class MATRIXLIKE,class VECTORLIKE>
00332                 void covariancesAndMean(const VECTOR_OF_VECTORS &elements,MATRIXLIKE &covariances,VECTORLIKE &means, const bool *elem_do_wrap2pi = NULL)
00333                 {   // The function below is inline-expanded here:
00334                         covariancesAndMeanWeighted<VECTOR_OF_VECTORS,MATRIXLIKE,VECTORLIKE,vector_double,vector_double>(elements,covariances,means,NULL,NULL,elem_do_wrap2pi);
00335                 }
00336 
00337 
00338                 /** Computes the weighted histogram for a vector of values and their corresponding weights.
00339                   *  \param values [IN] The N values
00340                   *  \param weights [IN] The weights for the corresponding N values (don't need to be normalized)
00341                   *  \param binWidth [IN] The desired width of the bins
00342                   *  \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"
00343                   *  \param out_binValues [OUT] The ratio of values at each given bin, such as the whole vector sums up the unity.
00344                   *  \sa weightedHistogramLog
00345                   */
00346                 template<class VECTORLIKE1,class VECTORLIKE2>
00347                         void  weightedHistogram(
00348                                 const VECTORLIKE1       &values,
00349                                 const VECTORLIKE1       &weights,
00350                                 float                           binWidth,
00351                                 VECTORLIKE2     &out_binCenters,
00352                                 VECTORLIKE2     &out_binValues )
00353                         {
00354                                 MRPT_START;
00355                                 typedef typename VECTORLIKE1::value_type TNum;
00356 
00357                                 ASSERT_( values.size() == weights.size() );
00358                                 ASSERT_( binWidth > 0 );
00359                                 TNum    minBin = minimum( values );
00360                                 unsigned int    nBins = static_cast<unsigned>(ceil((maximum( values )-minBin) / binWidth));
00361 
00362                                 // Generate bin center and border values:
00363                                 out_binCenters.resize(nBins);
00364                                 out_binValues.clear(); out_binValues.resize(nBins,0);
00365                                 TNum halfBin = TNum(0.5)*binWidth;;
00366                                 VECTORLIKE2   binBorders(nBins+1,minBin-halfBin);
00367                                 for (unsigned int i=0;i<nBins;i++)
00368                                 {
00369                                         binBorders[i+1] = binBorders[i]+binWidth;
00370                                         out_binCenters[i] = binBorders[i]+halfBin;
00371                                 }
00372 
00373                                 // Compute the histogram:
00374                                 TNum totalSum = 0;
00375                                 typename VECTORLIKE1::const_iterator itVal, itW;
00376                                 for (itVal = values.begin(), itW = weights.begin(); itVal!=values.end(); ++itVal, ++itW )
00377                                 {
00378                                         int idx = round(((*itVal)-minBin)/binWidth);
00379                                         if (idx>=int(nBins)) idx=nBins-1;
00380                                         ASSERTDEB_(idx>=0);
00381                                         out_binValues[idx] += *itW;
00382                                         totalSum+= *itW;
00383                                 }
00384 
00385                                 if (totalSum)
00386                                         out_binValues /= totalSum;
00387 
00388 
00389                                 MRPT_END;
00390                         }
00391 
00392                 /** Computes the weighted histogram for a vector of values and their corresponding log-weights.
00393                   *  \param values [IN] The N values
00394                   *  \param weights [IN] The log-weights for the corresponding N values (don't need to be normalized)
00395                   *  \param binWidth [IN] The desired width of the bins
00396                   *  \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"
00397                   *  \param out_binValues [OUT] The ratio of values at each given bin, such as the whole vector sums up the unity.
00398                   *  \sa weightedHistogram
00399                   */
00400                 template<class VECTORLIKE1,class VECTORLIKE2>
00401                         void  weightedHistogramLog(
00402                                 const VECTORLIKE1       &values,
00403                                 const VECTORLIKE1       &log_weights,
00404                                 float                           binWidth,
00405                                 VECTORLIKE2     &out_binCenters,
00406                                 VECTORLIKE2     &out_binValues )
00407                         {
00408                                 MRPT_START;
00409                                 typedef typename VECTORLIKE1::value_type TNum;
00410 
00411                                 ASSERT_( values.size() == log_weights.size() );
00412                                 ASSERT_( binWidth > 0 );
00413                                 TNum    minBin = minimum( values );
00414                                 unsigned int    nBins = static_cast<unsigned>(ceil((maximum( values )-minBin) / binWidth));
00415 
00416                                 // Generate bin center and border values:
00417                                 out_binCenters.resize(nBins);
00418                                 out_binValues.clear(); out_binValues.resize(nBins,0);
00419                                 TNum halfBin = TNum(0.5)*binWidth;;
00420                                 VECTORLIKE2   binBorders(nBins+1,minBin-halfBin);
00421                                 for (unsigned int i=0;i<nBins;i++)
00422                                 {
00423                                         binBorders[i+1] = binBorders[i]+binWidth;
00424                                         out_binCenters[i] = binBorders[i]+halfBin;
00425                                 }
00426 
00427                                 // Compute the histogram:
00428                                 const TNum max_log_weight = maximum(log_weights);
00429                                 TNum totalSum = 0;
00430                                 typename VECTORLIKE1::const_iterator itVal, itW;
00431                                 for (itVal = values.begin(), itW = log_weights.begin(); itVal!=values.end(); ++itVal, ++itW )
00432                                 {
00433                                         int idx = round(((*itVal)-minBin)/binWidth);
00434                                         if (idx>=int(nBins)) idx=nBins-1;
00435                                         ASSERTDEB_(idx>=0);
00436                                         const TNum w = exp(*itW-max_log_weight);
00437                                         out_binValues[idx] += w;
00438                                         totalSum+= w;
00439                                 }
00440 
00441                                 if (totalSum)
00442                                         out_binValues /= totalSum;
00443 
00444                                 MRPT_END;
00445                         }
00446 
00447 
00448                         /** Extract a column from a vector of vectors, and store it in another vector.
00449                           *  - Input data can be: std::vector<vector_double>, std::deque<std::list<double> >, std::list<CArrayDouble<5> >, etc. etc.
00450                           *  - Output is the sequence:  data[0][idx],data[1][idx],data[2][idx], etc..
00451                           *
00452                           *  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".
00453                           */
00454                         template <class VECTOR_OF_VECTORS, class VECTORLIKE>
00455                         inline void extractColumnFromVectorOfVectors(const size_t colIndex, const VECTOR_OF_VECTORS &data, VECTORLIKE &out_column)
00456                         {
00457                                 const size_t N = data.size();
00458                                 out_column.resize(N);
00459                                 for (size_t i=0;i<N;i++)
00460                                         out_column[i]=data[i][colIndex];
00461                         }
00462 
00463                 /** Computes the factorial of an integer number and returns it as a 64-bit integer number.
00464                   */
00465                 uint64_t BASE_IMPEXP  factorial64(unsigned int n);
00466 
00467                 /** Computes the factorial of an integer number and returns it as a double value (internally it uses logarithms for avoiding overflow).
00468                   */
00469                 double BASE_IMPEXP  factorial(unsigned int n);
00470 
00471                 /** Round up to the nearest power of two of a given number
00472                   */
00473                 template <class T>
00474                 T round2up(T val)
00475                 {
00476                         T n = 1;
00477                         while (n < val)
00478                         {
00479                                 n <<= 1;
00480                                 if (n<=1)
00481                                         THROW_EXCEPTION("Overflow!");
00482                         }
00483                         return n;
00484                 }
00485 
00486                 /** Round a decimal number up to the given 10'th power (eg, to 1000,100,10, and also fractions)
00487                   *  power10 means round up to: 1 -> 10, 2 -> 100, 3 -> 1000, ...  -1 -> 0.1, -2 -> 0.01, ...
00488                   */
00489                 template <class T>
00490                 T round_10power(T val, int power10)
00491                 {
00492                         long double F = ::pow((long double)10.0,-(long double)power10);
00493                         long int t = round_long( val * F );
00494                         return T(t/F);
00495                 }
00496 
00497                 /** Calculate the correlation between two matrices
00498                   *  (by AJOGD @ JAN-2007)
00499                   */
00500                 template<class T>
00501                 double  correlate_matrix(const CMatrixTemplateNumeric<T> &a1, const CMatrixTemplateNumeric<T> &a2)
00502                 {
00503                         if ((a1.getColCount()!=a2.getColCount())|(a1.getRowCount()!=a2.getRowCount()))
00504                                 THROW_EXCEPTION("Correlation Error!, images with no same size");
00505 
00506                         int i,j;
00507                         T x1,x2;
00508                         T syy=0, sxy=0, sxx=0, m1=0, m2=0 ,n=a1.getRowCount()*a2.getColCount();
00509 
00510                         //find the means
00511                         for (i=0;i<a1.getRowCount();i++)
00512                         {
00513                                 for (j=0;j<a1.getColCount();j++)
00514                                 {
00515                                         m1 += a1(i,j);
00516                                         m2 += a2(i,j);
00517                                 }
00518                         }
00519                         m1 /= n;
00520                         m2 /= n;
00521 
00522                         for (i=0;i<a1.getRowCount();i++)
00523                         {
00524                                 for (j=0;j<a1.getColCount();j++)
00525                                 {
00526                                         x1 = a1(i,j) - m1;
00527                                         x2 = a2(i,j) - m2;
00528                                         sxx += x1*x1;
00529                                         syy += x2*x2;
00530                                         sxy += x1*x2;
00531                                 }
00532                         }
00533 
00534                         return sxy / sqrt(sxx * syy);
00535                 }
00536 
00537 
00538 //              /**     Matrix QR decomposition. A = QR, where R is upper triangular and Q is orthogonal, that is, ~QQ = 1
00539 //                * If A is a LxM dimension matrix, this function only return the LxL upper triangular matrix R instead of LxM pseudo-upper
00540 //                * triangular matrix (been L<=M)
00541 //                * This function has been extracted from "Numerical Recipes in C".
00542 //                *     /param A is the original matrix to decompose
00543 //                * /param c,Q. The orthogonal matrix Q is represented as a product of n-1 Householder matrices Q1,...Qn-1, where Qj = 1 - u[j] x u[j]/c[j]
00544 //                *                             The i'th component of u[j] is zero for i = 1,...,j-1 while the nonzero components are returned in Q(i,j) for i=j,...,n
00545 //                *     /param R is the upper triangular matrix
00546 //                *     /param sign returns as true (1) is singularity is encountered during the decomposition, but the decomposition is still complete
00547 //                *                             in this case; otherwise it returns false (0)
00548 //                */
00549 //
00550 //                      template<class T>
00551 //                      void BASE_IMPEXP qr_decomposition(
00552 //                              CMatrixTemplateNumeric<T>       &A,
00553 //                              CMatrixTemplateNumeric<T>       &R,
00554 //                              CMatrixTemplateNumeric<T>       &Q,
00555 //                              CVectorTemplate<T>                      &c,
00556 //                              int                                                             &sing);
00557 //
00558 
00559 //              /**If R = CHOL(A) is the original Cholesky factorization of A, then R1 = CHOLUPDATE(R,X) returns the upper triangular
00560 //                * Cholesky factor of A + X*X', where X is a column vector of appropriate length.
00561 //                */
00562 //              template<class T>
00563 //                      void BASE_IMPEXP UpdateCholesky(
00564 //                              CMatrixTemplateNumeric<T>       &chol,
00565 //                              CVectorTemplate<T>                      &r1Modification);
00566 
00567 //              /** Compute the two eigenvalues of a 2x2 matrix.
00568 //                * \param in_matrx The 2x2 input matrix.
00569 //                * \param min_eigenvalue (out) The minimum eigenvalue of the matrix.
00570 //                * \param max_eigenvalue (out) The maximum eigenvalue of the matrix.
00571 //                * by FAMD, MAR-2007
00572 //                */
00573 //              void BASE_IMPEXP  computeEigenValues2x2(
00574 //                      const CMatrixFloat      &in_matrix,
00575 //                      float                                                           &min_eigenvalue,
00576 //                      float                                                           &max_eigenvalue );
00577 
00578                 /** A numerically-stable method to compute average likelihood values with strongly different ranges (unweighted likelihoods: compute the arithmetic mean).
00579                   *  This method implements this equation:
00580                   *
00581                   *  \f[ return = - \log N + \log  \sum_{i=1}^N e^{ll_i-ll_{max}} + ll_{max} \f]
00582                   *
00583                   * See also the <a href="http://www.mrpt.org/Averaging_Log-Likelihood_Values:Numerical_Stability">tutorial page</a>.
00584                   */
00585                 double BASE_IMPEXP averageLogLikelihood( const vector_double &logLikelihoods );
00586 
00587                 /** 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.
00588                   */
00589                 double BASE_IMPEXP averageWrap2Pi(const vector_double &angles );
00590 
00591                 /** A numerically-stable method to average likelihood values with strongly different ranges (weighted likelihoods).
00592                   *  This method implements this equation:
00593                   *
00594                   *  \f[ return = \log \left( \frac{1}{\sum_i e^{lw_i}} \sum_i  e^{lw_i} e^{ll_i}  \right) \f]
00595                   *
00596                   * See also the <a href="http://www.mrpt.org/Averaging_Log-Likelihood_Values:Numerical_Stability">tutorial page</a>.
00597                   */
00598                 double BASE_IMPEXP  averageLogLikelihood(
00599                         const vector_double &logWeights,
00600                         const vector_double &logLikelihoods );
00601 
00602                 /** Generates a string with the MATLAB commands required to plot an confidence interval (ellipse) for a 2D Gaussian ('float' version)..
00603                   *  \param cov22 The 2x2 covariance matrix
00604                   *  \param mean  The 2-length vector with the mean
00605                   *  \param stdCount How many "quantiles" to get into the area of the ellipse: 2: 95%, 3:99.97%,...
00606                   *  \param style A matlab style string, for colors, line styles,...
00607                   *  \param nEllipsePoints The number of points in the ellipse to generate
00608                   */
00609                 std::string BASE_IMPEXP  MATLAB_plotCovariance2D(
00610                         const CMatrixFloat  &cov22,
00611                         const vector_float  &mean,
00612                         const float         &stdCount,
00613                         const std::string   &style = std::string("b"),
00614                         const size_t        &nEllipsePoints = 30 );
00615 
00616                 /** Generates a string with the MATLAB commands required to plot an confidence interval (ellipse) for a 2D Gaussian ('double' version).
00617                   *  \param cov22 The 2x2 covariance matrix
00618                   *  \param mean  The 2-length vector with the mean
00619                   *  \param stdCount How many "quantiles" to get into the area of the ellipse: 2: 95%, 3:99.97%,...
00620                   *  \param style A matlab style string, for colors, line styles,...
00621                   *  \param nEllipsePoints The number of points in the ellipse to generate
00622                   */
00623                 std::string BASE_IMPEXP  MATLAB_plotCovariance2D(
00624                         const CMatrixDouble  &cov22,
00625                         const vector_double  &mean,
00626                         const float         &stdCount,
00627                         const std::string   &style = std::string("b"),
00628                         const size_t        &nEllipsePoints = 30 );
00629 
00630 
00631                 /** Efficiently compute the inverse of a 4x4 homogeneous matrix by only transposing the rotation 3x3 part and solving the translation with dot products.
00632                   *  This is a generic template which works with:
00633                   *    MATRIXLIKE: CMatrixTemplateNumeric, CMatrixFixedNumeric
00634                   */
00635                 template <class MATRIXLIKE1,class MATRIXLIKE2>
00636                 void homogeneousMatrixInverse(const MATRIXLIKE1 &M, MATRIXLIKE2 &out_inverse_M)
00637                 {
00638                         MRPT_START;
00639                         ASSERT_( M.isSquare() && size(M,1)==4);
00640 
00641                         /* Instead of performing a generic 4x4 matrix inversion, we only need to
00642                           transpose the rotation part, then replace the translation part by
00643                           three dot products. See, for example:
00644                          https://graphics.stanford.edu/courses/cs248-98-fall/Final/q4.html
00645 
00646                                 [ux vx wx tx] -1   [ux uy uz -dot(u,t)]
00647                                 [uy vy wy ty]      [vx vy vz -dot(v,t)]
00648                                 [uz vz wz tz]    = [wx wy wz -dot(w,t)]
00649                                 [ 0  0  0  1]      [ 0  0  0     1    ]
00650                         */
00651 
00652                         out_inverse_M.setSize(4,4);
00653 
00654                         // 3x3 rotation part:
00655                         out_inverse_M.set_unsafe(0,0, M.get_unsafe(0,0));
00656                         out_inverse_M.set_unsafe(0,1, M.get_unsafe(1,0));
00657                         out_inverse_M.set_unsafe(0,2, M.get_unsafe(2,0));
00658 
00659                         out_inverse_M.set_unsafe(1,0, M.get_unsafe(0,1));
00660                         out_inverse_M.set_unsafe(1,1, M.get_unsafe(1,1));
00661                         out_inverse_M.set_unsafe(1,2, M.get_unsafe(2,1));
00662 
00663                         out_inverse_M.set_unsafe(2,0, M.get_unsafe(0,2));
00664                         out_inverse_M.set_unsafe(2,1, M.get_unsafe(1,2));
00665                         out_inverse_M.set_unsafe(2,2, M.get_unsafe(2,2));
00666 
00667                         const double tx = -M.get_unsafe(0,3);
00668                         const double ty = -M.get_unsafe(1,3);
00669                         const double tz = -M.get_unsafe(2,3);
00670 
00671                         const double tx_ = tx*M.get_unsafe(0,0)+ty*M.get_unsafe(1,0)+tz*M.get_unsafe(2,0);
00672                         const double ty_ = tx*M.get_unsafe(0,1)+ty*M.get_unsafe(1,1)+tz*M.get_unsafe(2,1);
00673                         const double tz_ = tx*M.get_unsafe(0,2)+ty*M.get_unsafe(1,2)+tz*M.get_unsafe(2,2);
00674 
00675                         out_inverse_M.set_unsafe(0,3, tx_ );
00676                         out_inverse_M.set_unsafe(1,3, ty_ );
00677                         out_inverse_M.set_unsafe(2,3, tz_ );
00678 
00679                         out_inverse_M.set_unsafe(3,0,  0);
00680                         out_inverse_M.set_unsafe(3,1,  0);
00681                         out_inverse_M.set_unsafe(3,2,  0);
00682                         out_inverse_M.set_unsafe(3,3,  1);
00683 
00684                         MRPT_END;
00685                 }
00686                 //! \overload
00687                 template <class IN_ROTMATRIX,class IN_XYZ, class OUT_ROTMATRIX, class OUT_XYZ>
00688                 void homogeneousMatrixInverse(
00689                         const IN_ROTMATRIX  & in_R,
00690                         const IN_XYZ        & in_xyz,
00691                         OUT_ROTMATRIX & out_R,
00692                         OUT_XYZ       & out_xyz
00693                         )
00694                 {
00695                         MRPT_START
00696                         ASSERT_( in_R.isSquare() && size(in_R,1)==3 && in_xyz.size()==3)
00697                         out_R.setSize(3,3);
00698                         out_xyz.resize(3);
00699 
00700                         // translation part:
00701                         const double tx = -in_xyz[0];
00702                         const double ty = -in_xyz[1];
00703                         const double tz = -in_xyz[2];
00704 
00705                         out_xyz[0] = tx*in_R.get_unsafe(0,0)+ty*in_R.get_unsafe(1,0)+tz*in_R.get_unsafe(2,0);
00706                         out_xyz[1] = tx*in_R.get_unsafe(0,1)+ty*in_R.get_unsafe(1,1)+tz*in_R.get_unsafe(2,1);
00707                         out_xyz[2] = tx*in_R.get_unsafe(0,2)+ty*in_R.get_unsafe(1,2)+tz*in_R.get_unsafe(2,2);
00708 
00709                         // 3x3 rotation part: transpose
00710                         out_R = in_R.adjoint();
00711 
00712                         MRPT_END
00713                 }
00714                 //! \overload
00715                 template <class MATRIXLIKE>
00716                 inline void homogeneousMatrixInverse(MATRIXLIKE &M)
00717                 {
00718                         ASSERTDEB_( M.isSquare() && size(M,1)==4);
00719                         // translation:
00720                         const double tx = -M(0,3);
00721                         const double ty = -M(1,3);
00722                         const double tz = -M(2,3);
00723                         M(0,3) = tx*M(0,0)+ty*M(1,0)+tz*M(2,0);
00724                         M(1,3) = tx*M(0,1)+ty*M(1,1)+tz*M(2,1);
00725                         M(2,3) = tx*M(0,2)+ty*M(1,2)+tz*M(2,2);
00726                         // 3x3 rotation part:
00727                         std::swap( M(1,0),M(0,1) );
00728                         std::swap( M(2,0),M(0,2) );
00729                         std::swap( M(1,2),M(2,1) );
00730                 }
00731 
00732 
00733                 /** Estimate the Jacobian of a multi-dimensional function around a point "x", using finite differences of a given size in each input dimension.
00734                   *  The template argument USERPARAM is for the data can be passed to the functor.
00735                   *   If it is not required, set to "int" or any other basic type.
00736                   *
00737                   *  This is a generic template which works with:
00738                   *    VECTORLIKE: vector_float, vector_double, CArrayNumeric<>, double [N], ...
00739                   *    MATRIXLIKE: CMatrixTemplateNumeric, CMatrixFixedNumeric
00740                   */
00741                 template <class VECTORLIKE,class VECTORLIKE2, class VECTORLIKE3, class MATRIXLIKE, class USERPARAM >
00742                 void estimateJacobian(
00743                         const VECTORLIKE        &x,
00744                         void                            (*functor) (const VECTORLIKE &x,const USERPARAM &y, VECTORLIKE3  &out),
00745                         const VECTORLIKE2       &increments,
00746                         const USERPARAM         &userParam,
00747                         MATRIXLIKE                      &out_Jacobian )
00748                 {
00749                         MRPT_START
00750                         ASSERT_(x.size()>0 && increments.size() == x.size());
00751 
00752                         size_t m = 0;           // will determine automatically on the first call to "f":
00753                         const size_t n = x.size();
00754 
00755                         for (size_t j=0;j<n;j++) { ASSERT_( increments[j]>0 ) }         // Who knows...
00756 
00757                         VECTORLIKE3     f_minus, f_plus;
00758                         VECTORLIKE      x_mod(x);
00759 
00760                         // Evaluate the function "i" with increments in the "j" input x variable:
00761                         for (size_t j=0;j<n;j++)
00762                         {
00763                                 // Create the modified "x" vector:
00764                                 x_mod[j]=x[j]+increments[j];
00765                                 functor(x_mod,userParam,   f_plus);
00766 
00767                                 x_mod[j]=x[j]-increments[j];
00768                                 functor(x_mod,userParam,   f_minus);
00769 
00770                                 x_mod[j]=x[j]; // Leave as original
00771                                 const double Ax_2_inv = 0.5/increments[j];
00772 
00773                                 // The first time?
00774                                 if (j==0)
00775                                 {
00776                                         m = f_plus.size();
00777                                         out_Jacobian.setSize(m,n);
00778                                 }
00779 
00780                                 for (size_t i=0;i<m;i++)
00781                                         out_Jacobian.get_unsafe(i,j) = Ax_2_inv* (f_plus[i]-f_minus[i]);
00782 
00783                         } // end for j
00784 
00785                         MRPT_END
00786                 }
00787 
00788                 /** @name Interpolation functions
00789                 @{ */
00790 
00791                 /** Interpolate a data sequence "ys" ranging from "x0" to "x1" (equally spaced), to obtain the approximation of the sequence at the point "x".
00792                   *  If the point "x" is out of the range [x0,x1], the closest extreme "ys" value is returned.
00793                   * \sa spline, interpolate2points
00794                   */
00795                 template <class T,class VECTOR>
00796                 T interpolate(
00797                         const T                 &x,
00798                         const VECTOR    &ys,
00799                         const T                 &x0,
00800                         const T                 &x1 )
00801                 {
00802                         MRPT_START
00803                         ASSERT_(x1>x0); ASSERT_(!ys.empty());
00804                         const size_t N = ys.size();
00805                         if (x<=x0)      return ys[0];
00806                         if (x>=x1)      return ys[N-1];
00807                         const T Ax = (x1-x0)/T(N);
00808                         const size_t i = int( (x-x0)/Ax );
00809                         if (i>=N-1) return ys[N-1];
00810                         const T Ay = ys[i+1]-ys[i];
00811                         return ys[i] + (x-(x0+i*Ax))*Ay/Ax;
00812                         MRPT_END
00813                 }
00814 
00815                 /** Linear interpolation/extrapolation: evaluates at "x" the line (x0,y0)-(x1,y1).
00816                   *  If wrap2pi is true, output is wrapped to ]-pi,pi] (It is assumed that input "y" values already are in the correct range).
00817                   * \sa spline, interpolate, leastSquareLinearFit
00818                   */
00819                 double BASE_IMPEXP interpolate2points(const double x, const double x0, const double y0, const double x1, const double y1, bool wrap2pi = false);
00820 
00821                 /** Interpolates the value of a function in a point "t" given 4 SORTED points where "t" is between the two middle points
00822                   *  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).
00823                   * \sa leastSquareLinearFit
00824                   */
00825                 double BASE_IMPEXP  spline(const double t, const vector_double &x, const vector_double &y, bool wrap2pi = false);
00826 
00827                 /** Interpolates or extrapolates using a least-square linear fit of the set of values "x" and "y", evaluated at a single point "t".
00828                   *  The vectors x and y must have size >=2, and all values of "x" must be different.
00829                   *  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).
00830                   * \sa spline
00831                   * \sa getRegressionLine, getRegressionPlane
00832                   */
00833                 template <typename NUMTYPE,class VECTORLIKE>
00834                 NUMTYPE leastSquareLinearFit(const NUMTYPE t, const VECTORLIKE &x, const VECTORLIKE &y, bool wrap2pi = false)
00835                 {
00836                         MRPT_START
00837 
00838                         // http://en.wikipedia.org/wiki/Linear_least_squares
00839                         ASSERT_(x.size()==y.size());
00840                         ASSERT_(x.size()>1);
00841 
00842                         const size_t N = x.size();
00843 
00844                         typedef typename VECTORLIKE::value_type NUM;
00845 
00846                         // X= [1 columns of ones, x' ]
00847                         const NUM x_min = x.minimum();
00848                         CMatrixTemplateNumeric<NUM> Xt(2,N);
00849                         for (size_t i=0;i<N;i++)
00850                         {
00851                                 Xt.set_unsafe(0,i, 1);
00852                                 Xt.set_unsafe(1,i, x[i]-x_min);
00853                         }
00854 
00855                         CMatrixTemplateNumeric<NUM> XtX;
00856                         XtX.multiply_AAt(Xt);
00857 
00858                         CMatrixTemplateNumeric<NUM> XtXinv;
00859                         XtX.inv_fast(XtXinv);
00860 
00861                         CMatrixTemplateNumeric<NUM> XtXinvXt;   // B = inv(X' * X)*X'  (pseudoinverse)
00862                         XtXinvXt.multiply(XtXinv,Xt);
00863 
00864                         VECTORLIKE B;
00865                         XtXinvXt.multiply_Ab(y,B);
00866 
00867                         ASSERT_(B.size()==2)
00868 
00869                         NUM ret = B[0] + B[1]*(t-x_min);
00870 
00871                         // wrap?
00872                         if (!wrap2pi)
00873                                         return ret;
00874                         else    return mrpt::math::wrapToPi(ret);
00875 
00876                         MRPT_END
00877                 }
00878 
00879                 /** 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".
00880                   *  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).
00881                   * \sa spline, getRegressionLine, getRegressionPlane
00882                   */
00883                 template <class VECTORLIKE1,class VECTORLIKE2,class VECTORLIKE3>
00884                 void leastSquareLinearFit(
00885                         const VECTORLIKE1 &ts,
00886                         VECTORLIKE2 &outs,
00887                         const VECTORLIKE3 &x,
00888                         const VECTORLIKE3 &y,
00889                         bool wrap2pi = false)
00890                 {
00891                         MRPT_START
00892 
00893                         // http://en.wikipedia.org/wiki/Linear_least_squares
00894                         ASSERT_(x.size()==y.size());
00895                         ASSERT_(x.size()>1);
00896 
00897                         const size_t N = x.size();
00898 
00899                         // X= [1 columns of ones, x' ]
00900                         typedef typename VECTORLIKE3::value_type NUM;
00901                         const NUM x_min = x.minimum();
00902                         CMatrixTemplateNumeric<NUM> Xt(2,N);
00903                         for (size_t i=0;i<N;i++)
00904                         {
00905                                 Xt.set_unsafe(0,i, 1);
00906                                 Xt.set_unsafe(1,i, x[i]-x_min);
00907                         }
00908 
00909                         CMatrixTemplateNumeric<NUM> XtX;
00910                         XtX.multiply_AAt(Xt);
00911 
00912                         CMatrixTemplateNumeric<NUM> XtXinv;
00913                         XtX.inv_fast(XtXinv);
00914 
00915                         CMatrixTemplateNumeric<NUM> XtXinvXt;   // B = inv(X' * X)*X' (pseudoinverse)
00916                         XtXinvXt.multiply(XtXinv,Xt);
00917 
00918                         VECTORLIKE3 B;
00919                         XtXinvXt.multiply_Ab(y,B);
00920 
00921                         ASSERT_(B.size()==2)
00922 
00923                         const size_t tsN = size_t(ts.size());
00924                         outs.resize(tsN);
00925                         if (!wrap2pi)
00926                                 for (size_t k=0;k<tsN;k++)
00927                                         outs[k] = B[0] + B[1]*(ts[k]-x_min);
00928                         else
00929                                 for (size_t k=0;k<tsN;k++)
00930                                         outs[k] = mrpt::math::wrapToPi( B[0] + B[1]*(ts[k]-x_min) );
00931                         MRPT_END
00932                 }
00933 
00934                 /** @} */
00935 
00936 
00937                 /** Assignment operator for initializing a std::vector from a C array (The vector will be automatically set to the correct size).
00938                   * \code
00939                   *      vector_double  v;
00940                   *  const double numbers[] = { 1,2,3,5,6,7,8,9,10 };
00941                   *  loadVector( v, numbers );
00942                   * \endcode
00943                   * \note This operator performs the appropiate type castings, if required.
00944                   */
00945                 template <typename T, typename At, size_t N>
00946                 std::vector<T>& loadVector( std::vector<T> &v, At (&theArray)[N] )
00947                 {
00948                         MRPT_COMPILE_TIME_ASSERT(N!=0)
00949                         v.resize(N);
00950                         for (size_t i=0; i < N; i++)
00951                                 v[i] = static_cast<T>(theArray[i]);
00952                         return v;
00953                 }
00954                 //! \overload
00955                 template <typename Derived, typename At, size_t N>
00956                 Eigen::EigenBase<Derived>& loadVector( Eigen::EigenBase<Derived> &v, At (&theArray)[N] )
00957                 {
00958                         MRPT_COMPILE_TIME_ASSERT(N!=0)
00959                         v.derived().resize(N);
00960                         for (size_t i=0; i < N; i++)
00961                                 (v.derived())[i] = static_cast<typename Derived::Scalar>(theArray[i]);
00962                         return v;
00963                 }
00964 
00965                 /** Modify a sequence of angle values such as no consecutive values have a jump larger than PI in absolute value.
00966                   * \sa wrapToPi
00967                   */
00968                 void unwrap2PiSequence(vector_double &x);
00969 
00970                 /** @name Probability density distributions (pdf) distance metrics
00971                 @{ */
00972 
00973                 /** Computes the squared mahalanobis distance of a vector X given the mean MU and the covariance *inverse* COV_inv
00974                   *  \f[ d^2 =  (X-MU)^\top \Sigma^{-1} (X-MU)  \f]
00975                   */
00976                 template<class VECTORLIKE1,class VECTORLIKE2,class MAT>
00977                 typename VECTORLIKE1::value_type mahalanobisDistance2(
00978                         const VECTORLIKE1 &X,
00979                         const VECTORLIKE2 &MU,
00980                         const MAT &COV )
00981                 {
00982                         MRPT_START
00983                         #if defined(_DEBUG) || (MRPT_ALWAYS_CHECKS_DEBUG_MATRICES)
00984                                 ASSERT_( !X.empty() );
00985                                 ASSERT_( X.size()==MU.size() );
00986                                 ASSERT_( X.size()==size(COV,1) && COV.isSquare() );
00987                         #endif
00988                         const size_t N = X.size();
00989                         mrpt::dynamicsize_vector<typename VECTORLIKE1::value_type> X_MU(N);
00990                         for (size_t i=0;i<N;i++) X_MU[i]=X[i]-MU[i];
00991                         return multiply_HCHt_scalar(X_MU, COV.inv() );
00992                         MRPT_END
00993                 }
00994 
00995 
00996                 /** Computes the mahalanobis distance of a vector X given the mean MU and the covariance *inverse* COV_inv
00997                   *  \f[ d = \sqrt{ (X-MU)^\top \Sigma^{-1} (X-MU) }  \f]
00998                   */
00999                 template<class VECTORLIKE1,class VECTORLIKE2,class MAT>
01000                 inline typename VECTORLIKE1::value_type mahalanobisDistance(
01001                         const VECTORLIKE1 &X,
01002                         const VECTORLIKE2 &MU,
01003                         const MAT &COV )
01004                 {
01005                         return std::sqrt( mahalanobisDistance2(X,MU,COV) );
01006                 }
01007 
01008 
01009                 /** Computes the squared mahalanobis distance between two *non-independent* Gaussians, given the two covariance matrices and the vector with the difference of their means.
01010                   *  \f[ d^2 = \Delta_\mu^\top (\Sigma_1 + \Sigma_2 - 2 \Sigma_12 )^{-1} \Delta_\mu  \f]
01011                   */
01012                 template<class VECTORLIKE,class MAT1,class MAT2,class MAT3>
01013                 typename VECTORLIKE::value_type
01014                 mahalanobisDistance2(
01015                         const VECTORLIKE &mean_diffs,
01016                         const MAT1 &COV1,
01017                         const MAT2 &COV2,
01018                         const MAT3 &CROSS_COV12 )
01019                 {
01020                         MRPT_START
01021                         #if defined(_DEBUG) || (MRPT_ALWAYS_CHECKS_DEBUG_MATRICES)
01022                                 ASSERT_( !mean_diffs.empty() );
01023                                 ASSERT_( mean_diffs.size()==size(COV1,1));
01024                                 ASSERT_( COV1.isSquare() && COV2.isSquare() );
01025                                 ASSERT_( size(COV1,1)==size(COV2,1));
01026                         #endif
01027                         const size_t N = size(COV1,1);
01028                         MAT1 COV = COV1;
01029                         COV+=COV2;
01030                         COV.substract_An(CROSS_COV12,2);
01031                         MAT1 COV_inv;
01032                         COV.inv_fast(COV_inv);
01033                         return multiply_HCHt_scalar(mean_diffs,COV_inv);
01034                         MRPT_END
01035                 }
01036 
01037                 /** 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.
01038                   *  \f[ d = \sqrt{ \Delta_\mu^\top (\Sigma_1 + \Sigma_2 - 2 \Sigma_12 )^{-1} \Delta_\mu } \f]
01039                   */
01040                 template<class VECTORLIKE,class MAT1,class MAT2,class MAT3> inline typename VECTORLIKE::value_type
01041                 mahalanobisDistance(
01042                         const VECTORLIKE &mean_diffs,
01043                         const MAT1 &COV1,
01044                         const MAT2 &COV2,
01045                         const MAT3 &CROSS_COV12 )
01046                 {
01047                         return std::sqrt( mahalanobisDistance( mean_diffs, COV1,COV2,CROSS_COV12 ));
01048                 }
01049 
01050                 /** 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.
01051                   *  \f[ d^2 = \Delta_\mu^\top \Sigma^{-1} \Delta_\mu  \f]
01052                   */
01053                 template<class VECTORLIKE,class MATRIXLIKE>
01054                 inline typename MATRIXLIKE::value_type
01055                 mahalanobisDistance2(const VECTORLIKE &delta_mu,const MATRIXLIKE &cov)
01056                 {
01057                         ASSERTDEB_(cov.isSquare())
01058                         ASSERTDEB_(cov.getColCount()==delta_mu.size())
01059                         return multiply_HCHt_scalar(delta_mu,cov.inverse());
01060                 }
01061 
01062                 /** 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.
01063                   *  \f[ d^2 = \sqrt( \Delta_\mu^\top \Sigma^{-1} \Delta_\mu ) \f]
01064                   */
01065                 template<class VECTORLIKE,class MATRIXLIKE>
01066                 inline typename MATRIXLIKE::value_type
01067                 mahalanobisDistance(const VECTORLIKE &delta_mu,const MATRIXLIKE &cov)
01068                 {
01069                         return std::sqrt(mahalanobisDistance2(delta_mu,cov));
01070                 }
01071 
01072                 /** Computes the integral of the product of two Gaussians, with means separated by "mean_diffs" and covariances "COV1" and "COV2".
01073                   *  \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]
01074                   */
01075                 template <typename T>
01076                 T productIntegralTwoGaussians(
01077                         const std::vector<T> &mean_diffs,
01078                         const CMatrixTemplateNumeric<T> &COV1,
01079                         const CMatrixTemplateNumeric<T> &COV2
01080                         )
01081                 {
01082                         const size_t vector_dim = mean_diffs.size();
01083                         ASSERT_(vector_dim>=1)
01084 
01085                         CMatrixTemplateNumeric<T> C = COV1;
01086                         C+= COV2;       // Sum of covs:
01087                         const T cov_det = C.det();
01088                         CMatrixTemplateNumeric<T> C_inv;
01089                         C.inv_fast(C_inv);
01090 
01091                         return std::pow( M_2PI, -0.5*vector_dim ) * (1.0/std::sqrt( cov_det ))
01092                                 * exp( -0.5 * mean_diffs.multiply_HCHt_scalar(C_inv) );
01093                 }
01094 
01095                 /** Computes the integral of the product of two Gaussians, with means separated by "mean_diffs" and covariances "COV1" and "COV2".
01096                   *  \f[ D = \frac{1}{(2 \pi)^{0.5 N} \sqrt{}  }  \exp( \Delta_\mu^\top (\Sigma_1 + \Sigma_2)^{-1} \Delta_\mu)  \f]
01097                   */
01098                 template <typename T, size_t DIM>
01099                 T productIntegralTwoGaussians(
01100                         const std::vector<T> &mean_diffs,
01101                         const CMatrixFixedNumeric<T,DIM,DIM> &COV1,
01102                         const CMatrixFixedNumeric<T,DIM,DIM> &COV2
01103                         )
01104                 {
01105                         ASSERT_(mean_diffs.size()==DIM);
01106 
01107                         CMatrixFixedNumeric<T,DIM,DIM> C = COV1;
01108                         C+= COV2;       // Sum of covs:
01109                         const T cov_det = C.det();
01110                         CMatrixFixedNumeric<T,DIM,DIM> C_inv(UNINITIALIZED_MATRIX);
01111                         C.inv_fast(C_inv);
01112 
01113                         return std::pow( M_2PI, -0.5*DIM ) * (1.0/std::sqrt( cov_det ))
01114                                 * exp( -0.5 * mean_diffs.multiply_HCHt_scalar(C_inv) );
01115                 }
01116 
01117                 /** Computes both, the integral of the product of two Gaussians and their square Mahalanobis distance.
01118                   * \sa productIntegralTwoGaussians, mahalanobisDistance2
01119                   */
01120                 template <typename T, class VECLIKE,class MATLIKE1, class MATLIKE2>
01121                 void productIntegralAndMahalanobisTwoGaussians(
01122                         const VECLIKE   &mean_diffs,
01123                         const MATLIKE1  &COV1,
01124                         const MATLIKE2  &COV2,
01125                         T                               &maha2_out,
01126                         T                               &intprod_out,
01127                         const MATLIKE1  *CROSS_COV12=NULL
01128                         )
01129                 {
01130                         const size_t vector_dim = mean_diffs.size();
01131                         ASSERT_(vector_dim>=1)
01132 
01133                         MATLIKE1 C = COV1;
01134                         C+= COV2;       // Sum of covs:
01135                         if (CROSS_COV12) { C-=*CROSS_COV12; C-=*CROSS_COV12; }
01136                         const T cov_det = C.det();
01137                         MATLIKE1 C_inv;
01138                         C.inv_fast(C_inv);
01139 
01140                         maha2_out = mean_diffs.multiply_HCHt_scalar(C_inv);
01141                         intprod_out = std::pow( M_2PI, -0.5*vector_dim ) * (1.0/std::sqrt( cov_det ))*exp(-0.5*maha2_out);
01142                 }
01143 
01144                 /** 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.
01145                   * \sa productIntegralTwoGaussians, mahalanobisDistance2, normalPDF, mahalanobisDistance2AndPDF
01146                   */
01147                 template <typename T, class VECLIKE,class MATRIXLIKE>
01148                 void mahalanobisDistance2AndLogPDF(
01149                         const VECLIKE           &diff_mean,
01150                         const MATRIXLIKE        &cov,
01151                         T                                       &maha2_out,
01152                         T                                       &log_pdf_out)
01153                 {
01154                         MRPT_START
01155                         ASSERTDEB_(cov.isSquare())
01156                         ASSERTDEB_(size_t(cov.getColCount())==size_t(diff_mean.size()))
01157                         MATRIXLIKE C_inv;
01158                         cov.inv(C_inv);
01159                         maha2_out = multiply_HCHt_scalar(diff_mean,C_inv);
01160                         log_pdf_out = static_cast<typename MATRIXLIKE::value_type>(-0.5)* (
01161                                 maha2_out+
01162                                 static_cast<typename MATRIXLIKE::value_type>(cov.getColCount())*::log(static_cast<typename MATRIXLIKE::value_type>(M_2PI))+
01163                                 ::log(cov.det())
01164                                 );
01165                         MRPT_END
01166                 }
01167 
01168                 /** Computes both, the PDF and the square Mahalanobis distance between a point (given by its difference wrt the mean) and a Gaussian.
01169                   * \sa productIntegralTwoGaussians, mahalanobisDistance2, normalPDF
01170                   */
01171                 template <typename T, class VECLIKE,class MATRIXLIKE>
01172                 inline void mahalanobisDistance2AndPDF(
01173                         const VECLIKE           &diff_mean,
01174                         const MATRIXLIKE        &cov,
01175                         T                                       &maha2_out,
01176                         T                                       &pdf_out)
01177                 {
01178                         mahalanobisDistance2AndLogPDF(diff_mean,cov,maha2_out,pdf_out);
01179                         pdf_out = std::exp(pdf_out); // log to linear
01180                 }
01181 
01182                 /** @} */
01183 
01184                 /** A versatile template to build vectors on-the-fly in a style close to MATLAB's  v=[a b c d ...]
01185                   *  The first argument of the template is the vector length, and the second the type of the numbers.
01186                   *  Some examples:
01187                   *
01188                   *  \code
01189                   *    vector_double  = make_vector<4,double>(1.0,3.0,4.0,5.0);
01190                   *    vector_float   = make_vector<2,float>(-8.12, 3e4);
01191                   *  \endcode
01192                   */
01193                 template <size_t N, typename T>
01194                 std::vector<T> make_vector(const T val1, ...)
01195                 {
01196                         MRPT_COMPILE_TIME_ASSERT( N>0 )
01197                         std::vector<T>  ret;
01198                         ret.reserve(N);
01199 
01200                         ret.push_back(val1);
01201 
01202                         va_list args;
01203                         va_start(args,val1);
01204                         for (size_t i=0;i<N-1;i++)
01205                                 ret.push_back( va_arg(args,T) );
01206 
01207                         va_end(args);
01208                         return ret;
01209                 }
01210 
01211         } // End of MATH namespace
01212 
01213 } // End of namespace
01214 
01215 #endif



Page generated by Doxygen 1.7.3 for MRPT 0.9.4 SVN:exported at Tue Jan 25 21:56:31 UTC 2011