UMath.h

Go to the documentation of this file.

/*
*  utilite is a cross-platform library with
*  useful utilities for fast and small developing.
*  Copyright (C) 2010  Mathieu Labbe
*
*  utilite is free library: you can redistribute it and/or modify
*  it under the terms of the GNU Lesser General Public License as published by
*  the Free Software Foundation, either version 3 of the License, or
*  (at your option) any later version.
*
*  utilite 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 Lesser General Public License for more details.
*
*  You should have received a copy of the GNU Lesser General Public License
*  along with this program.  If not, see <http://www.gnu.org/licenses/>.
*/

#ifndef UMATH_H
#define UMATH_H

#include "rtabmap/utilite/UtiLiteExp.h" // DLL export/import defines

#include <cmath>
#include <list>
#include <vector>

#if _MSC_VER
        #undef min
        #undef max
#endif

template<class T>
inline bool uIsNan(const T & value)
{
#if _MSC_VER
        return _isnan(value) != 0;
#else
        return std::isnan(value);
#endif
}

template<class T>
inline bool uIsFinite(const T & value)
{
#if _MSC_VER
        return _finite(value) != 0;
#else
        return std::isfinite(value);
#endif
}

template<class T>
inline T uMin3( const T& a, const T& b, const T& c)
{
        float m=a<b?a:b;
        return m<c?m:c;
}

template<class T>
inline T uMax3( const T& a, const T& b, const T& c)
{
        float m=a>b?a:b;
        return m>c?m:c;
}

template<class T>
inline T uMax(const T * v, unsigned int size, unsigned int & index)
{
        T max = 0;
        index = 0;
        if(!v || size == 0)
        {
                return max;
        }
        max = v[0];
        for(unsigned int i=1; i<size; ++i)
        {
                if(uIsNan(max) || (max < v[i] && !uIsNan(v[i])))
                {
                        max = v[i];
                        index = i;
                }
        }

        return max;
}

template<class T>
inline T uMax(const std::vector<T> & v, unsigned int & index)
{
        return uMax(v.data(), v->size(), index);
}

template<class T>
inline T uMax(const T * v, unsigned int size)
{
        unsigned int index;
        return uMax(v, size, index);
}

template<class T>
inline T uMax(const std::vector<T> & v)
{
        return uMax(v.data(), v.size());
}

template<class T>
inline T uMin(const T * v, unsigned int size, unsigned int & index)
{
        T min = 0;
        index = 0;
        if(!v || size == 0)
        {
                return min;
        }
        min = v[0];
        for(unsigned int i=1; i<size; ++i)
        {
                if(uIsNan(min) || (min > v[i] && !uIsNan(v[i])))
                {
                        min = v[i];
                        index = i;
                }
        }

        return min;
}

template<class T>
inline T uMin(const std::vector<T> & v, unsigned int & index)
{
        return uMin(v.data(), v.size(), index);
}

template<class T>
inline T uMin(const T * v, unsigned int size)
{
        unsigned int index;
        return uMin(v, size, index);
}

template<class T>
inline T uMin(const std::vector<T> & v)
{
        return uMin(v.data(), v.size());
}

template<class T>
inline void uMinMax(const T * v, unsigned int size, T & min, T & max, unsigned int & indexMin, unsigned int & indexMax)
{
        min = 0;
        max = 0;
        indexMin = 0;
        indexMax = 0;
        if(!v || size == 0)
        {
                return;
        }
        min = v[0];
        max = v[0];

        for(unsigned int i=1; i<size; ++i)
        {
                if(uIsNan(min) || (min > v[i] && !uIsNan(v[i])))
                {
                        min = v[i];
                        indexMin = i;
                }

                if(uIsNan(max) || (max < v[i] && !uIsNan(v[i])))
                {
                        max = v[i];
                        indexMax = i;
                }
        }
}

template<class T>
inline void uMinMax(const std::vector<T> & v, T & min, T & max, unsigned int & indexMin, unsigned int & indexMax)
{
        uMinMax(v.data(), v.size(), min, max, indexMin, indexMax);
}

template<class T>
inline void uMinMax(const T * v, unsigned int size, T & min, T & max)
{
        unsigned int indexMin;
        unsigned int indexMax;
        uMinMax(v, size, min, max, indexMin, indexMax);
}

template<class T>
inline void uMinMax(const std::vector<T> & v, T & min, T & max)
{
        uMinMax(v.data(), v.size(), min, max);
}

template<class T>
inline int uSign(const T & v)
{
        if(v < 0)
        {
                return -1;
        }
        else
        {
                return 1;
        }
}

template<class T>
inline T uSum(const std::list<T> & list)
{
        T sum = 0;
        for(typename std::list<T>::const_iterator i=list.begin(); i!=list.end(); ++i)
        {
                sum += *i;
        }
        return sum;
}

template<class T>
inline T uSum(const T * v, unsigned int size)
{
        T sum = 0;
        if(v && size)
        {
                for(unsigned int i=0; i<size; ++i)
                {
                        sum += v[i];
                }
        }
        return sum;
}

template<class T>
inline T uSum(const std::vector<T> & v)
{
        return uSum(v.data(), (int)v.size());
}

template<class T>
inline T uSumSquared(const T * v, unsigned int size, T subtract = T())
{
        T sum = 0;
        if(v && size)
        {
                for(unsigned int i=0; i<size; ++i)
                {
                        sum += (v[i]-subtract)*(v[i]-subtract);
                }
        }
        return sum;
}

template<class T>
inline T uSumSquared(const std::vector<T> & v, T subtract = T())
{
        return uSumSquared(v.data(), v.size(), subtract);
}

template<class T>
inline T uMean(const T * v, unsigned int size)
{
        T buf = 0;
        if(v && size)
        {
                for(unsigned int i=0; i<size; ++i)
                {
                        buf += v[i];
                }
                buf /= size;
        }
        return buf;
}

template<class T>
inline T uMean(const std::list<T> & list)
{
        T m = 0;
        if(list.size())
        {
                for(typename std::list<T>::const_iterator i=list.begin(); i!=list.end(); ++i)
                {
                        m += *i;
                }
                m /= list.size();
        }
        return m;
}

template<class T>
inline T uMean(const std::vector<T> & v)
{
        return uMean(v.data(), v.size());
}

template<class T>
inline T uMeanSquaredError(const T * x, unsigned int sizeX, const T * y, unsigned int sizeY)
{
        T sum = 0;
        if(x && y && sizeX == sizeY)
        {
                for(unsigned int i=0; i<sizeX; ++i)
                {
                        T diff = x[i]-y[i];
                        sum += diff*diff;
                }
                return sum/(T)sizeX;
        }
        return (T)-1;
}

template<class T>
inline T uMeanSquaredError(const std::vector<T> & x, const std::vector<T> & y)
{
        return uMeanSquaredError(x.data(), x.size(), y.data(), y.size());
}

template<class T>
inline T uVariance(const T * v, unsigned int size, T meanV)
{
        T buf = 0;
        if(v && size>1)
        {
                float sum = 0;
                for(unsigned int i=0; i<size; ++i)
                {
                        sum += (v[i]-meanV)*(v[i]-meanV);
                }
                buf = sum/(size-1);
        }
        return buf;
}

template<class T>
inline T uVariance(const std::list<T> & list, const T & m)
{
        T buf = 0;
        if(list.size()>1)
        {
                float sum = 0;
                for(typename std::list<T>::const_iterator i=list.begin(); i!=list.end(); ++i)
                {
                        sum += (*i-m)*(*i-m);
                }
                buf = sum/(list.size()-1);
        }
        return buf;
}

template<class T>
inline T uVariance(const T * v, unsigned int size)
{
        T m = uMean(v, size);
        return uVariance(v, size, m);
}

template<class T>
inline T uVariance(const std::vector<T> & v, const T & m)
{
        return uVariance(v.data(), v.size(), m);
}

template<class T>
inline T uNormSquared(const std::vector<T> & v)
{
        float sum = 0.0f;
        for(unsigned int i=0; i<v.size(); ++i)
        {
                sum += v[i]*v[i];
        }
        return sum;
}

template<class T>
inline T uNorm(const std::vector<T> & v)
{
        return std::sqrt(uNormSquared(v));
}

template<class T>
inline T uNormSquared(const T & x1, const T & x2)
{
        return x1*x1 + x2*x2;
}

template<class T>
inline T uNorm(const T & x1, const T & x2)
{
        return std::sqrt(uNormSquared(x1, x2));
}

template<class T>
inline T uNormSquared(const T & x1, const T & x2, const T & x3)
{
        return x1*x1 + x2*x2 + x3*x3;
}

template<class T>
inline T uNorm(const T & x1, const T & x2, const T & x3)
{
        return std::sqrt(uNormSquared(x1, x2, x3));
}

template<class T>
inline std::vector<T> uNormalize(const std::vector<T> & v)
{
        float norm = uNorm(v);
        if(norm == 0)
        {
                return v;
        }
        else
        {
                std::vector<T> r(v.size());
                for(unsigned int i=0; i<v.size(); ++i)
                {
                        r[i] = v[i]/norm;
                }
                return r;
        }
}

template<class T>
inline std::list<unsigned int> uLocalMaxima(const T * v, unsigned int size)
{
        std::list<unsigned int> maxima;
        if(size)
        {
                for(unsigned int i=0; i<size; ++i)
                {
                        if(i == 0)
                        {
                                // first item
                                if((i+1 < size && v[i] > v[i+1]) ||
                                        i+1 >= size)
                                {
                                        maxima.push_back(i);
                                }
                        }
                        else if(i == size - 1)
                        {
                                //last item
                                if((i >= 1 && v[i] > v[i-1]) ||
                                        i == 0)
                                {
                                        maxima.push_back(i);
                                }
                        }
                        else
                        {
                                //all others, check previous and next
                                if(v[i] > v[i-1] && v[i] > v[i+1])
                                {
                                        maxima.push_back(i);
                                }
                        }
                }
        }
        return maxima;
}

template<class T>
inline std::list<unsigned int> uLocalMaxima(const std::vector<T> & v)
{
        return uLocalMaxima(v.data(), v.size());
}

enum UXMatchMethod{UXCorrRaw, UXCorrBiased, UXCorrUnbiased, UXCorrCoeff, UXCovRaw, UXCovBiased, UXCovUnbiased, UXCovCoeff};

template<class T>
inline std::vector<T> uXMatch(const T * vA, const T * vB, unsigned int sizeA, unsigned int sizeB, UXMatchMethod method)
{
        if(!vA || !vB || sizeA == 0 || sizeB == 0)
        {
                return std::vector<T>();
        }

        std::vector<T> result(sizeA + sizeB - 1);

        T meanA = 0;
        T meanB = 0;
        if(method > UXCorrCoeff)
        {
                meanA = uMean(vA, sizeA);
                meanB = uMean(vB, sizeB);
        }

        T den = 1;
        if(method == UXCorrCoeff || method == UXCovCoeff)
        {
                den = std::sqrt(uSumSquared(vA, sizeA, meanA) * uSumSquared(vB, sizeB, meanB));
        }
        else if(method == UXCorrBiased || method == UXCovBiased)
        {
                den = (T)std::max(sizeA, sizeB);
        }

        if(sizeA == sizeB)
        {
                T resultA;
                T resultB;

                int posA;
                int posB;
                unsigned int j;

                // Optimization, filling two results at once
                for(unsigned int i=0; i<sizeA; ++i)
                {
                        if(method == UXCorrUnbiased || method == UXCovUnbiased)
                        {
                                den = 0;
                        }

                        posA = sizeA - i - 1;
                        posB = sizeB - i - 1;
                        resultA = 0;
                        resultB = 0;
                        for(j=0; (j + posB) < sizeB && (j + posA) < sizeA; ++j)
                        {
                                resultA += (vA[j] - meanA) * (vB[j + posB] - meanB);
                                resultB += (vA[j + posA] - meanA) * (vB[j] - meanB);
                                if(method == UXCorrUnbiased || method == UXCovUnbiased)
                                {
                                        ++den;
                                }
                        }

                        result[i] = resultA / den;
                        result[result.size()-1 -i] = resultB / den;
                }
        }
        else
        {
                for(unsigned int i=0; i<result.size(); ++i)
                {
                        if(method == UXCorrUnbiased || method == UXCovUnbiased)
                        {
                                den = 0;
                        }

                        int posB = sizeB - i - 1;
                        T r = 0;
                        if(posB >= 0)
                        {
                                for(unsigned int j=0; (j + posB) < sizeB && j < sizeA; ++j)
                                {
                                        r += (vA[j] - meanA) * (vB[j + posB] - meanB);
                                        if(method == UXCorrUnbiased || method == UXCovUnbiased)
                                        {
                                                ++den;
                                        }
                                }
                        }
                        else
                        {
                                int posA = posB*-1;
                                for(unsigned int i=0; (i+posA) < sizeA && i < sizeB; ++i)
                                {
                                        r += (vA[i+posA] - meanA) * (vB[i] - meanB);
                                        if(method == UXCorrUnbiased || method == UXCovUnbiased)
                                        {
                                                ++den;
                                        }
                                }
                        }

                        result[i] = r / den;
                }
        }

        return result;
}

template<class T>
inline std::vector<T> uXMatch(const std::vector<T> & vA, const std::vector<T> & vB, UXMatchMethod method)
{
        return uXMatch(vA.data(), vB.data(), vA.size(), vB.size(), method);
}

template<class T>
inline T uXMatch(const T * vA, const T * vB, unsigned int sizeA, unsigned int sizeB, unsigned int index, UXMatchMethod method)
{
        T result = 0;
        if(!vA || !vB || sizeA == 0 || sizeB == 0)
        {
                return result;
        }

        T meanA = 0;
        T meanB = 0;
        if(method > UXCorrCoeff)
        {
                meanA = uMean(vA, sizeA);
                meanB = uMean(vB, sizeB);
        }
        unsigned int size = sizeA + sizeB - 1;

        T den = 1;
        if(method == UXCorrCoeff || method == UXCovCoeff)
        {
                den = std::sqrt(uSumSquared(vA, sizeA, meanA) * uSumSquared(vB, sizeB, meanB));
        }
        else if(method == UXCorrBiased || method == UXCovBiased)
        {
                den = (T)std::max(sizeA, sizeB);
        }
        else if(method == UXCorrUnbiased || method == UXCovUnbiased)
        {
                den = 0;
        }

        if(index < size)
        {
                int posB = sizeB - index - 1;
                unsigned int i;
                if(posB >= 0)
                {
                        for(i=0; (i + posB) < sizeB && i < sizeA; ++i)
                        {
                                result += (vA[i] - meanA) * (vB[i + posB] - meanB);
                                if(method == UXCorrUnbiased || method == UXCovUnbiased)
                                {
                                        ++den;
                                }
                        }
                }
                else
                {
                        int posA = posB*-1;
                        for(i=0; (i+posA) < sizeA && i < sizeB; ++i)
                        {
                                result += (vA[i+posA] - meanA) * (vB[i] - meanB);
                                if(method == UXCorrUnbiased || method == UXCovUnbiased)
                                {
                                        ++den;
                                }
                        }
                }
        }
        return result / den;
}

template<class T>
inline T uXMatch(const std::vector<T> & vA, const std::vector<T> & vB, unsigned int index, UXMatchMethod method)
{
        return uXMatch(vA.data(), vB.data(), vA.size(), vB.size(), index, method);
}

inline std::vector<float> uHamming(unsigned int L)
{
        std::vector<float> w(L);
        unsigned int N = L-1;
        float pi = 3.14159265f;
        for(unsigned int n=0; n<N; ++n)
        {
                w[n] = 0.54f-0.46f*std::cos(2.0f*pi*float(n)/float(N));
        }
        return w;
}

template <typename T>
bool uIsInBounds(const T& value, const T& low, const T& high)
{
        return uIsFinite(value) && !(value < low) && !(value >= high);
}

#endif // UMATH_H