mirror of
https://github.com/introlab/rtabmap_ros.git
synced 2026-10-04 00:37:46 +08:00
940 lines
20 KiB
C++
940 lines
20 KiB
C++
/*
|
|
* 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
|
|
|
|
/** \file UMath.h
|
|
\brief Basic mathematics functions
|
|
*/
|
|
|
|
#include "rtabmap/utilite/UtiLiteExp.h" // DLL export/import defines
|
|
|
|
#include <cmath>
|
|
#include <list>
|
|
#include <vector>
|
|
|
|
#if _MSC_VER
|
|
#undef min
|
|
#undef max
|
|
#endif
|
|
|
|
/**
|
|
* Return true if the number is NAN.
|
|
*/
|
|
template<class T>
|
|
inline bool uIsNan(const T & value)
|
|
{
|
|
#if _MSC_VER
|
|
return _isnan(value) != 0;
|
|
#else
|
|
return std::isnan(value);
|
|
#endif
|
|
}
|
|
|
|
/**
|
|
* Return true if the number is finite.
|
|
*/
|
|
template<class T>
|
|
inline bool uIsFinite(const T & value)
|
|
{
|
|
#if _MSC_VER
|
|
return _finite(value) != 0;
|
|
#else
|
|
return std::isfinite(value);
|
|
#endif
|
|
}
|
|
|
|
/**
|
|
* Get the minimum of the 3 values.
|
|
* @return the minimum value
|
|
*/
|
|
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;
|
|
}
|
|
|
|
/**
|
|
* Get the maximum of the 3 values.
|
|
* @return the maximum value
|
|
*/
|
|
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;
|
|
}
|
|
|
|
/**
|
|
* Get the maximum of a vector.
|
|
* @param v the array
|
|
* @param size the size of the array
|
|
* @param index the index of the maximum value in the vector.
|
|
* @return the maximum value of the array
|
|
*/
|
|
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;
|
|
}
|
|
|
|
/**
|
|
* Get the maximum of a vector.
|
|
* @param v the array
|
|
* @param index the index of the maximum value in the vector.
|
|
* @return the maximum value of the array
|
|
*/
|
|
template<class T>
|
|
inline T uMax(const std::vector<T> & v, unsigned int & index)
|
|
{
|
|
return uMax(v.data(), v->size(), index);
|
|
}
|
|
|
|
/**
|
|
* Get the maximum of a vector.
|
|
* @param v the array
|
|
* @param size the size of the array
|
|
* @return the maximum value of the array
|
|
*/
|
|
template<class T>
|
|
inline T uMax(const T * v, unsigned int size)
|
|
{
|
|
unsigned int index;
|
|
return uMax(v, size, index);
|
|
}
|
|
|
|
/**
|
|
* Get the maximum of a vector.
|
|
* @param v the array
|
|
* @return the maximum value of the array
|
|
*/
|
|
template<class T>
|
|
inline T uMax(const std::vector<T> & v)
|
|
{
|
|
return uMax(v.data(), v.size());
|
|
}
|
|
|
|
/**
|
|
* Get the minimum of a vector.
|
|
* @param v the array
|
|
* @param size the size of the array
|
|
* @param index the index of the minimum value in the vector.
|
|
* @return the minimum value of the array
|
|
*/
|
|
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;
|
|
}
|
|
|
|
/**
|
|
* Get the minimum of a vector.
|
|
* @param v the array
|
|
* @param index the index of the minimum value in the vector.
|
|
* @return the minimum value of the array
|
|
*/
|
|
template<class T>
|
|
inline T uMin(const std::vector<T> & v, unsigned int & index)
|
|
{
|
|
return uMin(v.data(), v.size(), index);
|
|
}
|
|
|
|
/**
|
|
* Get the minimum of a vector.
|
|
* @param v the array
|
|
* @param size the size of the array
|
|
* @return the minimum value of the array
|
|
*/
|
|
template<class T>
|
|
inline T uMin(const T * v, unsigned int size)
|
|
{
|
|
unsigned int index;
|
|
return uMin(v, size, index);
|
|
}
|
|
|
|
/**
|
|
* Get the minimum of a vector.
|
|
* @param v the array
|
|
* @return the minimum value of the array
|
|
*/
|
|
template<class T>
|
|
inline T uMin(const std::vector<T> & v)
|
|
{
|
|
return uMin(v.data(), v.size());
|
|
}
|
|
|
|
/**
|
|
* Get the minimum and maximum of a vector.
|
|
* @param v the array
|
|
* @param size the size of the array
|
|
* @param min reference to output minimum
|
|
* @param max reference to output maximum
|
|
* @param min reference to output minimum index
|
|
* @param max reference to output maximum index
|
|
*/
|
|
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;
|
|
}
|
|
}
|
|
}
|
|
|
|
/**
|
|
* Get the minimum and maximum of a vector.
|
|
* @param v the array
|
|
* @param min reference to output minimum
|
|
* @param max reference to output maximum
|
|
* @param min reference to output minimum index
|
|
* @param max reference to output maximum index
|
|
*/
|
|
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);
|
|
}
|
|
|
|
/**
|
|
* Get the minimum and maximum of a vector.
|
|
* @param v the array
|
|
* @param size the size of the array
|
|
* @param min reference to output minimum
|
|
* @param max reference to output maximum
|
|
*/
|
|
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);
|
|
}
|
|
|
|
/**
|
|
* Get the minimum and maximum of a vector.
|
|
* @param v the array
|
|
* @param min reference to output minimum
|
|
* @param max reference to output maximum
|
|
*/
|
|
template<class T>
|
|
inline void uMinMax(const std::vector<T> & v, T & min, T & max)
|
|
{
|
|
uMinMax(v.data(), v.size(), min, max);
|
|
}
|
|
|
|
/**
|
|
* Get the sign of value.
|
|
* @param v the value
|
|
* @return -1 if v<0, otherwise 1
|
|
*/
|
|
template<class T>
|
|
inline int uSign(const T & v)
|
|
{
|
|
if(v < 0)
|
|
{
|
|
return -1;
|
|
}
|
|
else
|
|
{
|
|
return 1;
|
|
}
|
|
}
|
|
|
|
/**
|
|
* Get the sum of all values contained in a list. Provided for convenience.
|
|
* @param list the list
|
|
* @return the sum of values of the list
|
|
*/
|
|
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;
|
|
}
|
|
|
|
/**
|
|
* Get the sum of all values contained in an array: sum(x).
|
|
* @param v the array
|
|
* @param size the size of the array
|
|
* @return the sum of values of the array
|
|
*/
|
|
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;
|
|
}
|
|
|
|
/**
|
|
* Get the sum of all values contained in a vector. Provided for convenience.
|
|
* @param v the vector
|
|
* @return the sum of values of the vector
|
|
*/
|
|
template<class T>
|
|
inline T uSum(const std::vector<T> & v)
|
|
{
|
|
return uSum(v.data(), (int)v.size());
|
|
}
|
|
|
|
/**
|
|
* Get the sum of all squared values contained in an array: sum(x.^2).
|
|
* @param v the array
|
|
* @param size the size of the array
|
|
* @param subtract an optional value to remove to v before squaring v: sum((x-sub).^2)
|
|
* @return the sum of values of the array
|
|
*/
|
|
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;
|
|
}
|
|
|
|
/**
|
|
* Get the sum of all squared values contained in an array: sum(x.^2).
|
|
* @param v the array
|
|
* @param subtract an optional value to remove to v before squaring v: sum((x-sub).^2)
|
|
* @return the sum of values of the array
|
|
*/
|
|
template<class T>
|
|
inline T uSumSquared(const std::vector<T> & v, T subtract = T())
|
|
{
|
|
return uSumSquared(v.data(), v.size(), subtract);
|
|
}
|
|
|
|
/**
|
|
* Compute the mean of an array.
|
|
* @param v the array
|
|
* @param size the size of the array
|
|
* @return the mean
|
|
*/
|
|
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;
|
|
}
|
|
|
|
/**
|
|
* Get the mean of a list. Provided for convenience.
|
|
* @param list the list
|
|
* @return the mean
|
|
*/
|
|
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;
|
|
}
|
|
|
|
/**
|
|
* Get the mean of a vector. Provided for convenience.
|
|
* @param v the vector
|
|
* @return the mean
|
|
*/
|
|
template<class T>
|
|
inline T uMean(const std::vector<T> & v)
|
|
{
|
|
return uMean(v.data(), v.size());
|
|
}
|
|
|
|
/**
|
|
* Compute mean squared error between two arrays: mean((x-y).^2).
|
|
* @param x the first array
|
|
* @param sizeX the size of the array x
|
|
* @param y the second array
|
|
* @param sizeY the size of the array y (must be same size as x)
|
|
* @return the mean squared error (return -1 if error cannot be computed)
|
|
*/
|
|
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;
|
|
}
|
|
|
|
/**
|
|
* Compute mean squared error between two arrays: mean((x-y).^2).
|
|
* @param x the first array
|
|
* @param y the second array (must be same size as x)
|
|
* @return the mean squared error (return -1 if error cannot be computed)
|
|
*/
|
|
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());
|
|
}
|
|
|
|
/**
|
|
* Compute the variance of an array.
|
|
* @param v the array
|
|
* @param size the size of the array
|
|
* @param meanV the mean of the array
|
|
* @return the variance
|
|
* @see mean()
|
|
*/
|
|
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;
|
|
}
|
|
|
|
/**
|
|
* Get the variance of a list. Provided for convenience.
|
|
* @param list the list
|
|
* @param m the mean of the list
|
|
* @return the variance
|
|
* @see mean()
|
|
*/
|
|
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;
|
|
}
|
|
|
|
/**
|
|
* Compute the variance of an array.
|
|
* @param v the array
|
|
* @param size the size of the array
|
|
* @return the variance
|
|
*/
|
|
template<class T>
|
|
inline T uVariance(const T * v, unsigned int size)
|
|
{
|
|
T m = uMean(v, size);
|
|
return uVariance(v, size, m);
|
|
}
|
|
|
|
/**
|
|
* Get the variance of a vector. Provided for convenience.
|
|
* @param v the vector
|
|
* @param m the mean of the vector
|
|
* @return the variance
|
|
* @see mean()
|
|
*/
|
|
template<class T>
|
|
inline T uVariance(const std::vector<T> & v, const T & m)
|
|
{
|
|
return uVariance(v.data(), v.size(), m);
|
|
}
|
|
|
|
/**
|
|
* Get the squared norm of the vector : return x1*x1 + x2*x2 + x3*x3 + ...
|
|
* @return the squared norm of the vector
|
|
*/
|
|
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;
|
|
}
|
|
|
|
/**
|
|
* Get the norm of the vector : return sqrt(x1*x1 + x2*x2 + x3*x3 + ...)
|
|
* @return the norm of the vector
|
|
*/
|
|
template<class T>
|
|
inline T uNorm(const std::vector<T> & v)
|
|
{
|
|
return std::sqrt(uNormSquared(v));
|
|
}
|
|
|
|
/**
|
|
* Get the squared norm of the vector : return x1*x1 + x2*x2
|
|
* @return the squared norm of the vector
|
|
*/
|
|
template<class T>
|
|
inline T uNormSquared(const T & x1, const T & x2)
|
|
{
|
|
return x1*x1 + x2*x2;
|
|
}
|
|
|
|
/**
|
|
* Get the norm of the vector : return sqrt(x1*x1 + x2*x2 + x3*x3)
|
|
* @return the norm of the vector
|
|
*/
|
|
template<class T>
|
|
inline T uNorm(const T & x1, const T & x2)
|
|
{
|
|
return std::sqrt(uNormSquared(x1, x2));
|
|
}
|
|
|
|
/**
|
|
* Get the squared norm of the vector : return x1*x1 + x2*x2 + x3*x3
|
|
* @return the squared norm of the vector
|
|
*/
|
|
template<class T>
|
|
inline T uNormSquared(const T & x1, const T & x2, const T & x3)
|
|
{
|
|
return x1*x1 + x2*x2 + x3*x3;
|
|
}
|
|
|
|
/**
|
|
* Get the norm of the vector : return sqrt(x1*x1 + x2*x2 + x3*x3)
|
|
* @return the norm of the vector
|
|
*/
|
|
template<class T>
|
|
inline T uNorm(const T & x1, const T & x2, const T & x3)
|
|
{
|
|
return std::sqrt(uNormSquared(x1, x2, x3));
|
|
}
|
|
|
|
/**
|
|
* Normalize the vector : [x1 x2 x3 ...] ./ uNorm([x1 x2 x3 ...])
|
|
* @return the vector normalized
|
|
*/
|
|
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;
|
|
}
|
|
}
|
|
|
|
/**
|
|
* Find all local maxima.
|
|
*/
|
|
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;
|
|
}
|
|
|
|
/**
|
|
* Find all local maxima.
|
|
*/
|
|
template<class T>
|
|
inline std::list<unsigned int> uLocalMaxima(const std::vector<T> & v)
|
|
{
|
|
return uLocalMaxima(v.data(), v.size());
|
|
}
|
|
|
|
/**
|
|
* Enum of cross matching methods (cross-correlation, cross-covariance) :
|
|
* UXCorrRaw, UXCorrBiased, UXCorrUnbiased, UXCorrCoeff, UXCovRaw, UXCovBiased, UXCovUnbiased, UXCovCoeff.
|
|
*/
|
|
enum UXMatchMethod{UXCorrRaw, UXCorrBiased, UXCorrUnbiased, UXCorrCoeff, UXCovRaw, UXCovBiased, UXCovUnbiased, UXCovCoeff};
|
|
|
|
/**
|
|
* Do a full cross-correlation or cross-covariance between 2 arrays.
|
|
* @param vA the first array
|
|
* @param vB the second array
|
|
* @param sizeA the size of the first array
|
|
* @param sizeB the size of the second array
|
|
* @param method see UXMatchMethod
|
|
* @return the resulting correlation/covariance vector of size = sizeA + sizeB - 1
|
|
*/
|
|
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;
|
|
}
|
|
|
|
/**
|
|
* Do a full cross-correlation or cross-covariance between 2 arrays.
|
|
* @param vA the first array
|
|
* @param vB the second array
|
|
* @param method see UXMatchMethod
|
|
* @return the resulting correlation/covariance vector of size = sizeA + sizeB - 1
|
|
*/
|
|
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);
|
|
}
|
|
|
|
/**
|
|
* Do a cross correlation between 2 arrays at a specified index.
|
|
* @param vA the first array
|
|
* @param vB the second array
|
|
* @param sizeA the size of the first array
|
|
* @param sizeB the size of the second array
|
|
* @param index the index to correlate
|
|
* @param method see UXMatchMethod
|
|
* @return the resulting correlation value
|
|
*/
|
|
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;
|
|
}
|
|
|
|
/**
|
|
* Do a cross correlation between 2 arrays at a specified index.
|
|
* @param vA the first array
|
|
* @param vB the second array
|
|
* @param sizeA the size of the first array
|
|
* @param sizeB the size of the second array
|
|
* @param index the index to correlate
|
|
* @param method see UXMatchMethod
|
|
* @return the resulting correlation value
|
|
*/
|
|
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);
|
|
}
|
|
|
|
/**
|
|
* Return Hamming window of length L.
|
|
* @param L the window length
|
|
* @return the Hamming window (values are between 0 and 1)
|
|
*/
|
|
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
|