UMath.h
Go to the documentation of this file.
00001 /*
00002 *  utilite is a cross-platform library with
00003 *  useful utilities for fast and small developing.
00004 *  Copyright (C) 2010  Mathieu Labbe
00005 *
00006 *  utilite is free library: you can redistribute it and/or modify
00007 *  it under the terms of the GNU Lesser General Public License as published by
00008 *  the Free Software Foundation, either version 3 of the License, or
00009 *  (at your option) any later version.
00010 *
00011 *  utilite is distributed in the hope that it will be useful,
00012 *  but WITHOUT ANY WARRANTY; without even the implied warranty of
00013 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00014 *  GNU Lesser General Public License for more details.
00015 *
00016 *  You should have received a copy of the GNU Lesser General Public License
00017 *  along with this program.  If not, see <http://www.gnu.org/licenses/>.
00018 */
00019 
00020 #ifndef UMATH_H
00021 #define UMATH_H
00022 
00027 #include "rtabmap/utilite/UtiLiteExp.h" // DLL export/import defines
00028 
00029 #include <cmath>
00030 #include <list>
00031 #include <vector>
00032 
00033 #if _MSC_VER
00034         #undef min
00035         #undef max
00036 #endif
00037 
00041 template<class T>
00042 inline bool uIsNan(const T & value)
00043 {
00044 #if _MSC_VER
00045         return _isnan(value) != 0;
00046 #else
00047         return std::isnan(value);
00048 #endif
00049 }
00050 
00054 template<class T>
00055 inline bool uIsFinite(const T & value)
00056 {
00057 #if _MSC_VER
00058         return _finite(value) != 0;
00059 #else
00060         return std::isfinite(value);
00061 #endif
00062 }
00063 
00068 template<class T>
00069 inline T uMin3( const T& a, const T& b, const T& c)
00070 {
00071         float m=a<b?a:b;
00072         return m<c?m:c;
00073 }
00074 
00079 template<class T>
00080 inline T uMax3( const T& a, const T& b, const T& c)
00081 {
00082         float m=a>b?a:b;
00083         return m>c?m:c;
00084 }
00085 
00093 template<class T>
00094 inline T uMax(const T * v, unsigned int size, unsigned int & index)
00095 {
00096         T max = 0;
00097         index = 0;
00098         if(!v || size == 0)
00099         {
00100                 return max;
00101         }
00102         max = v[0];
00103         for(unsigned int i=1; i<size; ++i)
00104         {
00105                 if(uIsNan(max) || (max < v[i] && !uIsNan(v[i])))
00106                 {
00107                         max = v[i];
00108                         index = i;
00109                 }
00110         }
00111 
00112         return max;
00113 }
00114 
00121 template<class T>
00122 inline T uMax(const std::vector<T> & v, unsigned int & index)
00123 {
00124         return uMax(v.data(), v->size(), index);
00125 }
00126 
00133 template<class T>
00134 inline T uMax(const T * v, unsigned int size)
00135 {
00136         unsigned int index;
00137         return uMax(v, size, index);
00138 }
00139 
00145 template<class T>
00146 inline T uMax(const std::vector<T> & v)
00147 {
00148         return uMax(v.data(), v.size());
00149 }
00150 
00158 template<class T>
00159 inline T uMin(const T * v, unsigned int size, unsigned int & index)
00160 {
00161         T min = 0;
00162         index = 0;
00163         if(!v || size == 0)
00164         {
00165                 return min;
00166         }
00167         min = v[0];
00168         for(unsigned int i=1; i<size; ++i)
00169         {
00170                 if(uIsNan(min) || (min > v[i] && !uIsNan(v[i])))
00171                 {
00172                         min = v[i];
00173                         index = i;
00174                 }
00175         }
00176 
00177         return min;
00178 }
00179 
00186 template<class T>
00187 inline T uMin(const std::vector<T> & v, unsigned int & index)
00188 {
00189         return uMin(v.data(), v.size(), index);
00190 }
00191 
00198 template<class T>
00199 inline T uMin(const T * v, unsigned int size)
00200 {
00201         unsigned int index;
00202         return uMin(v, size, index);
00203 }
00204 
00210 template<class T>
00211 inline T uMin(const std::vector<T> & v)
00212 {
00213         return uMin(v.data(), v.size());
00214 }
00215 
00225 template<class T>
00226 inline void uMinMax(const T * v, unsigned int size, T & min, T & max, unsigned int & indexMin, unsigned int & indexMax)
00227 {
00228         min = 0;
00229         max = 0;
00230         indexMin = 0;
00231         indexMax = 0;
00232         if(!v || size == 0)
00233         {
00234                 return;
00235         }
00236         min = v[0];
00237         max = v[0];
00238 
00239         for(unsigned int i=1; i<size; ++i)
00240         {
00241                 if(uIsNan(min) || (min > v[i] && !uIsNan(v[i])))
00242                 {
00243                         min = v[i];
00244                         indexMin = i;
00245                 }
00246 
00247                 if(uIsNan(max) || (max < v[i] && !uIsNan(v[i])))
00248                 {
00249                         max = v[i];
00250                         indexMax = i;
00251                 }
00252         }
00253 }
00254 
00263 template<class T>
00264 inline void uMinMax(const std::vector<T> & v, T & min, T & max, unsigned int & indexMin, unsigned int & indexMax)
00265 {
00266         uMinMax(v.data(), v.size(), min, max, indexMin, indexMax);
00267 }
00268 
00276 template<class T>
00277 inline void uMinMax(const T * v, unsigned int size, T & min, T & max)
00278 {
00279         unsigned int indexMin;
00280         unsigned int indexMax;
00281         uMinMax(v, size, min, max, indexMin, indexMax);
00282 }
00283 
00290 template<class T>
00291 inline void uMinMax(const std::vector<T> & v, T & min, T & max)
00292 {
00293         uMinMax(v.data(), v.size(), min, max);
00294 }
00295 
00301 template<class T>
00302 inline int uSign(const T & v)
00303 {
00304         if(v < 0)
00305         {
00306                 return -1;
00307         }
00308         else
00309         {
00310                 return 1;
00311         }
00312 }
00313 
00319 template<class T>
00320 inline T uSum(const std::list<T> & list)
00321 {
00322         T sum = 0;
00323         for(typename std::list<T>::const_iterator i=list.begin(); i!=list.end(); ++i)
00324         {
00325                 sum += *i;
00326         }
00327         return sum;
00328 }
00329 
00336 template<class T>
00337 inline T uSum(const T * v, unsigned int size)
00338 {
00339         T sum = 0;
00340         if(v && size)
00341         {
00342                 for(unsigned int i=0; i<size; ++i)
00343                 {
00344                         sum += v[i];
00345                 }
00346         }
00347         return sum;
00348 }
00349 
00355 template<class T>
00356 inline T uSum(const std::vector<T> & v)
00357 {
00358         return uSum(v.data(), (int)v.size());
00359 }
00360 
00368 template<class T>
00369 inline T uSumSquared(const T * v, unsigned int size, T subtract = T())
00370 {
00371         T sum = 0;
00372         if(v && size)
00373         {
00374                 for(unsigned int i=0; i<size; ++i)
00375                 {
00376                         sum += (v[i]-subtract)*(v[i]-subtract);
00377                 }
00378         }
00379         return sum;
00380 }
00381 
00388 template<class T>
00389 inline T uSumSquared(const std::vector<T> & v, T subtract = T())
00390 {
00391         return uSumSquared(v.data(), v.size(), subtract);
00392 }
00393 
00400 template<class T>
00401 inline T uMean(const T * v, unsigned int size)
00402 {
00403         T buf = 0;
00404         if(v && size)
00405         {
00406                 for(unsigned int i=0; i<size; ++i)
00407                 {
00408                         buf += v[i];
00409                 }
00410                 buf /= size;
00411         }
00412         return buf;
00413 }
00414 
00420 template<class T>
00421 inline T uMean(const std::list<T> & list)
00422 {
00423         T m = 0;
00424         if(list.size())
00425         {
00426                 for(typename std::list<T>::const_iterator i=list.begin(); i!=list.end(); ++i)
00427                 {
00428                         m += *i;
00429                 }
00430                 m /= list.size();
00431         }
00432         return m;
00433 }
00434 
00440 template<class T>
00441 inline T uMean(const std::vector<T> & v)
00442 {
00443         return uMean(v.data(), v.size());
00444 }
00445 
00454 template<class T>
00455 inline T uMeanSquaredError(const T * x, unsigned int sizeX, const T * y, unsigned int sizeY)
00456 {
00457         T sum = 0;
00458         if(x && y && sizeX == sizeY)
00459         {
00460                 for(unsigned int i=0; i<sizeX; ++i)
00461                 {
00462                         T diff = x[i]-y[i];
00463                         sum += diff*diff;
00464                 }
00465                 return sum/(T)sizeX;
00466         }
00467         return (T)-1;
00468 }
00469 
00476 template<class T>
00477 inline T uMeanSquaredError(const std::vector<T> & x, const std::vector<T> & y)
00478 {
00479         return uMeanSquaredError(x.data(), x.size(), y.data(), y.size());
00480 }
00481 
00490 template<class T>
00491 inline T uVariance(const T * v, unsigned int size, T meanV)
00492 {
00493         T buf = 0;
00494         if(v && size>1)
00495         {
00496                 float sum = 0;
00497                 for(unsigned int i=0; i<size; ++i)
00498                 {
00499                         sum += (v[i]-meanV)*(v[i]-meanV);
00500                 }
00501                 buf = sum/(size-1);
00502         }
00503         return buf;
00504 }
00505 
00513 template<class T>
00514 inline T uVariance(const std::list<T> & list, const T & m)
00515 {
00516         T buf = 0;
00517         if(list.size()>1)
00518         {
00519                 float sum = 0;
00520                 for(typename std::list<T>::const_iterator i=list.begin(); i!=list.end(); ++i)
00521                 {
00522                         sum += (*i-m)*(*i-m);
00523                 }
00524                 buf = sum/(list.size()-1);
00525         }
00526         return buf;
00527 }
00528 
00535 template<class T>
00536 inline T uVariance(const T * v, unsigned int size)
00537 {
00538         T m = uMean(v, size);
00539         return uVariance(v, size, m);
00540 }
00541 
00549 template<class T>
00550 inline T uVariance(const std::vector<T> & v, const T & m)
00551 {
00552         return uVariance(v.data(), v.size(), m);
00553 }
00554 
00559 template<class T>
00560 inline T uNormSquared(const std::vector<T> & v)
00561 {
00562         float sum = 0.0f;
00563         for(unsigned int i=0; i<v.size(); ++i)
00564         {
00565                 sum += v[i]*v[i];
00566         }
00567         return sum;
00568 }
00569 
00574 template<class T>
00575 inline T uNorm(const std::vector<T> & v)
00576 {
00577         return std::sqrt(uNormSquared(v));
00578 }
00579 
00584 template<class T>
00585 inline T uNormSquared(const T & x1, const T & x2)
00586 {
00587         return x1*x1 + x2*x2;
00588 }
00589 
00594 template<class T>
00595 inline T uNorm(const T & x1, const T & x2)
00596 {
00597         return std::sqrt(uNormSquared(x1, x2));
00598 }
00599 
00604 template<class T>
00605 inline T uNormSquared(const T & x1, const T & x2, const T & x3)
00606 {
00607         return x1*x1 + x2*x2 + x3*x3;
00608 }
00609 
00614 template<class T>
00615 inline T uNorm(const T & x1, const T & x2, const T & x3)
00616 {
00617         return std::sqrt(uNormSquared(x1, x2, x3));
00618 }
00619 
00624 template<class T>
00625 inline std::vector<T> uNormalize(const std::vector<T> & v)
00626 {
00627         float norm = uNorm(v);
00628         if(norm == 0)
00629         {
00630                 return v;
00631         }
00632         else
00633         {
00634                 std::vector<T> r(v.size());
00635                 for(unsigned int i=0; i<v.size(); ++i)
00636                 {
00637                         r[i] = v[i]/norm;
00638                 }
00639                 return r;
00640         }
00641 }
00642 
00646 template<class T>
00647 inline std::list<unsigned int> uLocalMaxima(const T * v, unsigned int size)
00648 {
00649         std::list<unsigned int> maxima;
00650         if(size)
00651         {
00652                 for(unsigned int i=0; i<size; ++i)
00653                 {
00654                         if(i == 0)
00655                         {
00656                                 // first item
00657                                 if((i+1 < size && v[i] > v[i+1]) ||
00658                                         i+1 >= size)
00659                                 {
00660                                         maxima.push_back(i);
00661                                 }
00662                         }
00663                         else if(i == size - 1)
00664                         {
00665                                 //last item
00666                                 if((i >= 1 && v[i] > v[i-1]) ||
00667                                         i == 0)
00668                                 {
00669                                         maxima.push_back(i);
00670                                 }
00671                         }
00672                         else
00673                         {
00674                                 //all others, check previous and next
00675                                 if(v[i] > v[i-1] && v[i] > v[i+1])
00676                                 {
00677                                         maxima.push_back(i);
00678                                 }
00679                         }
00680                 }
00681         }
00682         return maxima;
00683 }
00684 
00688 template<class T>
00689 inline std::list<unsigned int> uLocalMaxima(const std::vector<T> & v)
00690 {
00691         return uLocalMaxima(v.data(), v.size());
00692 }
00693 
00698 enum UXMatchMethod{UXCorrRaw, UXCorrBiased, UXCorrUnbiased, UXCorrCoeff, UXCovRaw, UXCovBiased, UXCovUnbiased, UXCovCoeff};
00699 
00709 template<class T>
00710 inline std::vector<T> uXMatch(const T * vA, const T * vB, unsigned int sizeA, unsigned int sizeB, UXMatchMethod method)
00711 {
00712         if(!vA || !vB || sizeA == 0 || sizeB == 0)
00713         {
00714                 return std::vector<T>();
00715         }
00716 
00717         std::vector<T> result(sizeA + sizeB - 1);
00718 
00719         T meanA = 0;
00720         T meanB = 0;
00721         if(method > UXCorrCoeff)
00722         {
00723                 meanA = uMean(vA, sizeA);
00724                 meanB = uMean(vB, sizeB);
00725         }
00726 
00727         T den = 1;
00728         if(method == UXCorrCoeff || method == UXCovCoeff)
00729         {
00730                 den = std::sqrt(uSumSquared(vA, sizeA, meanA) * uSumSquared(vB, sizeB, meanB));
00731         }
00732         else if(method == UXCorrBiased || method == UXCovBiased)
00733         {
00734                 den = (T)std::max(sizeA, sizeB);
00735         }
00736 
00737         if(sizeA == sizeB)
00738         {
00739                 T resultA;
00740                 T resultB;
00741 
00742                 int posA;
00743                 int posB;
00744                 unsigned int j;
00745 
00746                 // Optimization, filling two results at once
00747                 for(unsigned int i=0; i<sizeA; ++i)
00748                 {
00749                         if(method == UXCorrUnbiased || method == UXCovUnbiased)
00750                         {
00751                                 den = 0;
00752                         }
00753 
00754                         posA = sizeA - i - 1;
00755                         posB = sizeB - i - 1;
00756                         resultA = 0;
00757                         resultB = 0;
00758                         for(j=0; (j + posB) < sizeB && (j + posA) < sizeA; ++j)
00759                         {
00760                                 resultA += (vA[j] - meanA) * (vB[j + posB] - meanB);
00761                                 resultB += (vA[j + posA] - meanA) * (vB[j] - meanB);
00762                                 if(method == UXCorrUnbiased || method == UXCovUnbiased)
00763                                 {
00764                                         ++den;
00765                                 }
00766                         }
00767 
00768                         result[i] = resultA / den;
00769                         result[result.size()-1 -i] = resultB / den;
00770                 }
00771         }
00772         else
00773         {
00774                 for(unsigned int i=0; i<result.size(); ++i)
00775                 {
00776                         if(method == UXCorrUnbiased || method == UXCovUnbiased)
00777                         {
00778                                 den = 0;
00779                         }
00780 
00781                         int posB = sizeB - i - 1;
00782                         T r = 0;
00783                         if(posB >= 0)
00784                         {
00785                                 for(unsigned int j=0; (j + posB) < sizeB && j < sizeA; ++j)
00786                                 {
00787                                         r += (vA[j] - meanA) * (vB[j + posB] - meanB);
00788                                         if(method == UXCorrUnbiased || method == UXCovUnbiased)
00789                                         {
00790                                                 ++den;
00791                                         }
00792                                 }
00793                         }
00794                         else
00795                         {
00796                                 int posA = posB*-1;
00797                                 for(unsigned int i=0; (i+posA) < sizeA && i < sizeB; ++i)
00798                                 {
00799                                         r += (vA[i+posA] - meanA) * (vB[i] - meanB);
00800                                         if(method == UXCorrUnbiased || method == UXCovUnbiased)
00801                                         {
00802                                                 ++den;
00803                                         }
00804                                 }
00805                         }
00806 
00807                         result[i] = r / den;
00808                 }
00809         }
00810 
00811         return result;
00812 }
00813 
00821 template<class T>
00822 inline std::vector<T> uXMatch(const std::vector<T> & vA, const std::vector<T> & vB, UXMatchMethod method)
00823 {
00824         return uXMatch(vA.data(), vB.data(), vA.size(), vB.size(), method);
00825 }
00826 
00837 template<class T>
00838 inline T uXMatch(const T * vA, const T * vB, unsigned int sizeA, unsigned int sizeB, unsigned int index, UXMatchMethod method)
00839 {
00840         T result = 0;
00841         if(!vA || !vB || sizeA == 0 || sizeB == 0)
00842         {
00843                 return result;
00844         }
00845 
00846         T meanA = 0;
00847         T meanB = 0;
00848         if(method > UXCorrCoeff)
00849         {
00850                 meanA = uMean(vA, sizeA);
00851                 meanB = uMean(vB, sizeB);
00852         }
00853         unsigned int size = sizeA + sizeB - 1;
00854 
00855         T den = 1;
00856         if(method == UXCorrCoeff || method == UXCovCoeff)
00857         {
00858                 den = std::sqrt(uSumSquared(vA, sizeA, meanA) * uSumSquared(vB, sizeB, meanB));
00859         }
00860         else if(method == UXCorrBiased || method == UXCovBiased)
00861         {
00862                 den = (T)std::max(sizeA, sizeB);
00863         }
00864         else if(method == UXCorrUnbiased || method == UXCovUnbiased)
00865         {
00866                 den = 0;
00867         }
00868 
00869         if(index < size)
00870         {
00871                 int posB = sizeB - index - 1;
00872                 unsigned int i;
00873                 if(posB >= 0)
00874                 {
00875                         for(i=0; (i + posB) < sizeB && i < sizeA; ++i)
00876                         {
00877                                 result += (vA[i] - meanA) * (vB[i + posB] - meanB);
00878                                 if(method == UXCorrUnbiased || method == UXCovUnbiased)
00879                                 {
00880                                         ++den;
00881                                 }
00882                         }
00883                 }
00884                 else
00885                 {
00886                         int posA = posB*-1;
00887                         for(i=0; (i+posA) < sizeA && i < sizeB; ++i)
00888                         {
00889                                 result += (vA[i+posA] - meanA) * (vB[i] - meanB);
00890                                 if(method == UXCorrUnbiased || method == UXCovUnbiased)
00891                                 {
00892                                         ++den;
00893                                 }
00894                         }
00895                 }
00896         }
00897         return result / den;
00898 }
00899 
00910 template<class T>
00911 inline T uXMatch(const std::vector<T> & vA, const std::vector<T> & vB, unsigned int index, UXMatchMethod method)
00912 {
00913         return uXMatch(vA.data(), vB.data(), vA.size(), vB.size(), index, method);
00914 }
00915 
00921 inline std::vector<float> uHamming(unsigned int L)
00922 {
00923         std::vector<float> w(L);
00924         unsigned int N = L-1;
00925         float pi = 3.14159265f;
00926         for(unsigned int n=0; n<N; ++n)
00927         {
00928                 w[n] = 0.54f-0.46f*std::cos(2.0f*pi*float(n)/float(N));
00929         }
00930         return w;
00931 }
00932 
00933 template <typename T>
00934 bool uIsInBounds(const T& value, const T& low, const T& high)
00935 {
00936         return uIsFinite(value) && !(value < low) && !(value >= high);
00937 }
00938 
00939 #endif // UMATH_H


rtabmap
Author(s): Mathieu Labbe
autogenerated on Thu Jun 6 2019 21:59:32