utils.h

Go to the documentation of this file.

/* +---------------------------------------------------------------------------+
   |          The Mobile Robot Programming Toolkit (MRPT) C++ library          |
   |                                                                           |
   |                   http://mrpt.sourceforge.net/                            |
   |                                                                           |
   |   Copyright (C) 2005-2010  University of Malaga                           |
   |                                                                           |
   |    This software was written by the Machine Perception and Intelligent    |
   |      Robotics Lab, University of Malaga (Spain).                          |
   |    Contact: Jose-Luis Blanco  <jlblanco@ctima.uma.es>                     |
   |                                                                           |
   |  This file is part of the MRPT project.                                   |
   |                                                                           |
   |     MRPT is free software: you can redistribute it and/or modify          |
   |     it under the terms of the GNU General Public License as published by  |
   |     the Free Software Foundation, either version 3 of the License, or     |
   |     (at your option) any later version.                                   |
   |                                                                           |
   |   MRPT is distributed in the hope that it will be useful,                 |
   |     but WITHOUT ANY WARRANTY; without even the implied warranty of        |
   |     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         |
   |     GNU General Public License for more details.                          |
   |                                                                           |
   |     You should have received a copy of the GNU General Public License     |
   |     along with MRPT.  If not, see <http://www.gnu.org/licenses/>.         |
   |                                                                           |
   +---------------------------------------------------------------------------+ */
#ifndef  MRPT_MATH_H
#define  MRPT_MATH_H

#include <mrpt/utils/utils_defs.h>
#include <mrpt/math/CMatrixTemplateNumeric.h>
#include <mrpt/math/CMatrixFixedNumeric.h>
#include <mrpt/math/CVectorTemplate.h>
#include <mrpt/math/vector_ops.h>
#include <mrpt/math/CHistogram.h>

#include <numeric>
#include <cmath>

/*---------------------------------------------------------------
                Namespace
  ---------------------------------------------------------------*/
namespace mrpt
{
        /** This base provides a set of functions for maths stuff.
         */
        namespace math
        {
            using namespace mrpt::utils;

                /** Loads one row of a text file as a numerical std::vector.
                  * \return false on EOF or invalid format.
                  * The body of the function is implemented in MATH.cpp
                        */
                bool MRPTDLLIMPEXP loadVector( utils::CFileStream &f, std::vector<int> &d);

                /** Loads one row of a text file as a numerical std::vector.
                  * \return false on EOF or invalid format.
                  * The body of the function is implemented in MATH.cpp
                        */
                bool MRPTDLLIMPEXP loadVector( utils::CFileStream &f, std::vector<double> &d);

                /** Returns true if value is Not-a-number (NAN)
              */
            bool MRPTDLLIMPEXP isNan(float v);

            /** Returns true if value is Not-a-number (NAN)
              */
            bool MRPTDLLIMPEXP isNan(double v);

            /** Returns true if value is finite
              */
            bool MRPTDLLIMPEXP isFinite(float v);

            /** Returns true if value is finite
              */
            bool MRPTDLLIMPEXP isFinite(double v);

                /** A template for counting how many elements in an array are non-Zero.
                        */
                template <class T>
                size_t  countNonZero(const std::vector<T> &a)
                {
                        typename std::vector<T>::const_iterator it_a;
                        size_t          count=0;
                        for (it_a=a.begin(); it_a!=a.end(); it_a++) if (*it_a) count++;
                        return count;
                }

                /** Finds the maximum value (and the corresponding zero-based index) from a given vector.
                  */
                template<class T>
                T  maximum(const std::vector<T> &v, unsigned int *maxIndex = NULL)
                {
                        typename std::vector<T>::const_iterator maxIt = std::max_element(v.begin(),v.end());
                        if (maxIndex) *maxIndex = static_cast<unsigned int>( std::distance(v.begin(),maxIt) );
                        return *maxIt;
                }

                /** Compute the norm-infinite of a vector ($f[ ||\mathbf{v}||_\infnty $f]), ie the maximum absolute value of the elements.
                  */
                template<class T>
                T  norm_inf(const std::vector<T> &v, unsigned int *maxIndex = NULL)
                {
                        double  M=0;
                        int             i,M_idx=-1;
                        typename std::vector<T>::const_iterator it;
                        for (i=0, it=v.begin(); it!=v.end();it++,i++)
                        {
                                double  it_abs = fabs( static_cast<double>(*it));
                                if (it_abs>M || M_idx==-1)
                                {
                                        M = it_abs;
                                        M_idx = i;
                                }
                        }
                        if (maxIndex) *maxIndex = M_idx;
                        return static_cast<T>(M);
                }


                /** Compute the 2-norm of the vector (the Euclidean distance to the origin).
                  */
                template<class T>
                T  norm(const std::vector<T> &v)
                {
                        T       total=0;
                        typename std::vector<T>::const_iterator it;
                        for (it=v.begin(); it!=v.end();it++)
                                total += square(*it);
                        return ::sqrt(total);
                }

                /** Finds the maximum value (and the corresponding zero-based index) from a given vector.
                  * \sa maximum, minimum_maximum
                  */
                template <class T>
                T  minimum(const std::vector<T> &v, unsigned int *minIndex = NULL)
                {
                        typename std::vector<T>::const_iterator minIt = std::min_element(v.begin(),v.end());
                        if (minIndex) *minIndex = static_cast<unsigned int>( std::distance(v.begin(),minIt) );
                        return *minIt;
                }

                /** Compute the minimum and maximum of a vector at once
                  * \sa maximum, minimum
                  */
                template<class T>
                void minimum_maximum(const std::vector<T> &v, T& out_min, T& out_max, unsigned int *minIndex = NULL,unsigned int *maxIndex = NULL)
                {
                        size_t N = v.size();
                        if (N)
                        {
                                out_max = out_min = v[0];
                                unsigned int min_idx=0,max_idx=0;
                                for (size_t i=0;i<N;i++)
                                {
                                        if (v[i]<out_min)
                                        {
                                                out_min=v[i];
                                                min_idx = i;
                                        }
                                        if (v[i]>out_max)
                                        {
                                                out_max=v[i];
                                                max_idx = i;
                                        }
                                }
                                if (minIndex) *minIndex = min_idx;
                                if (maxIndex) *maxIndex = max_idx;
                        }
                }

                /** Computes the mean value of a vector
                  * \sa math::stddev,math::meanAndStd
                  */
                template<class T>
                        double  mean(const std::vector<T> &v)
                {
                        if (v.empty())
                                        return 0;
                        else    return static_cast<double>( std::accumulate(v.begin(),v.end(), static_cast<T>(0) ) ) / v.size();
                }

                /** Computes the sum of all the elements of a vector
                  * \sa cumsum
                  */
                template<class T>
                        T  sum(const std::vector<T> &v)
                {
                        return std::accumulate(v.begin(),v.end(), static_cast<T>(0) );
                }

                /** Generates an equidistant sequence of numbers given the first one, the last one and the desired number of points.
                  \sa sequence */
                template<typename T,typename K>
                void linspace(T first,T last, size_t count, std::vector<K> &out_vector)
                {
                        if (count<2)
                        {
                                out_vector.assign(1,last);
                                return;
                        }
                        else
                        {
                                out_vector.resize(count);
                                const T incr = (last-first)/T(count-1);
                                T c = first;
                                for (size_t i=0;i<count;i++,c+=incr)
                                        out_vector[i] = K(c);
                        }
                }

                /** Generates an equidistant sequence of numbers given the first one, the last one and the desired number of points. 
                  \sa sequence */
                template<class T>
                inline std::vector<T> linspace(T first,T last, size_t count)
                {
                        std::vector<T> ret;
                        mrpt::math::linspace(first,last,count,ret);
                        return ret;
                }

                /** Generates a sequence of values [first,first+STEP,first+2*STEP,...]   \sa linspace */
                template<class T,T STEP>
                inline std::vector<T> sequence(T first,size_t length)
                {
                        std::vector<T> ret(length);
                        if (!length) return ret;
                        size_t i=0;
                        while (length--) { ret[i++]=first; first+=STEP; }
                        return ret;
                }

                /** Generates a vector of all ones of the given length. */
                template<class T>
                std::vector<T> ones(size_t count)
                {
                        return std::vector<T>(count,1);
                }

                /** Generates a vector of all zeros of the given length. */
                template<class T>
                std::vector<T> zeros(size_t count)
                {
                        return std::vector<T>(count,0);
                }

                /** Normalize a vector, such as its norm is the unity.
                  *  If the vector has a null norm, the output is a null vector.
                  */
                template<class T>
                        void normalize(const std::vector<T> &v, std::vector<T> &out_v)
                {
                        T       total=0;
                        typename std::vector<T>::const_iterator it;
                        for (it=v.begin(); it!=v.end();it++)
                                total += square(*it);
                        total = ::sqrt(total);
                        if (total)
                        {
                                out_v.resize(v.size());
                                typename std::vector<T>::iterator q;
                                for (it=v.begin(),q=out_v.begin(); q!=out_v.end();it++,q++)
                                        *q = *it / total;
                        }
                        else out_v.assign(v.size(),0);
                }

                /** Computes the cumulative sum of all the elements of a vector
                  * \sa sum
                  */
                template<class T>
                        std::vector<T>  cumsum(const std::vector<T> &v)
                {
                        T                            last = 0;
                        std::vector<T>   ret(v.size());
                        typename std::vector<T>::const_iterator it;
                        typename std::vector<T>::iterator               it2;
                        for (it = v.begin(),it2=ret.begin();it!=v.end();it++,it2++)
                                last = (*it2) = last + (*it);
                        return ret;
                }

                /** Computes the cumulative sum of all the elements of a vector, saving the result in a given vector.
                  * \sa sum
                  */
                template<class T>
                        void cumsum(const std::vector<T> &v, std::vector<T> &out_cumsum)
                {
                        T                            last = 0;
                        out_cumsum.resize(v.size());
                        typename std::vector<T>::const_iterator it;
                        typename std::vector<T>::iterator               it2;
                        for (it = v.begin(),it2=out_cumsum.begin();it!=v.end();it++,it2++)
                                last = (*it2) = last + (*it);
                }

                /** Computes the standard deviation of a vector
                  * \param v The set of data
                  * \param unbiased If set to true or false the std is normalized by "N-1" or "N", respectively.
                  * \sa math::mean,math::meanAndStd
                  */
                template<class T>
                        double  stddev(const std::vector<T> &v, bool unbiased = true)
                {
                        if (v.size()<2)
                                return 0;
                        else
                        {
                                // Compute the mean:
                                typename std::vector<T>::const_iterator it;
                                double                                   vector_std=0,vector_mean = 0;
                                for (it = v.begin();it!=v.end();it++) vector_mean += (*it);
                                vector_mean /= static_cast<double>(v.size());
                                // Compute the std:
                                for (it = v.begin();it!=v.end();it++) vector_std += square((*it)-vector_mean);
                                vector_std = sqrt(vector_std  / static_cast<double>(v.size() - (unbiased ? 1:0)) );

                                return vector_std;
                        }
                }


                /** Computes the mean vector and covariance from a list of values given as a vector of vectors, where each row is a sample.
                  * \param v The set of data, as a vector of N vectors of M elements.
                  * \param out_mean The output M-vector for the estimated mean.
                  * \param out_cov The output MxM matrix for the estimated covariance matrix.
                  * \sa math::mean,math::stddev, math::cov
                  */
                template<class T>
                void  meanAndCov(
                        const std::vector<std::vector<T> > &v,
                        vector_double   &out_mean,
                        CMatrixDouble   &out_cov
                        )
                {
                        const size_t N = v.size();
                        ASSERTMSG_(N>0,"The input vector contains no elements");
                        const double N_inv = 1.0/N;

                        const size_t M = v[0].size();
                        ASSERTMSG_(M>0,"The input vector contains rows of length 0");

                        // First: Compute the mean
                        out_mean.assign(M,0);
                        for (size_t i=0;i<N;i++)
                                for (size_t j=0;j<M;j++)
                                        out_mean[j]+=v[i][j];
                        out_mean*=N_inv;

                        // Second: Compute the covariance
                        //  Save only the above-diagonal part, then after averaging
                        //  duplicate that part to the other half.
                        out_cov.zeros(M,M);
                        for (size_t i=0;i<N;i++)
                        {
                                for (size_t j=0;j<M;j++)
                                        out_cov.get_unsafe(j,j)+=square(v[i][j]-out_mean[j]);

                                for (size_t j=0;j<M;j++)
                                        for (size_t k=j+1;k<M;k++)
                                                out_cov.get_unsafe(j,k)+=(v[i][j]-out_mean[j])*(v[i][k]-out_mean[k]);
                        }
                        for (size_t j=0;j<M;j++)
                                for (size_t k=j+1;k<M;k++)
                                        out_cov.get_unsafe(k,j) = out_cov.get_unsafe(j,k);
                        out_cov*=N_inv;
                }

                /** Computes the mean vector and covariance from a list of samples in an NxM matrix, where each row is a sample, so the covariance is MxM.
                  * \param v The set of data as a NxM matrix, of types: CMatrixTemplateNumeric or CMatrixFixedNumeric
                  * \param out_mean The output M-vector for the estimated mean.
                  * \param out_cov The output MxM matrix for the estimated covariance matrix, this can also be either a fixed-size of dynamic size matrix.
                  * \sa math::mean,math::stddev, math::cov
                  */
                template<class MAT_IN, class MAT_OUT>
                void  meanAndCov(
                        const MAT_IN &v,
                        vector_double   &out_mean,
                        MAT_OUT                 &out_cov
                        )
                {
                        const size_t N = v.getRowCount();
                        ASSERTMSG_(N>0,"The input matrix contains no elements");
                        const double N_inv = 1.0/N;

                        const size_t M = v.getColCount();
                        ASSERTMSG_(M>0,"The input matrix contains rows of length 0");

                        // First: Compute the mean
                        out_mean.assign(M,0);
                        for (size_t i=0;i<N;i++)
                                for (size_t j=0;j<M;j++)
                                        out_mean[j]+=v.get_unsafe(i,j);
                        out_mean*=N_inv;

                        // Second: Compute the covariance
                        //  Save only the above-diagonal part, then after averaging
                        //  duplicate that part to the other half.
                        out_cov.zeros(M,M);
                        for (size_t i=0;i<N;i++)
                        {
                                for (size_t j=0;j<M;j++)
                                        out_cov.get_unsafe(j,j)+=square(v.get_unsafe(i,j)-out_mean[j]);

                                for (size_t j=0;j<M;j++)
                                        for (size_t k=j+1;k<M;k++)
                                                out_cov.get_unsafe(j,k)+=(v.get_unsafe(i,j)-out_mean[j])*(v.get_unsafe(i,k)-out_mean[k]);
                        }
                        for (size_t j=0;j<M;j++)
                                for (size_t k=j+1;k<M;k++)
                                        out_cov.get_unsafe(k,j) = out_cov.get_unsafe(j,k);
                        out_cov*=N_inv;
                }


                /** Computes the covariance matrix from a list of values given as a vector of vectors, where each row is a sample.
                  * \param v The set of data, as a vector of N vectors of M elements.
                  * \param out_cov The output MxM matrix for the estimated covariance matrix.
                  * \sa math::mean,math::stddev, math::cov
                  */
                template<class T>
                CMatrixDouble cov( const std::vector<std::vector<T> > &v )
                {
                        vector_double m;
                        CMatrixDouble C;
                        meanAndCov(v,m,C);
                        return C;
                }

                /** Computes the covariance matrix from a list of samples in an NxM matrix, where each row is a sample, so the covariance is MxM.
                  * \param v The set of data, as a NxM matrix.
                  * \param out_cov The output MxM matrix for the estimated covariance matrix.
                  * \sa math::mean,math::stddev, math::cov
                  */
                template<class T>
                CMatrixDouble cov( const CMatrixTemplate<T> &v )
                {
                        vector_double m;
                        CMatrixDouble C;
                        meanAndCov(v,m,C);
                        return C;
                }

                /** Computes the covariance matrix from a list of samples in an NxM matrix, where each row is a sample, so the covariance is MxM.
                  * \param v The set of data, as a NxM matrix.
                  * \param out_cov The output MxM matrix for the estimated covariance matrix.
                  * \sa math::mean,math::stddev, math::cov
                  */
                template<class T, size_t N, size_t M>
                CMatrixFixedNumeric<T,M,M> cov( const CMatrixFixedNumeric<T,N,M> &v )
                {
                        vector_double m;
                        CMatrixFixedNumeric<T,M,M> C;
                        meanAndCov(v,m,C);
                        return C;
                }

                /** Computes the standard deviation of a vector
                  * \param v The set of data
                  * \param out_mean The output for the estimated mean
                  * \param out_std The output for the estimated standard deviation
                  * \param unbiased If set to true or false the std is normalized by "N-1" or "N", respectively.
                  * \sa math::mean,math::stddev
                  */
                template<class T>
                        void  meanAndStd(
                                const std::vector<T>    &v,
                                double                  &out_mean,
                                double                  &out_std,
                                bool                    unbiased = true)
                {
                        if (v.size()<2)
                        {
                                out_std = 0;
                                if (v.size()==1)
                                        out_mean = v[0];
                        }
                        else
                        {
                                // Compute the mean:
                                typename std::vector<T>::const_iterator it;
                                out_std=0,out_mean = 0;
                                for (it = v.begin();it!=v.end();it++) out_mean += (*it);
                                out_mean /= static_cast<double>(v.size());

                                // Compute the std:
                                for (it = v.begin();it!=v.end();it++) out_std += square(static_cast<double>(*it)-out_mean);
                                out_std = sqrt(out_std / static_cast<double>((v.size() - (unbiased ? 1:0)) ));
                        }
                }

                /** Computes the weighted histogram for a vector of values and their corresponding weights.
                  *  \param values [IN] The N values
                  *  \param weights [IN] The weights for the corresponding N values
                  *  \param binWidth [IN] The desired width of the bins
                  *  \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"
                  *  \param out_binValues [OUT] The ratio of values at each given bin, such as the whole vector sums up the unity.
                  */
                template<class T>
                        void  weightedHistogram(
                                const std::vector<T>            &values,
                                const std::vector<T>            &weights,
                                float                           binWidth,
                                std::vector<float>      &out_binCenters,
                                std::vector<float>      &out_binValues )
                        {
                                MRPT_START;

                                ASSERT_( values.size() == weights.size() );
                                ASSERT_( binWidth > 0 );
                                T                               minBin = minimum( values );
                                unsigned int    nBins = static_cast<unsigned>(ceil((maximum( values )-minBin) / binWidth));

                                // Generate bin center and border values:
                                out_binCenters.resize(nBins);
                                out_binValues.clear(); out_binValues.resize(nBins,0);
                                float              halfBin = 0.5f*binWidth;;
                                vector_float   binBorders(nBins+1,minBin-halfBin);
                                for (unsigned int i=0;i<nBins;i++)
                                {
                                        binBorders[i+1] = binBorders[i]+binWidth;
                                        out_binCenters[i] = binBorders[i]+halfBin;
                                }

                                // Compute the histogram:
                                float   totalSum = 0;
                                for (typename  std::vector<T>::const_iterator itVal = values.begin(), itW = weights.begin(); itVal!=values.end(); ++itVal, ++itW )
                                {
                                        int idx = round(((*itVal)-minBin)/binWidth);
                                        if (idx>=nBins) idx=nBins-1;
                                        ASSERT_(idx>=0);
                                        out_binValues[idx] += *itW;
                                        totalSum+= *itW;
                                }

                                if (totalSum)
                                        out_binValues = out_binValues / totalSum;


                                MRPT_END;
                        }


                /** Computes the factorial of an integer number and returns it as a 64-bit integer number.
                  */
                uint64_t MRPTDLLIMPEXP  factorial64(unsigned int n);

                /** Computes the factorial of an integer number and returns it as a double value (internally it uses logarithms for avoiding overflow).
                  */
                double MRPTDLLIMPEXP  factorial(unsigned int n);


                /** Modifies the given angle to translate it into the [0,2pi[ range.
                  * \note Take care of not instancing this template for integer numbers, since it only works for float, double and long double.
                  * \sa wrapToPi, wrapTo2Pi, unwrap2PiSequence
                  */
                template <class T>
                void wrapTo2PiInPlace(T &a)
                {
                        bool was_neg = a<0;
                        a = fmod(a, static_cast<T>(M_2PI) );
                        if (was_neg) a+=static_cast<T>(M_2PI);
                }

                /** Modifies the given angle to translate it into the [0,2pi[ range.
                  * \note Take care of not instancing this template for integer numbers, since it only works for float, double and long double.
                  * \sa wrapToPi, wrapTo2Pi, unwrap2PiSequence
                  */
                template <class T>
                T wrapTo2Pi(T a)
                {
                        wrapTo2PiInPlace(a);
                        return a;
                }

                /** Modifies the given angle to translate it into the ]-pi,pi] range.
                  * \note Take care of not instancing this template for integer numbers, since it only works for float, double and long double.
                  * \sa wrapTo2Pi, wrapToPiInPlace, unwrap2PiSequence
                  */
                template <class T>
                T wrapToPi(T a)
                {
                        return wrapTo2Pi( a + static_cast<T>(M_PI) )-static_cast<T>(M_PI);
                }

                /** Modifies the given angle to translate it into the ]-pi,pi] range.
                  * \note Take care of not instancing this template for integer numbers, since it only works for float, double and long double.
                  * \sa wrapToPi,wrapTo2Pi, unwrap2PiSequence
                  */
                template <class T>
                void wrapToPiInPlace(T &a)
                {
                        a = wrapToPi(a);
                }

                /** Round up to the nearest power of two of a given number
                  */
                template <class T>
                T round2up(T val)
                {
                        T n = 1;
                        while (n < val)
                        {
                                n <<= 1;
                                if (n<=1)
                                        THROW_EXCEPTION("Overflow!");
                        }
                        return n;
                }

                /** Round a decimal number up to the given 10'th power (eg, to 1000,100,10, and also fractions)
                  *  power10 means round up to: 1 -> 10, 2 -> 100, 3 -> 1000, ...  -1 -> 0.1, -2 -> 0.01, ...
                  */
                template <class T>
                T round_10power(T val, int power10)
                {
                        long double F = ::pow((long double)10.0,-(long double)power10);
                        long int t = round_long( val * F );
                        return T(t/F);
                }

                /** Cholesky factorization: in = out' · out
                  *     Given a positive-definite symmetric matrix, this routine constructs its Cholesky decomposition.
                  * On input, only the upper triangle of "IN" need be given; it is not modified.
                  *  The Cholesky factorization is returned in "out" in the upper triangle.
                  *  (by AJOGD @ JAN-2007)
                  */
                template<class T>
                void  chol(const CMatrixTemplateNumeric<T> &in,CMatrixTemplateNumeric<T> &out)
                {
                        if (in.getColCount() != in.getRowCount()) THROW_EXCEPTION("Cholesky factorization error, in matrix not square");
                        size_t  i,j,k;
                        T               sum;
                        out.setSize(in.getRowCount(),in.getColCount());
                        for (i=0;i<in.getRowCount();i++)
                        {
                                for (j=i;j<in.getColCount();j++)
                                {
                                        sum=in(i,j);
                                        for (k=i-1;(k>=0)&(k<in.getColCount());k--)
                                        {
                                                sum -= out(k,i)*out(k,j);
                                        }
                                        if (i==j)
                                        {
                                                if (sum<0)
                                                {
                                                        THROW_EXCEPTION("Cholesky factorization error, in matrix not defined-positive");
                                                }
                                                out(i,j)=sqrt(sum);
                                        }
                                        else
                                        {
                                                out(i,j)=sum/out(i,i);
                                                out(j,i)=0;
                                        }
                                }
                        }
                }

                /** Calculate the correlation between two matrices
                  *  (by AJOGD @ JAN-2007)
                  */
                template<class T>
                double  correlate_matrix(const CMatrixTemplateNumeric<T> &a1, const CMatrixTemplateNumeric<T> &a2)
                {
                        if ((a1.getColCount()!=a2.getColCount())|(a1.getRowCount()!=a2.getRowCount()))
                                THROW_EXCEPTION("Correlation Error!, images with no same size");

                        int i,j;
                        T x1,x2;
                        T syy=0, sxy=0, sxx=0, m1=0, m2=0 ,n=a1.getRowCount()*a2.getColCount();

                        //find the means
                        for (i=0;i<a1.getRowCount();i++)
                        {
                                for (j=0;j<a1.getColCount();j++)
                                {
                                        m1 += a1(i,j);
                                        m2 += a2(i,j);
                                }
                        }
                        m1 /= n;
                        m2 /= n;

                        for (i=0;i<a1.getRowCount();i++)
                        {
                                for (j=0;j<a1.getColCount();j++)
                                {
                                        x1 = a1(i,j) - m1;
                                        x2 = a2(i,j) - m2;
                                        sxx += x1*x1;
                                        syy += x2*x2;
                                        sxy += x1*x2;
                                }
                        }

                        return sxy / sqrt(sxx * syy);
                }


                /**     Matrix QR decomposition. A = QR, where R is upper triangular and Q is orthogonal, that is, ~QQ = 1
                  * If A is a LxM dimension matrix, this function only return the LxL upper triangular matrix R instead of LxM pseudo-upper
                  * triangular matrix (been L<=M)
                  * This function has been extracted from "Numerical Recipes in C".
                  *     /param A is the original matrix to decompose
                  * /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]
                  *                             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
                  *     /param R is the upper triangular matrix
                  *     /param sign returns as true (1) is singularity is encountered during the decomposition, but the decomposition is still complete
                  *                             in this case; otherwise it returns false (0)
                  */

                        template<class T>
                        void MRPTDLLIMPEXP qr_decomposition(
                                CMatrixTemplateNumeric<T>       &A,
                                CMatrixTemplateNumeric<T>       &R,
                                CMatrixTemplateNumeric<T>       &Q,
                                CVectorTemplate<T>                      &c,
                                int                                                             &sing);


                /**If R = CHOL(A) is the original Cholesky factorization of A, then R1 = CHOLUPDATE(R,X) returns the upper triangular
                  * Cholesky factor of A + X*X', where X is a column vector of appropriate length.
                  */
                template<class T>
                        void MRPTDLLIMPEXP UpdateCholesky(
                                CMatrixTemplateNumeric<T>       &chol,
                                CVectorTemplate<T>                      &r1Modification);

                /** Compute the two eigenvalues of a 2x2 matrix.
                  * \param in_matrx The 2x2 input matrix.
                  * \param min_eigenvalue (out) The minimum eigenvalue of the matrix.
                  * \param max_eigenvalue (out) The maximum eigenvalue of the matrix.
                  * by FAMD, MAR-2007
                  */
                void MRPTDLLIMPEXP  computeEigenValues2x2(
                        const CMatrixFloat      &in_matrix,
                        float                                                           &min_eigenvalue,
                        float                                                           &max_eigenvalue );

                /** Computes the 'exp' of all the elements of a vector
                  * \sa math::Log
                  */
                template<class T>
                        std::vector<T> Exp(const std::vector<T> &v)
                {
                        std::vector<T>  ret(v.size());
                        typename std::vector<T>::const_iterator it;
                        typename std::vector<T>::iterator               it2;
                        for (it = v.begin(),it2=ret.begin();it!=v.end();it++,it2++) *it2 = ::exp(*it);
                        return ret;
                }

                /** Computes the 'log' of all the elements of a vector
                  * \sa math::Exp
                  */
                template<class T>
                        std::vector<T> Log(const std::vector<T> &v)
                {
                        std::vector<T>  ret(v.size());
                        typename std::vector<T>::const_iterator it;
                        typename std::vector<T>::iterator               it2;
                        for (it = v.begin(),it2=ret.begin();it!=v.end();it++,it2++) *it2 = ::log(*it);
                        return ret;
                }

                /** A numerically-stable method to compute average likelihood values with strongly different ranges (unweighted likelihoods: compute the arithmetic mean).
                  *  This method implements this equation:
                  *
                  *  \f[ return = - \log N + \log  \sum_{i=1}^N e^{ll_i-ll_{max}} + ll_{max} \f]
                  *
                  * See also the <a href="http://babel.isa.uma.es/mrpt/index.php/Averaging_Log-Likelihood_Values:Numerical_Stability">tutorial page</a>.
                  */
                double MRPTDLLIMPEXP averageLogLikelihood( const vector_double &logLikelihoods );

                /** 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.
                  */
                double MRPTDLLIMPEXP averageWrap2Pi(const vector_double &angles );

                /** A numerically-stable method to average likelihood values with strongly different ranges (weighted likelihoods).
                  *  This method implements this equation:
                  *
                  *  \f[ return = \log \left( \frac{1}{\sum_i e^{lw_i}} \sum_i  e^{lw_i} e^{ll_i}  \right) \f]
                  *
                  * See also the <a href="http://babel.isa.uma.es/mrpt/index.php/Averaging_Log-Likelihood_Values:Numerical_Stability">tutorial page</a>.
                  */
                double MRPTDLLIMPEXP  averageLogLikelihood(
                        const vector_double &logWeights,
                        const vector_double &logLikelihoods );

                /** Generates a string with the MATLAB commands required to plot an confidence interval (ellipse) for a 2D Gaussian ('float' version)..
                  *  \param cov22 The 2x2 covariance matrix
                  *  \param mean  The 2-length vector with the mean
                  *  \param stdCount How many "quantiles" to get into the area of the ellipse: 2: 95%, 3:99.97%,...
                  *  \param style A matlab style string, for colors, line styles,...
                  *  \param nEllipsePoints The number of points in the ellipse to generate
                  */
                std::string MRPTDLLIMPEXP  MATLAB_plotCovariance2D(
                        const CMatrixFloat  &cov22,
                        const CVectorFloat  &mean,
                        const float         &stdCount,
                        const std::string   &style = std::string("b"),
                        const size_t        &nEllipsePoints = 30 );

                /** Generates a string with the MATLAB commands required to plot an confidence interval (ellipse) for a 2D Gaussian ('double' version).
                  *  \param cov22 The 2x2 covariance matrix
                  *  \param mean  The 2-length vector with the mean
                  *  \param stdCount How many "quantiles" to get into the area of the ellipse: 2: 95%, 3:99.97%,...
                  *  \param style A matlab style string, for colors, line styles,...
                  *  \param nEllipsePoints The number of points in the ellipse to generate
                  */
                std::string MRPTDLLIMPEXP  MATLAB_plotCovariance2D(
                        const CMatrixDouble  &cov22,
                        const CVectorDouble  &mean,
                        const float         &stdCount,
                        const std::string   &style = std::string("b"),
                        const size_t        &nEllipsePoints = 30 );


                /** Efficiently compute the inverse of a 4x4 homogeneous matrix by only transposing the rotation 3x3 part and solving the translation with dot products.
                  */
                void MRPTDLLIMPEXP  homogeneousMatrixInverse(
                        const CMatrixDouble &M,
                        CMatrixDouble &out_inverse_M);

                /** Efficiently compute the inverse of a 4x4 homogeneous matrix by only transposing the rotation 3x3 part and solving the translation with dot products.
                  */
                void MRPTDLLIMPEXP  homogeneousMatrixInverse(
                        const CMatrixDouble44 &M,
                        CMatrixDouble44 &out_inverse_M);

                /** Counts the number of elements that appear in both vectors (comparison through the == operator)
                  *  It is assumed that no repeated elements appear within each of the vectors.
                  */
                template<class T>
                size_t  countCommonElements(
            const std::vector<T> &a,
            const std::vector<T> &b)
                {
                    size_t ret=0;
                        typename std::vector<T>::const_iterator it1;
                        typename std::vector<T>::const_iterator it2;
                        for (it1 = a.begin();it1!=a.end();it1++)
                            for (it2 = b.begin();it2!=b.end();it2++)
                    if ( (*it1) == (*it2) )
                         ret++;

                        return ret;
                }


                /** Estimate the Jacobian of a multi-dimensional function around a point "x", using finite differences of a given size in each input dimension.
                  *  The template argument USERPARAM is for the data can be passed to the functor.
                  *   If it is not required, set to "int" or any other basic type.
                  */
                template <typename T, class USERPARAM >
                void estimateJacobian(
                        const std::vector<T>    &x,
                        void (*functor)(const std::vector<T> &x,const USERPARAM &y, std::vector<T>  &out),
                        const std::vector<T>    &increments,
                        const USERPARAM &userParam,
                        CMatrixTemplateNumeric<T>  &out_Jacobian )
                {
                        MRPT_START;
                        ASSERT_(x.size()>0 && increments.size() == x.size());

                        size_t m = 0;           // will determine automatically on the first call to "f":
                        const size_t n = x.size();

                        for (size_t j=0;j<n;j++) { ASSERT_( increments[j]>0 ) }         // Who knows...

                        std::vector<T>  f_minus, f_plus;
                        std::vector<T>  x_mod(x);

                        // Evaluate the function "i" with increments in the "j" input x variable:
                        for (size_t j=0;j<n;j++)
                        {
                                // Create the modified "x" vector:
                                x_mod[j]+=increments[j];
                                functor(x_mod,userParam,   f_plus);

                                x_mod[j]=x[j]-increments[j];
                                functor(x_mod,userParam,   f_minus);

                                x_mod[j]=x[j]; // Leave as original
                                const T Ax_2_inv = T(0.5)/increments[j];

                                // The first time?
                                if (j==0)
                                {
                                        m = f_plus.size();
                                        out_Jacobian.setSize(m,n);
                                }

                                for (size_t i=0;i<m;i++)
                                {
                                        out_Jacobian(i,j) = Ax_2_inv* (f_plus[i]-f_minus[i]);
                                } // end for i

                        } // end for j

                        MRPT_END;
                }

                /** @name Interpolation functions
                @{ */

                /** Interpolate a data sequence "ys" ranging from "x0" to "x1" (equally spaced), to obtain the approximation of the sequence at the point "x".
                  *  If the point "x" is out of the range [x0,x1], the closest extreme "ys" value is returned.
                  * \sa spline, interpolate2points
                  */
                template <class T>
                T interpolate(
                        const T                                 &x,
                        const std::vector<T>    &ys,
                        const T                                 &x0,
                        const T                                 &x1 )
                {
                        MRPT_START
                        ASSERT_(x1>x0); ASSERT_(!ys.empty());
                        const size_t N = ys.size();
                        if (x<=x0)      return ys[0];
                        if (x>=x1)      return ys[N-1];
                        const T Ax = (x1-x0)/T(N);
                        const size_t i = int( (x-x0)/Ax );
                        if (i>=N-1) return ys[N-1];
                        const T Ay = ys[i+1]-ys[i];
                        return ys[i] + (x-(x0+i*Ax))*Ay/Ax;
                        MRPT_END
                }

                /** Linear interpolation/extrapolation: evaluates at "x" the line (x0,y0)-(x1,y1).
                  *  If wrap2pi is true, output is wrapped to ]-pi,pi] (It is assumed that input "y" values already are in the correct range).
                  * \sa spline, interpolate, leastSquareLinearFit
                  */
                double MRPTDLLIMPEXP interpolate2points(const double x, const double x0, const double y0, const double x1, const double y1, bool wrap2pi = false);

                /** Interpolates the value of a function in a point "t" given 4 SORTED points where "t" is between the two middle points
                  *  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).
                  * \sa leastSquareLinearFit
                  */
                double MRPTDLLIMPEXP  spline(const double t, const std::vector<double> &x, const std::vector<double> &y, bool wrap2pi = false);

                /** Interpolates or extrapolates using a least-square linear fit of the set of values "x" and "y", evaluated at a single point "t".
                  *  The vectors x and y must have size >=2, and all values of "x" must be different.
                  *  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).
                  * \sa spline
                  */
                double MRPTDLLIMPEXP  leastSquareLinearFit(const double t, const std::vector<double> &x, const std::vector<double> &y, bool wrap2pi = false);

                /** 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".
                  *  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).
                  */
                void MRPTDLLIMPEXP  leastSquareLinearFit(const std::vector<double> &ts, std::vector<double> &outs, const std::vector<double> &x, const std::vector<double> &y, bool wrap2pi = false);

                /** @} */

                /** Computes the normalized or normal histogram of a sequence of numbers given the number of bins and the limits.
                  */
                template<class T>
                        vector_double histogram(const std::vector<T> &v, double limit_min, double limit_max, size_t number_bins, bool do_normalization = false )
                {
                        mrpt::math::CHistogram  H( limit_min, limit_max, number_bins );
                        vector_double ret(number_bins);
                        size_t i;
                        for (i=0;i<v.size();i++)        H.add(static_cast<double>( v[i] ));
                        for (i=0;i<number_bins;i++)     ret[i] = do_normalization ? H.getBinRatio(i) : H.getBinCount(i);
                        return ret;
                }

                /** Assignment operator for initializing a std::vector from a C array (The vector will be automatically set to the correct size).
                  * \code
                  *      vector_double  v;
                  *  const double numbers[] = { 1,2,3,5,6,7,8,9,10 };
                  *  loadVector( v, numbers );
                  * \endcode
                  * \note This operator performs the appropiate type castings, if required.
                  */
                template <typename T, typename At, size_t N>
                std::vector<T>& loadVector( std::vector<T> &v, At (&theArray)[N] )
                {
                        MRPT_COMPILE_TIME_ASSERT(N!=0)
                        v.resize(N);
                        for (size_t i=0; i < N; i++)
                                v[i] = static_cast<T>(theArray[i]);
                        return v;
                }

                /** Absolute value of a vector. */
                template <class T>
                std::vector<T>  Abs(const std::vector<T> &a)
                {
                        typename std::vector<T> res(a.size());
                        for (size_t i=0;i<a.size();i++)
                                res[i] =  static_cast<T>( fabs( static_cast<double>( a[i] ) ) );
                        return res;
                }

                /** Modify a sequence of angle values such as no consecutive values have a jump larger than PI in absolute value.
                  * \sa wrapToPi
                  */
                void unwrap2PiSequence(vector_double &x);

                /** @name Probability density distributions (pdf) distance metrics
                @{ */

                /** Computes the squared mahalanobis distance of a vector X given the mean MU and the covariance *inverse* COV_inv
                  *  \f[ d^2 =  (X-MU)^\top \Sigma^{-1} (X-MU)  \f]
                  */
                template<class T>
                T MRPTDLLIMPEXP mahalanobisDistance2(
                        const std::vector<T> &X,
                        const std::vector<T> &MU,
                        const CMatrixTemplateNumeric<T> &COV_inv );

                /** Computes the mahalanobis distance of a vector X given the mean MU and the covariance *inverse* COV_inv
                  *  \f[ d = \sqrt{  (X-MU)^\top \Sigma^{-1} (X-MU) }  \f]
                  */
                template<class T>
                T mahalanobisDistance(
                        const std::vector<T> &X,
                        const std::vector<T> &MU,
                        const CMatrixTemplateNumeric<T> &COV_inv )
                {
                        return std::sqrt( mahalanobisDistance2(X,MU,COV_inv) );
                }


                /** Computes the squared mahalanobis distance between two *non-independent* Gaussians, given the two covariance matrices and the vector with the difference of their means.
                  *  \f[ d^2 = \Delta_\mu^\top (\Sigma_1 + \Sigma_2 - 2 \Sigma_12 )^{-1} \Delta_\mu  \f]
                  */
                template<class T>
                T MRPTDLLIMPEXP mahalanobisDistance2(
                        const std::vector<T> &mean_diffs,
                        const CMatrixTemplateNumeric<T> &COV1,
                        const CMatrixTemplateNumeric<T> &COV2,
                        const CMatrixTemplateNumeric<T> *CROSS_COV12=NULL
                        );


                /** Computes the integral of the product of two Gaussians, with means separated by "mean_diffs" and covariances "COV1" and "COV2".
                  *  \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]
                  */
                template <typename T>
                T productIntegralTwoGaussians(
                        const std::vector<T> &mean_diffs,
                        const CMatrixTemplateNumeric<T> &COV1,
                        const CMatrixTemplateNumeric<T> &COV2
                        )
                {
                        const size_t vector_dim = mean_diffs.size();
                        ASSERT_(vector_dim>=1)

                        CMatrixTemplateNumeric<T> C = COV1;
                        C+= COV2;       // Sum of covs:
                        const T cov_det = C.det();
                        CMatrixTemplateNumeric<T> C_inv;
                        C.inv_fast(C_inv);

                        return std::pow( M_2PI, -0.5*vector_dim ) * (1.0/std::sqrt( cov_det ))
                                * exp( -0.5 * mrpt::math::multiply_HCHt_scalar(mean_diffs,C_inv) );
                }

                /** Computes the integral of the product of two Gaussians, with means separated by "mean_diffs" and covariances "COV1" and "COV2".
                  *  \f[ D = \frac{1}{(2 \pi)^{0.5 N} \sqrt{}  }  \exp( \Delta_\mu^\top (\Sigma_1 + \Sigma_2)^{-1} \Delta_\mu)  \f]
                  */
                template <typename T, size_t DIM>
                T productIntegralTwoGaussians(
                        const std::vector<T> &mean_diffs,
                        const CMatrixFixedNumeric<T,DIM,DIM> &COV1,
                        const CMatrixFixedNumeric<T,DIM,DIM> &COV2
                        )
                {
                        ASSERT_(mean_diffs.size()==DIM);

                        CMatrixFixedNumeric<T,DIM,DIM> C = COV1;
                        C+= COV2;       // Sum of covs:
                        const T cov_det = C.det();
                        CMatrixFixedNumeric<T,DIM,DIM> C_inv(false,false);
                        C.inv_fast(C_inv);

                        return std::pow( M_2PI, -0.5*DIM ) * (1.0/std::sqrt( cov_det ))
                                * exp( -0.5 * mrpt::math::multiply_HCHt_scalar(mean_diffs,C_inv) );
                }

                /** Computes both, the integral of the product of two Gaussians and their Mahalanobis distance.
                  * \sa productIntegralTwoGaussians, mahalanobisDistance2
                  */
                template <typename T>
                void productIntegralAndMahalanobisTwoGaussians(
                        const std::vector<T> &mean_diffs,
                        const CMatrixTemplateNumeric<T> &COV1,
                        const CMatrixTemplateNumeric<T> &COV2,
                        T &maha_out,
                        T &intprod_out,
                        const CMatrixTemplateNumeric<T> *CROSS_COV12=NULL
                        )
                {
                        const size_t vector_dim = mean_diffs.size();
                        ASSERT_(vector_dim>=1)

                        CMatrixTemplateNumeric<T> C = COV1;
                        C+= COV2;       // Sum of covs:
                        if (CROSS_COV12) 
                                C.substract_An(*CROSS_COV12,2);
                        const T cov_det = C.det();
                        CMatrixTemplateNumeric<T> C_inv;
                        C.inv_fast(C_inv);

                        maha_out = mrpt::math::multiply_HCHt_scalar(mean_diffs,C_inv);
                        intprod_out = std::pow( M_2PI, -0.5*vector_dim ) * (1.0/std::sqrt( cov_det ))*exp(-0.5*maha_out);
                }


                /** @} */

                /** Computes covariances and mean of any vector of containers. T is some container of U which allows operator[].
                  * N is the size of the containers, and thus the size of the resulting matrix and array.
                  * This method may adopt additional interfaces in the future.
                  */
                template<typename T,size_t N,typename U>
                void covariancesAndMean(const std::vector<T> &elements,CMatrixTemplateNumeric<U> &covariances,U (&means)[N])    {
                        size_t nElms=elements.size();
                        for (size_t i=0;i<N;i++)        {
                                means[i]=0;
                                for (size_t j=0;j<nElms;j++) means[i]+=elements[j][i];
                                means[i]/=nElms;
                        }
                        covariances.resize(N,N);
                        for (size_t i=0;i<N;i++) for (size_t j=0;j<=i;j++)      {
                                U elem=0;
                                for (size_t k=0;k<nElms;k++) elem+=(elements[k][i]-means[i])*(elements[k][j]-means[j]);
                                elem/=nElms;
                                covariances.get_unsafe(i,j) = elem;
                                if (i!=j) covariances.get_unsafe(j,i)=elem;
                        }
                }

                /** Computes covariances and mean of any vector of containers. T is some container of U which allows operator[].
                  * N is the size of the containers, and thus the size of the resulting matrix and array.
                  * This method may adopt additional interfaces in the future.
                  */
                template<typename T,size_t N,typename U>
                void covariancesAndMean(const std::vector<T> &elements,CMatrixFixedNumeric<U,N,N> &covariances,U (&means)[N])   {
                        size_t nElms=elements.size();
                        for (size_t i=0;i<N;i++)        {
                                means[i]=0;
                                for (size_t j=0;j<nElms;j++) means[i]+=elements[j][i];
                                means[i]/=nElms;
                        }
                        for (size_t i=0;i<N;i++) for (size_t j=0;j<=i;j++)      {
                                U elem=0;
                                for (size_t k=0;k<nElms;k++) elem+=(elements[k][i]-means[i])*(elements[k][j]-means[j]);
                                elem/=nElms;
                                covariances.get_unsafe(i,j) = elem;
                                if (i!=j) covariances.get_unsafe(j,i)=elem;
                        }
                }

                /** Computes covariances and mean of any vector of containers. T is some container of U which allows operator[].
                  * N is the size of the containers, and thus the size of the resulting matrix and array.
                  * This method may adopt additional interfaces in the future.
                  */
                template<typename T,size_t N,typename U>
                void covariancesAndMean(const std::vector<T> &elements,CMatrixFixedNumeric<U,N,N> &covariances,std::vector<U> &means )  {
                        means.resize(N);
                        size_t nElms=elements.size();
                        for (size_t i=0;i<N;i++)        {
                                means[i]=0;
                                for (size_t j=0;j<nElms;j++) means[i]+=elements[j][i];
                                means[i]/=nElms;
                        }
                        for (size_t i=0;i<N;i++) for (size_t j=0;j<=i;j++)      {
                                U elem=0;
                                for (size_t k=0;k<nElms;k++) elem+=(elements[k][i]-means[i])*(elements[k][j]-means[j]);
                                elem/=nElms;
                                covariances.get_unsafe(i,j) = elem;
                                if (i!=j) covariances.get_unsafe(j,i)=elem;
                        }
                }

                /** The dot product of two vectors v1·v2 */
                template<size_t N,class T,class U,class V>
                inline T dotProduct(const U &v1,const V &v2)    {
                        T res=0;
                        for (size_t i=0;i<N;i++) res+=v1[i]*v2[i];
                        return res;
                }

                /** Square norm of a vector of the given length N */
                template<size_t N,class T,class U>
                inline T squareNorm(const U &v) {
                        T res=0;
                        for (size_t i=0;i<N;i++) res+=v[i]*v[i];
                        return res;
                }

                /** A versatile template to build vectors on-the-fly in a style close to MATLAB's  v=[a b c d ...]
                  *  The first argument of the template is the vector length, and the second the type of the numbers.
                  *  Some examples:
                  *
                  *  \code
                  *    vector_double  = make_vector<4,double>(1.0,3.0,4.0,5.0);
                  *    vector_float   = make_vector<2,float>(-8.12, 3e4);
                  *  \endcode
                  */
                template <size_t N, typename T>
                std::vector<T> make_vector(const T val1, ...)
                {
                        MRPT_COMPILE_TIME_ASSERT( N>0 )
                        std::vector<T>  ret;
                        ret.reserve(N);

                        ret.push_back(val1);

                        va_list args;
                        va_start(args,val1);
                        for (size_t i=0;i<N-1;i++)
                                ret.push_back( va_arg(args,T) );

                        va_end(args);
                        return ret;
                }

        } // End of MATH namespace

} // End of namespace

#endif

---

Page generated by Doxygen 1.6.2 for MRPT 0.8.1 SVN:exported at Mon Feb 15 22:01:07 UTC 2010