UMath.h
Go to the documentation of this file.
1 /*
2 * utilite is a cross-platform library with
3 * useful utilities for fast and small developing.
4 * Copyright (C) 2010 Mathieu Labbe
5 *
6 * utilite is free library: you can redistribute it and/or modify
7 * it under the terms of the GNU Lesser General Public License as published by
8 * the Free Software Foundation, either version 3 of the License, or
9 * (at your option) any later version.
10 *
11 * utilite is distributed in the hope that it will be useful,
12 * but WITHOUT ANY WARRANTY; without even the implied warranty of
13 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
14 * GNU Lesser General Public License for more details.
15 *
16 * You should have received a copy of the GNU Lesser General Public License
17 * along with this program. If not, see <http://www.gnu.org/licenses/>.
18 */
19 
20 #ifndef UMATH_H
21 #define UMATH_H
22 
27 #include "rtabmap/utilite/UtiLiteExp.h" // DLL export/import defines
28 
29 #include <cmath>
30 #include <list>
31 #include <vector>
32 
33 #if _MSC_VER
34  #undef min
35  #undef max
36 #endif
37 
41 template<class T>
42 inline bool uIsNan(const T & value)
43 {
44 #if _MSC_VER
45  return _isnan(value) != 0;
46 #else
47  return std::isnan(value);
48 #endif
49 }
50 
54 template<class T>
55 inline bool uIsFinite(const T & value)
56 {
57 #if _MSC_VER
58  return _finite(value) != 0;
59 #else
60  return std::isfinite(value);
61 #endif
62 }
63 
68 template<class T>
69 inline T uMin3( const T& a, const T& b, const T& c)
70 {
71  float m=a<b?a:b;
72  return m<c?m:c;
73 }
74 
79 template<class T>
80 inline T uMax3( const T& a, const T& b, const T& c)
81 {
82  float m=a>b?a:b;
83  return m>c?m:c;
84 }
85 
93 template<class T>
94 inline T uMax(const T * v, unsigned int size, unsigned int & index)
95 {
96  T max = 0;
97  index = 0;
98  if(!v || size == 0)
99  {
100  return max;
101  }
102  max = v[0];
103  for(unsigned int i=1; i<size; ++i)
104  {
105  if(uIsNan(max) || (max < v[i] && !uIsNan(v[i])))
106  {
107  max = v[i];
108  index = i;
109  }
110  }
111 
112  return max;
113 }
114 
121 template<class T>
122 inline T uMax(const std::vector<T> & v, unsigned int & index)
123 {
124  return uMax(v.data(), v->size(), index);
125 }
126 
133 template<class T>
134 inline T uMax(const T * v, unsigned int size)
135 {
136  unsigned int index;
137  return uMax(v, size, index);
138 }
139 
145 template<class T>
146 inline T uMax(const std::vector<T> & v)
147 {
148  return uMax(v.data(), v.size());
149 }
150 
158 template<class T>
159 inline T uMin(const T * v, unsigned int size, unsigned int & index)
160 {
161  T min = 0;
162  index = 0;
163  if(!v || size == 0)
164  {
165  return min;
166  }
167  min = v[0];
168  for(unsigned int i=1; i<size; ++i)
169  {
170  if(uIsNan(min) || (min > v[i] && !uIsNan(v[i])))
171  {
172  min = v[i];
173  index = i;
174  }
175  }
176 
177  return min;
178 }
179 
186 template<class T>
187 inline T uMin(const std::vector<T> & v, unsigned int & index)
188 {
189  return uMin(v.data(), v.size(), index);
190 }
191 
198 template<class T>
199 inline T uMin(const T * v, unsigned int size)
200 {
201  unsigned int index;
202  return uMin(v, size, index);
203 }
204 
210 template<class T>
211 inline T uMin(const std::vector<T> & v)
212 {
213  return uMin(v.data(), v.size());
214 }
215 
225 template<class T>
226 inline void uMinMax(const T * v, unsigned int size, T & min, T & max, unsigned int & indexMin, unsigned int & indexMax)
227 {
228  min = 0;
229  max = 0;
230  indexMin = 0;
231  indexMax = 0;
232  if(!v || size == 0)
233  {
234  return;
235  }
236  min = v[0];
237  max = v[0];
238 
239  for(unsigned int i=1; i<size; ++i)
240  {
241  if(uIsNan(min) || (min > v[i] && !uIsNan(v[i])))
242  {
243  min = v[i];
244  indexMin = i;
245  }
246 
247  if(uIsNan(max) || (max < v[i] && !uIsNan(v[i])))
248  {
249  max = v[i];
250  indexMax = i;
251  }
252  }
253 }
254 
263 template<class T>
264 inline void uMinMax(const std::vector<T> & v, T & min, T & max, unsigned int & indexMin, unsigned int & indexMax)
265 {
266  uMinMax(v.data(), v.size(), min, max, indexMin, indexMax);
267 }
268 
276 template<class T>
277 inline void uMinMax(const T * v, unsigned int size, T & min, T & max)
278 {
279  unsigned int indexMin;
280  unsigned int indexMax;
281  uMinMax(v, size, min, max, indexMin, indexMax);
282 }
283 
290 template<class T>
291 inline void uMinMax(const std::vector<T> & v, T & min, T & max)
292 {
293  uMinMax(v.data(), v.size(), min, max);
294 }
295 
301 template<class T>
302 inline int uSign(const T & v)
303 {
304  if(v < 0)
305  {
306  return -1;
307  }
308  else
309  {
310  return 1;
311  }
312 }
313 
319 template<class T>
320 inline T uSum(const std::list<T> & list)
321 {
322  T sum = 0;
323  for(typename std::list<T>::const_iterator i=list.begin(); i!=list.end(); ++i)
324  {
325  sum += *i;
326  }
327  return sum;
328 }
329 
336 template<class T>
337 inline T uSum(const T * v, unsigned int size)
338 {
339  T sum = 0;
340  if(v && size)
341  {
342  for(unsigned int i=0; i<size; ++i)
343  {
344  sum += v[i];
345  }
346  }
347  return sum;
348 }
349 
355 template<class T>
356 inline T uSum(const std::vector<T> & v)
357 {
358  return uSum(v.data(), (int)v.size());
359 }
360 
368 template<class T>
369 inline T uSumSquared(const T * v, unsigned int size, T subtract = T())
370 {
371  T sum = 0;
372  if(v && size)
373  {
374  for(unsigned int i=0; i<size; ++i)
375  {
376  sum += (v[i]-subtract)*(v[i]-subtract);
377  }
378  }
379  return sum;
380 }
381 
388 template<class T>
389 inline T uSumSquared(const std::vector<T> & v, T subtract = T())
390 {
391  return uSumSquared(v.data(), v.size(), subtract);
392 }
393 
400 template<class T>
401 inline T uMean(const T * v, unsigned int size)
402 {
403  T buf = 0;
404  if(v && size)
405  {
406  for(unsigned int i=0; i<size; ++i)
407  {
408  buf += v[i];
409  }
410  buf /= size;
411  }
412  return buf;
413 }
414 
420 template<class T>
421 inline T uMean(const std::list<T> & list)
422 {
423  T m = 0;
424  if(list.size())
425  {
426  for(typename std::list<T>::const_iterator i=list.begin(); i!=list.end(); ++i)
427  {
428  m += *i;
429  }
430  m /= list.size();
431  }
432  return m;
433 }
434 
440 template<class T>
441 inline T uMean(const std::vector<T> & v)
442 {
443  return uMean(v.data(), v.size());
444 }
445 
454 template<class T>
455 inline T uMeanSquaredError(const T * x, unsigned int sizeX, const T * y, unsigned int sizeY)
456 {
457  T sum = 0;
458  if(x && y && sizeX == sizeY)
459  {
460  for(unsigned int i=0; i<sizeX; ++i)
461  {
462  T diff = x[i]-y[i];
463  sum += diff*diff;
464  }
465  return sum/(T)sizeX;
466  }
467  return (T)-1;
468 }
469 
476 template<class T>
477 inline T uMeanSquaredError(const std::vector<T> & x, const std::vector<T> & y)
478 {
479  return uMeanSquaredError(x.data(), x.size(), y.data(), y.size());
480 }
481 
490 template<class T>
491 inline T uVariance(const T * v, unsigned int size, T meanV)
492 {
493  T buf = 0;
494  if(v && size>1)
495  {
496  float sum = 0;
497  for(unsigned int i=0; i<size; ++i)
498  {
499  sum += (v[i]-meanV)*(v[i]-meanV);
500  }
501  buf = sum/(size-1);
502  }
503  return buf;
504 }
505 
513 template<class T>
514 inline T uVariance(const std::list<T> & list, const T & m)
515 {
516  T buf = 0;
517  if(list.size()>1)
518  {
519  float sum = 0;
520  for(typename std::list<T>::const_iterator i=list.begin(); i!=list.end(); ++i)
521  {
522  sum += (*i-m)*(*i-m);
523  }
524  buf = sum/(list.size()-1);
525  }
526  return buf;
527 }
528 
535 template<class T>
536 inline T uVariance(const T * v, unsigned int size)
537 {
538  T m = uMean(v, size);
539  return uVariance(v, size, m);
540 }
541 
549 template<class T>
550 inline T uVariance(const std::vector<T> & v, const T & m)
551 {
552  return uVariance(v.data(), v.size(), m);
553 }
554 
559 template<class T>
560 inline T uNormSquared(const std::vector<T> & v)
561 {
562  float sum = 0.0f;
563  for(unsigned int i=0; i<v.size(); ++i)
564  {
565  sum += v[i]*v[i];
566  }
567  return sum;
568 }
569 
574 template<class T>
575 inline T uNorm(const std::vector<T> & v)
576 {
577  return std::sqrt(uNormSquared(v));
578 }
579 
584 template<class T>
585 inline T uNormSquared(const T & x1, const T & x2)
586 {
587  return x1*x1 + x2*x2;
588 }
589 
594 template<class T>
595 inline T uNorm(const T & x1, const T & x2)
596 {
597  return std::sqrt(uNormSquared(x1, x2));
598 }
599 
604 template<class T>
605 inline T uNormSquared(const T & x1, const T & x2, const T & x3)
606 {
607  return x1*x1 + x2*x2 + x3*x3;
608 }
609 
614 template<class T>
615 inline T uNorm(const T & x1, const T & x2, const T & x3)
616 {
617  return std::sqrt(uNormSquared(x1, x2, x3));
618 }
619 
624 template<class T>
625 inline std::vector<T> uNormalize(const std::vector<T> & v)
626 {
627  float norm = uNorm(v);
628  if(norm == 0)
629  {
630  return v;
631  }
632  else
633  {
634  std::vector<T> r(v.size());
635  for(unsigned int i=0; i<v.size(); ++i)
636  {
637  r[i] = v[i]/norm;
638  }
639  return r;
640  }
641 }
642 
646 template<class T>
647 inline std::list<unsigned int> uLocalMaxima(const T * v, unsigned int size)
648 {
649  std::list<unsigned int> maxima;
650  if(size)
651  {
652  for(unsigned int i=0; i<size; ++i)
653  {
654  if(i == 0)
655  {
656  // first item
657  if((i+1 < size && v[i] > v[i+1]) ||
658  i+1 >= size)
659  {
660  maxima.push_back(i);
661  }
662  }
663  else if(i == size - 1)
664  {
665  //last item
666  if((i >= 1 && v[i] > v[i-1]) ||
667  i == 0)
668  {
669  maxima.push_back(i);
670  }
671  }
672  else
673  {
674  //all others, check previous and next
675  if(v[i] > v[i-1] && v[i] > v[i+1])
676  {
677  maxima.push_back(i);
678  }
679  }
680  }
681  }
682  return maxima;
683 }
684 
688 template<class T>
689 inline std::list<unsigned int> uLocalMaxima(const std::vector<T> & v)
690 {
691  return uLocalMaxima(v.data(), v.size());
692 }
693 
698 enum UXMatchMethod{UXCorrRaw, UXCorrBiased, UXCorrUnbiased, UXCorrCoeff, UXCovRaw, UXCovBiased, UXCovUnbiased, UXCovCoeff};
699 
709 template<class T>
710 inline std::vector<T> uXMatch(const T * vA, const T * vB, unsigned int sizeA, unsigned int sizeB, UXMatchMethod method)
711 {
712  if(!vA || !vB || sizeA == 0 || sizeB == 0)
713  {
714  return std::vector<T>();
715  }
716 
717  std::vector<T> result(sizeA + sizeB - 1);
718 
719  T meanA = 0;
720  T meanB = 0;
721  if(method > UXCorrCoeff)
722  {
723  meanA = uMean(vA, sizeA);
724  meanB = uMean(vB, sizeB);
725  }
726 
727  T den = 1;
728  if(method == UXCorrCoeff || method == UXCovCoeff)
729  {
730  den = std::sqrt(uSumSquared(vA, sizeA, meanA) * uSumSquared(vB, sizeB, meanB));
731  }
732  else if(method == UXCorrBiased || method == UXCovBiased)
733  {
734  den = (T)std::max(sizeA, sizeB);
735  }
736 
737  if(sizeA == sizeB)
738  {
739  T resultA;
740  T resultB;
741 
742  int posA;
743  int posB;
744  unsigned int j;
745 
746  // Optimization, filling two results at once
747  for(unsigned int i=0; i<sizeA; ++i)
748  {
749  if(method == UXCorrUnbiased || method == UXCovUnbiased)
750  {
751  den = 0;
752  }
753 
754  posA = sizeA - i - 1;
755  posB = sizeB - i - 1;
756  resultA = 0;
757  resultB = 0;
758  for(j=0; (j + posB) < sizeB && (j + posA) < sizeA; ++j)
759  {
760  resultA += (vA[j] - meanA) * (vB[j + posB] - meanB);
761  resultB += (vA[j + posA] - meanA) * (vB[j] - meanB);
762  if(method == UXCorrUnbiased || method == UXCovUnbiased)
763  {
764  ++den;
765  }
766  }
767 
768  result[i] = resultA / den;
769  result[result.size()-1 -i] = resultB / den;
770  }
771  }
772  else
773  {
774  for(unsigned int i=0; i<result.size(); ++i)
775  {
776  if(method == UXCorrUnbiased || method == UXCovUnbiased)
777  {
778  den = 0;
779  }
780 
781  int posB = sizeB - i - 1;
782  T r = 0;
783  if(posB >= 0)
784  {
785  for(unsigned int j=0; (j + posB) < sizeB && j < sizeA; ++j)
786  {
787  r += (vA[j] - meanA) * (vB[j + posB] - meanB);
788  if(method == UXCorrUnbiased || method == UXCovUnbiased)
789  {
790  ++den;
791  }
792  }
793  }
794  else
795  {
796  int posA = posB*-1;
797  for(unsigned int i=0; (i+posA) < sizeA && i < sizeB; ++i)
798  {
799  r += (vA[i+posA] - meanA) * (vB[i] - meanB);
800  if(method == UXCorrUnbiased || method == UXCovUnbiased)
801  {
802  ++den;
803  }
804  }
805  }
806 
807  result[i] = r / den;
808  }
809  }
810 
811  return result;
812 }
813 
821 template<class T>
822 inline std::vector<T> uXMatch(const std::vector<T> & vA, const std::vector<T> & vB, UXMatchMethod method)
823 {
824  return uXMatch(vA.data(), vB.data(), vA.size(), vB.size(), method);
825 }
826 
837 template<class T>
838 inline T uXMatch(const T * vA, const T * vB, unsigned int sizeA, unsigned int sizeB, unsigned int index, UXMatchMethod method)
839 {
840  T result = 0;
841  if(!vA || !vB || sizeA == 0 || sizeB == 0)
842  {
843  return result;
844  }
845 
846  T meanA = 0;
847  T meanB = 0;
848  if(method > UXCorrCoeff)
849  {
850  meanA = uMean(vA, sizeA);
851  meanB = uMean(vB, sizeB);
852  }
853  unsigned int size = sizeA + sizeB - 1;
854 
855  T den = 1;
856  if(method == UXCorrCoeff || method == UXCovCoeff)
857  {
858  den = std::sqrt(uSumSquared(vA, sizeA, meanA) * uSumSquared(vB, sizeB, meanB));
859  }
860  else if(method == UXCorrBiased || method == UXCovBiased)
861  {
862  den = (T)std::max(sizeA, sizeB);
863  }
864  else if(method == UXCorrUnbiased || method == UXCovUnbiased)
865  {
866  den = 0;
867  }
868 
869  if(index < size)
870  {
871  int posB = sizeB - index - 1;
872  unsigned int i;
873  if(posB >= 0)
874  {
875  for(i=0; (i + posB) < sizeB && i < sizeA; ++i)
876  {
877  result += (vA[i] - meanA) * (vB[i + posB] - meanB);
878  if(method == UXCorrUnbiased || method == UXCovUnbiased)
879  {
880  ++den;
881  }
882  }
883  }
884  else
885  {
886  int posA = posB*-1;
887  for(i=0; (i+posA) < sizeA && i < sizeB; ++i)
888  {
889  result += (vA[i+posA] - meanA) * (vB[i] - meanB);
890  if(method == UXCorrUnbiased || method == UXCovUnbiased)
891  {
892  ++den;
893  }
894  }
895  }
896  }
897  return result / den;
898 }
899 
910 template<class T>
911 inline T uXMatch(const std::vector<T> & vA, const std::vector<T> & vB, unsigned int index, UXMatchMethod method)
912 {
913  return uXMatch(vA.data(), vB.data(), vA.size(), vB.size(), index, method);
914 }
915 
921 inline std::vector<float> uHamming(unsigned int L)
922 {
923  std::vector<float> w(L);
924  unsigned int N = L-1;
925  float pi = 3.14159265f;
926  for(unsigned int n=0; n<N; ++n)
927  {
928  w[n] = 0.54f-0.46f*std::cos(2.0f*pi*float(n)/float(N));
929  }
930  return w;
931 }
932 
933 template <typename T>
934 bool uIsInBounds(const T& value, const T& low, const T& high)
935 {
936  return uIsFinite(value) && !(value < low) && !(value >= high);
937 }
938 
939 #endif // UMATH_H
uVariance
T uVariance(const T *v, unsigned int size, T meanV)
Definition: UMath.h:491
uMean
T uMean(const T *v, unsigned int size)
Definition: UMath.h:401
uNorm
T uNorm(const std::vector< T > &v)
Definition: UMath.h:575
glm::isfinite
GLM_FUNC_DECL bool isfinite(genType const &x)
Test whether or not a scalar or each vector component is a finite value. (From GLM_GTX_compatibility)...
glm::min
GLM_FUNC_DECL genType min(genType const &x, genType const &y)
uMinMax
void uMinMax(const T *v, unsigned int size, T &min, T &max, unsigned int &indexMin, unsigned int &indexMax)
Definition: UMath.h:226
f
f
uIsInBounds
bool uIsInBounds(const T &value, const T &low, const T &high)
Definition: UMath.h:934
glm::sqrt
GLM_FUNC_DECL vecType< T, P > sqrt(vecType< T, P > const &x)
uMin3
T uMin3(const T &a, const T &b, const T &c)
Definition: UMath.h:69
uNormalize
std::vector< T > uNormalize(const std::vector< T > &v)
Definition: UMath.h:625
uXMatch
std::vector< T > uXMatch(const T *vA, const T *vB, unsigned int sizeA, unsigned int sizeB, UXMatchMethod method)
Definition: UMath.h:710
uSign
int uSign(const T &v)
Definition: UMath.h:302
uLocalMaxima
std::list< unsigned int > uLocalMaxima(const T *v, unsigned int size)
Definition: UMath.h:647
uMin
T uMin(const T *v, unsigned int size, unsigned int &index)
Definition: UMath.h:159
uIsFinite
bool uIsFinite(const T &value)
Definition: UMath.h:55
glm::cos
GLM_FUNC_DECL genType cos(genType const &angle)
uHamming
std::vector< float > uHamming(unsigned int L)
Definition: UMath.h:921
uMax3
T uMax3(const T &a, const T &b, const T &c)
Definition: UMath.h:80
glm::pi
GLM_FUNC_DECL genType pi()
uSumSquared
T uSumSquared(const T *v, unsigned int size, T subtract=T())
Definition: UMath.h:369
uMeanSquaredError
T uMeanSquaredError(const T *x, unsigned int sizeX, const T *y, unsigned int sizeY)
Definition: UMath.h:455
uMax
T uMax(const T *v, unsigned int size, unsigned int &index)
Definition: UMath.h:94
glm::max
GLM_FUNC_DECL genType max(genType const &x, genType const &y)
uNormSquared
T uNormSquared(const std::vector< T > &v)
Definition: UMath.h:560
glm::isnan
GLM_FUNC_DECL genType::bool_type isnan(genType const &x)
uIsNan
bool uIsNan(const T &value)
Definition: UMath.h:42
uSum
T uSum(const std::list< T > &list)
Definition: UMath.h:320
UXMatchMethod
UXMatchMethod
Definition: UMath.h:698

rtabmap
Author(s): Mathieu Labbe
autogenerated on Mon Dec 14 2020 03:37:06