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 <cmath>
28 #include <list>
29 #include <vector>
30 
31 #if _MSC_VER
32  #undef min
33  #undef max
34 #endif
35 
39 template<class T>
40 inline bool uIsNan(const T & value)
41 {
42 #if _MSC_VER
43  return _isnan(value) != 0;
44 #else
45  return std::isnan(value);
46 #endif
47 }
48 
52 template<class T>
53 inline bool uIsFinite(const T & value)
54 {
55 #if _MSC_VER
56  return _finite(value) != 0;
57 #else
58  return std::isfinite(value);
59 #endif
60 }
61 
66 template<class T>
67 inline T uMin3( const T& a, const T& b, const T& c)
68 {
69  float m=a<b?a:b;
70  return m<c?m:c;
71 }
72 
77 template<class T>
78 inline T uMax3( const T& a, const T& b, const T& c)
79 {
80  float m=a>b?a:b;
81  return m>c?m:c;
82 }
83 
91 template<class T>
92 inline T uMax(const T * v, unsigned int size, unsigned int & index)
93 {
94  T max = 0;
95  index = 0;
96  if(!v || size == 0)
97  {
98  return max;
99  }
100  max = v[0];
101  for(unsigned int i=1; i<size; ++i)
102  {
103  if(uIsNan(max) || (max < v[i] && !uIsNan(v[i])))
104  {
105  max = v[i];
106  index = i;
107  }
108  }
109 
110  return max;
111 }
112 
119 template<class T>
120 inline T uMax(const std::vector<T> & v, unsigned int & index)
121 {
122  return uMax(v.data(), v->size(), index);
123 }
124 
131 template<class T>
132 inline T uMax(const T * v, unsigned int size)
133 {
134  unsigned int index;
135  return uMax(v, size, index);
136 }
137 
143 template<class T>
144 inline T uMax(const std::vector<T> & v)
145 {
146  return uMax(v.data(), v.size());
147 }
148 
156 template<class T>
157 inline T uMin(const T * v, unsigned int size, unsigned int & index)
158 {
159  T min = 0;
160  index = 0;
161  if(!v || size == 0)
162  {
163  return min;
164  }
165  min = v[0];
166  for(unsigned int i=1; i<size; ++i)
167  {
168  if(uIsNan(min) || (min > v[i] && !uIsNan(v[i])))
169  {
170  min = v[i];
171  index = i;
172  }
173  }
174 
175  return min;
176 }
177 
184 template<class T>
185 inline T uMin(const std::vector<T> & v, unsigned int & index)
186 {
187  return uMin(v.data(), v.size(), index);
188 }
189 
196 template<class T>
197 inline T uMin(const T * v, unsigned int size)
198 {
199  unsigned int index;
200  return uMin(v, size, index);
201 }
202 
208 template<class T>
209 inline T uMin(const std::vector<T> & v)
210 {
211  return uMin(v.data(), v.size());
212 }
213 
223 template<class T>
224 inline void uMinMax(const T * v, unsigned int size, T & min, T & max, unsigned int & indexMin, unsigned int & indexMax)
225 {
226  min = 0;
227  max = 0;
228  indexMin = 0;
229  indexMax = 0;
230  if(!v || size == 0)
231  {
232  return;
233  }
234  min = v[0];
235  max = v[0];
236 
237  for(unsigned int i=1; i<size; ++i)
238  {
239  if(uIsNan(min) || (min > v[i] && !uIsNan(v[i])))
240  {
241  min = v[i];
242  indexMin = i;
243  }
244 
245  if(uIsNan(max) || (max < v[i] && !uIsNan(v[i])))
246  {
247  max = v[i];
248  indexMax = i;
249  }
250  }
251 }
252 
261 template<class T>
262 inline void uMinMax(const std::vector<T> & v, T & min, T & max, unsigned int & indexMin, unsigned int & indexMax)
263 {
264  uMinMax(v.data(), v.size(), min, max, indexMin, indexMax);
265 }
266 
274 template<class T>
275 inline void uMinMax(const T * v, unsigned int size, T & min, T & max)
276 {
277  unsigned int indexMin;
278  unsigned int indexMax;
279  uMinMax(v, size, min, max, indexMin, indexMax);
280 }
281 
288 template<class T>
289 inline void uMinMax(const std::vector<T> & v, T & min, T & max)
290 {
291  uMinMax(v.data(), v.size(), min, max);
292 }
293 
299 template<class T>
300 inline int uSign(const T & v)
301 {
302  if(v < 0)
303  {
304  return -1;
305  }
306  else
307  {
308  return 1;
309  }
310 }
311 
317 template<class T>
318 inline T uSum(const std::list<T> & list)
319 {
320  T sum = 0;
321  for(typename std::list<T>::const_iterator i=list.begin(); i!=list.end(); ++i)
322  {
323  sum += *i;
324  }
325  return sum;
326 }
327 
334 template<class T>
335 inline T uSum(const T * v, unsigned int size)
336 {
337  T sum = 0;
338  if(v && size)
339  {
340  for(unsigned int i=0; i<size; ++i)
341  {
342  sum += v[i];
343  }
344  }
345  return sum;
346 }
347 
353 template<class T>
354 inline T uSum(const std::vector<T> & v)
355 {
356  return uSum(v.data(), (int)v.size());
357 }
358 
366 template<class T>
367 inline T uSumSquared(const T * v, unsigned int size, T subtract = T())
368 {
369  T sum = 0;
370  if(v && size)
371  {
372  for(unsigned int i=0; i<size; ++i)
373  {
374  sum += (v[i]-subtract)*(v[i]-subtract);
375  }
376  }
377  return sum;
378 }
379 
386 template<class T>
387 inline T uSumSquared(const std::vector<T> & v, T subtract = T())
388 {
389  return uSumSquared(v.data(), v.size(), subtract);
390 }
391 
398 template<class T>
399 inline T uMean(const T * v, unsigned int size)
400 {
401  T buf = 0;
402  if(v && size)
403  {
404  for(unsigned int i=0; i<size; ++i)
405  {
406  buf += v[i];
407  }
408  buf /= size;
409  }
410  return buf;
411 }
412 
418 template<class T>
419 inline T uMean(const std::list<T> & list)
420 {
421  T m = 0;
422  if(list.size())
423  {
424  for(typename std::list<T>::const_iterator i=list.begin(); i!=list.end(); ++i)
425  {
426  m += *i;
427  }
428  m /= list.size();
429  }
430  return m;
431 }
432 
438 template<class T>
439 inline T uMean(const std::vector<T> & v)
440 {
441  return uMean(v.data(), v.size());
442 }
443 
452 template<class T>
453 inline T uMeanSquaredError(const T * x, unsigned int sizeX, const T * y, unsigned int sizeY)
454 {
455  T sum = 0;
456  if(x && y && sizeX == sizeY)
457  {
458  for(unsigned int i=0; i<sizeX; ++i)
459  {
460  T diff = x[i]-y[i];
461  sum += diff*diff;
462  }
463  return sum/(T)sizeX;
464  }
465  return (T)-1;
466 }
467 
474 template<class T>
475 inline T uMeanSquaredError(const std::vector<T> & x, const std::vector<T> & y)
476 {
477  return uMeanSquaredError(x.data(), x.size(), y.data(), y.size());
478 }
479 
488 template<class T>
489 inline T uVariance(const T * v, unsigned int size, T meanV)
490 {
491  T buf = 0;
492  if(v && size>1)
493  {
494  float sum = 0;
495  for(unsigned int i=0; i<size; ++i)
496  {
497  sum += (v[i]-meanV)*(v[i]-meanV);
498  }
499  buf = sum/(size-1);
500  }
501  return buf;
502 }
503 
511 template<class T>
512 inline T uVariance(const std::list<T> & list, const T & m)
513 {
514  T buf = 0;
515  if(list.size()>1)
516  {
517  float sum = 0;
518  for(typename std::list<T>::const_iterator i=list.begin(); i!=list.end(); ++i)
519  {
520  sum += (*i-m)*(*i-m);
521  }
522  buf = sum/(list.size()-1);
523  }
524  return buf;
525 }
526 
533 template<class T>
534 inline T uVariance(const T * v, unsigned int size)
535 {
536  T m = uMean(v, size);
537  return uVariance(v, size, m);
538 }
539 
547 template<class T>
548 inline T uVariance(const std::vector<T> & v, const T & m)
549 {
550  return uVariance(v.data(), v.size(), m);
551 }
552 
557 template<class T>
558 inline T uNormSquared(const std::vector<T> & v)
559 {
560  float sum = 0.0f;
561  for(unsigned int i=0; i<v.size(); ++i)
562  {
563  sum += v[i]*v[i];
564  }
565  return sum;
566 }
567 
572 template<class T>
573 inline T uNorm(const std::vector<T> & v)
574 {
575  return std::sqrt(uNormSquared(v));
576 }
577 
582 template<class T>
583 inline T uNormSquared(const T & x1, const T & x2)
584 {
585  return x1*x1 + x2*x2;
586 }
587 
592 template<class T>
593 inline T uNorm(const T & x1, const T & x2)
594 {
595  return std::sqrt(uNormSquared(x1, x2));
596 }
597 
602 template<class T>
603 inline T uNormSquared(const T & x1, const T & x2, const T & x3)
604 {
605  return x1*x1 + x2*x2 + x3*x3;
606 }
607 
612 template<class T>
613 inline T uNorm(const T & x1, const T & x2, const T & x3)
614 {
615  return std::sqrt(uNormSquared(x1, x2, x3));
616 }
617 
622 template<class T>
623 inline std::vector<T> uNormalize(const std::vector<T> & v)
624 {
625  float norm = uNorm(v);
626  if(norm == 0)
627  {
628  return v;
629  }
630  else
631  {
632  std::vector<T> r(v.size());
633  for(unsigned int i=0; i<v.size(); ++i)
634  {
635  r[i] = v[i]/norm;
636  }
637  return r;
638  }
639 }
640 
644 template<class T>
645 inline std::list<unsigned int> uLocalMaxima(const T * v, unsigned int size)
646 {
647  std::list<unsigned int> maxima;
648  if(size)
649  {
650  for(unsigned int i=0; i<size; ++i)
651  {
652  if(i == 0)
653  {
654  // first item
655  if((i+1 < size && v[i] > v[i+1]) ||
656  i+1 >= size)
657  {
658  maxima.push_back(i);
659  }
660  }
661  else if(i == size - 1)
662  {
663  //last item
664  if((i >= 1 && v[i] > v[i-1]) ||
665  i == 0)
666  {
667  maxima.push_back(i);
668  }
669  }
670  else
671  {
672  //all others, check previous and next
673  if(v[i] > v[i-1] && v[i] > v[i+1])
674  {
675  maxima.push_back(i);
676  }
677  }
678  }
679  }
680  return maxima;
681 }
682 
686 template<class T>
687 inline std::list<unsigned int> uLocalMaxima(const std::vector<T> & v)
688 {
689  return uLocalMaxima(v.data(), v.size());
690 }
691 
697 
707 template<class T>
708 inline std::vector<T> uXMatch(const T * vA, const T * vB, unsigned int sizeA, unsigned int sizeB, UXMatchMethod method)
709 {
710  if(!vA || !vB || sizeA == 0 || sizeB == 0)
711  {
712  return std::vector<T>();
713  }
714 
715  std::vector<T> result(sizeA + sizeB - 1);
716 
717  T meanA = 0;
718  T meanB = 0;
719  if(method > UXCorrCoeff)
720  {
721  meanA = uMean(vA, sizeA);
722  meanB = uMean(vB, sizeB);
723  }
724 
725  T den = 1;
726  if(method == UXCorrCoeff || method == UXCovCoeff)
727  {
728  den = std::sqrt(uSumSquared(vA, sizeA, meanA) * uSumSquared(vB, sizeB, meanB));
729  }
730  else if(method == UXCorrBiased || method == UXCovBiased)
731  {
732  den = (T)std::max(sizeA, sizeB);
733  }
734 
735  if(sizeA == sizeB)
736  {
737  T resultA;
738  T resultB;
739 
740  int posA;
741  int posB;
742  unsigned int j;
743 
744  // Optimization, filling two results at once
745  for(unsigned int i=0; i<sizeA; ++i)
746  {
747  if(method == UXCorrUnbiased || method == UXCovUnbiased)
748  {
749  den = 0;
750  }
751 
752  posA = sizeA - i - 1;
753  posB = sizeB - i - 1;
754  resultA = 0;
755  resultB = 0;
756  for(j=0; (j + posB) < sizeB && (j + posA) < sizeA; ++j)
757  {
758  resultA += (vA[j] - meanA) * (vB[j + posB] - meanB);
759  resultB += (vA[j + posA] - meanA) * (vB[j] - meanB);
760  if(method == UXCorrUnbiased || method == UXCovUnbiased)
761  {
762  ++den;
763  }
764  }
765 
766  result[i] = resultA / den;
767  result[result.size()-1 -i] = resultB / den;
768  }
769  }
770  else
771  {
772  for(unsigned int i=0; i<result.size(); ++i)
773  {
774  if(method == UXCorrUnbiased || method == UXCovUnbiased)
775  {
776  den = 0;
777  }
778 
779  int posB = sizeB - i - 1;
780  T r = 0;
781  if(posB >= 0)
782  {
783  for(unsigned int j=0; (j + posB) < sizeB && j < sizeA; ++j)
784  {
785  r += (vA[j] - meanA) * (vB[j + posB] - meanB);
786  if(method == UXCorrUnbiased || method == UXCovUnbiased)
787  {
788  ++den;
789  }
790  }
791  }
792  else
793  {
794  int posA = posB*-1;
795  for(unsigned int i=0; (i+posA) < sizeA && i < sizeB; ++i)
796  {
797  r += (vA[i+posA] - meanA) * (vB[i] - meanB);
798  if(method == UXCorrUnbiased || method == UXCovUnbiased)
799  {
800  ++den;
801  }
802  }
803  }
804 
805  result[i] = r / den;
806  }
807  }
808 
809  return result;
810 }
811 
819 template<class T>
820 inline std::vector<T> uXMatch(const std::vector<T> & vA, const std::vector<T> & vB, UXMatchMethod method)
821 {
822  return uXMatch(vA.data(), vB.data(), vA.size(), vB.size(), method);
823 }
824 
835 template<class T>
836 inline T uXMatch(const T * vA, const T * vB, unsigned int sizeA, unsigned int sizeB, unsigned int index, UXMatchMethod method)
837 {
838  T result = 0;
839  if(!vA || !vB || sizeA == 0 || sizeB == 0)
840  {
841  return result;
842  }
843 
844  T meanA = 0;
845  T meanB = 0;
846  if(method > UXCorrCoeff)
847  {
848  meanA = uMean(vA, sizeA);
849  meanB = uMean(vB, sizeB);
850  }
851  unsigned int size = sizeA + sizeB - 1;
852 
853  T den = 1;
854  if(method == UXCorrCoeff || method == UXCovCoeff)
855  {
856  den = std::sqrt(uSumSquared(vA, sizeA, meanA) * uSumSquared(vB, sizeB, meanB));
857  }
858  else if(method == UXCorrBiased || method == UXCovBiased)
859  {
860  den = (T)std::max(sizeA, sizeB);
861  }
862  else if(method == UXCorrUnbiased || method == UXCovUnbiased)
863  {
864  den = 0;
865  }
866 
867  if(index < size)
868  {
869  int posB = sizeB - index - 1;
870  unsigned int i;
871  if(posB >= 0)
872  {
873  for(i=0; (i + posB) < sizeB && i < sizeA; ++i)
874  {
875  result += (vA[i] - meanA) * (vB[i + posB] - meanB);
876  if(method == UXCorrUnbiased || method == UXCovUnbiased)
877  {
878  ++den;
879  }
880  }
881  }
882  else
883  {
884  int posA = posB*-1;
885  for(i=0; (i+posA) < sizeA && i < sizeB; ++i)
886  {
887  result += (vA[i+posA] - meanA) * (vB[i] - meanB);
888  if(method == UXCorrUnbiased || method == UXCovUnbiased)
889  {
890  ++den;
891  }
892  }
893  }
894  }
895  return result / den;
896 }
897 
908 template<class T>
909 inline T uXMatch(const std::vector<T> & vA, const std::vector<T> & vB, unsigned int index, UXMatchMethod method)
910 {
911  return uXMatch(vA.data(), vB.data(), vA.size(), vB.size(), index, method);
912 }
913 
919 inline std::vector<float> uHamming(unsigned int L)
920 {
921  std::vector<float> w(L);
922  unsigned int N = L-1;
923  float pi = 3.14159265f;
924  for(unsigned int n=0; n<N; ++n)
925  {
926  w[n] = 0.54f-0.46f*std::cos(2.0f*pi*float(n)/float(N));
927  }
928  return w;
929 }
930 
931 template <typename T>
932 bool uIsInBounds(const T& value, const T& low, const T& high)
933 {
934  return uIsFinite(value) && !(value < low) && !(value >= high);
935 }
936 
937 #endif // UMATH_H
w
RowVector3d w
glm::min
GLM_FUNC_DECL genType min(genType const &x, genType const &y)
uXMatch
std::vector< T > uXMatch(const T *vA, const T *vB, unsigned int sizeA, unsigned int sizeB, UXMatchMethod method)
Definition: UMath.h:708
uSumSquared
T uSumSquared(const T *v, unsigned int size, T subtract=T())
Definition: UMath.h:367
list::end
detail::list_iterator end() const
uMean
T uMean(const T *v, unsigned int size)
Definition: UMath.h:399
uNorm
T uNorm(const std::vector< T > &v)
Definition: UMath.h:573
x1
x1
uMin3
T uMin3(const T &a, const T &b, const T &c)
Definition: UMath.h:67
b
Array< int, 3, 1 > b
list
uSum
T uSum(const std::list< T > &list)
Definition: UMath.h:318
c
Scalar Scalar * c
size
Index size
uMin
T uMin(const T *v, unsigned int size, unsigned int &index)
Definition: UMath.h:157
y
Matrix3f y
uVariance
T uVariance(const T *v, unsigned int size, T meanV)
Definition: UMath.h:489
vB
Vector vB(size_t i)
glm::isnan
GLM_FUNC_DECL genType::bool_type isnan(genType const &x)
n
int n
uIsNan
bool uIsNan(const T &value)
Definition: UMath.h:40
vA
Vector vA(size_t i)
list::size
size_t size() const
j
std::ptrdiff_t j
UXCovCoeff
@ UXCovCoeff
Definition: UMath.h:696
glm::sqrt
GLM_FUNC_DECL vecType< T, P > sqrt(vecType< T, P > const &x)
uMax3
T uMax3(const T &a, const T &b, const T &c)
Definition: UMath.h:78
glm::pi
GLM_FUNC_DECL genType pi()
glm::max
GLM_FUNC_DECL genType max(genType const &x, genType const &y)
PlainObjectBase< Matrix< _Scalar, _Rows, _Cols, _Options, _MaxRows, _MaxCols > >::data
EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Scalar * data()
uHamming
std::vector< float > uHamming(unsigned int L)
Definition: UMath.h:919
x2
x2
L
MatrixXd L
x
x
m
Matrix3f m
Eigen::Triplet
uIsInBounds
bool uIsInBounds(const T &value, const T &low, const T &high)
Definition: UMath.h:932
UXCorrRaw
@ UXCorrRaw
Definition: UMath.h:696
UXCorrUnbiased
@ UXCorrUnbiased
Definition: UMath.h:696
UXMatchMethod
UXMatchMethod
Definition: UMath.h:696
UXCorrCoeff
@ UXCorrCoeff
Definition: UMath.h:696
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)
f
Point2(* f)(const Point3 &, OptionalJacobian< 2, 3 >)
uNormalize
std::vector< T > uNormalize(const std::vector< T > &v)
Definition: UMath.h:623
uMeanSquaredError
T uMeanSquaredError(const T *x, unsigned int sizeX, const T *y, unsigned int sizeY)
Definition: UMath.h:453
a
ArrayXXi a
uMinMax
void uMinMax(const T *v, unsigned int size, T &min, T &max, unsigned int &indexMin, unsigned int &indexMax)
Definition: UMath.h:224
x3
x3
N
N
uSign
int uSign(const T &v)
Definition: UMath.h:300
UXCorrBiased
@ UXCorrBiased
Definition: UMath.h:696
uLocalMaxima
std::list< unsigned int > uLocalMaxima(const T *v, unsigned int size)
Definition: UMath.h:645
glm::cos
GLM_FUNC_DECL genType cos(genType const &angle)
diff
diff
v
Array< int, Dynamic, 1 > v
UXCovBiased
@ UXCovBiased
Definition: UMath.h:696
uNormSquared
T uNormSquared(const std::vector< T > &v)
Definition: UMath.h:558
uMax
T uMax(const T *v, unsigned int size, unsigned int &index)
Definition: UMath.h:92
UXCovRaw
@ UXCovRaw
Definition: UMath.h:696
value
value
i
int i
uIsFinite
bool uIsFinite(const T &value)
Definition: UMath.h:53
result
RESULT & result
UXCovUnbiased
@ UXCovUnbiased
Definition: UMath.h:696
list::begin
detail::list_iterator begin() const


rtabmap
Author(s): Mathieu Labbe
autogenerated on Thu Jul 25 2024 02:50:23