00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028 #ifndef MRPT_MATH_H
00029 #define MRPT_MATH_H
00030
00031 #include <mrpt/utils/utils_defs.h>
00032 #include <mrpt/math/CMatrixTemplateNumeric.h>
00033 #include <mrpt/math/CMatrixFixedNumeric.h>
00034 #include <mrpt/math/CVectorTemplate.h>
00035 #include <mrpt/math/vector_ops.h>
00036 #include <mrpt/math/CHistogram.h>
00037
00038 #include <numeric>
00039 #include <cmath>
00040
00041
00042
00043
00044 namespace mrpt
00045 {
00046
00047
00048 namespace math
00049 {
00050 using namespace mrpt::utils;
00051
00052
00053
00054
00055
00056 bool MRPTDLLIMPEXP loadVector( utils::CFileStream &f, std::vector<int> &d);
00057
00058
00059
00060
00061
00062 bool MRPTDLLIMPEXP loadVector( utils::CFileStream &f, std::vector<double> &d);
00063
00064
00065
00066 bool MRPTDLLIMPEXP isNan(float v);
00067
00068
00069
00070 bool MRPTDLLIMPEXP isNan(double v);
00071
00072
00073
00074 bool MRPTDLLIMPEXP isFinite(float v);
00075
00076
00077
00078 bool MRPTDLLIMPEXP isFinite(double v);
00079
00080
00081
00082 template <class T>
00083 size_t countNonZero(const std::vector<T> &a)
00084 {
00085 typename std::vector<T>::const_iterator it_a;
00086 size_t count=0;
00087 for (it_a=a.begin(); it_a!=a.end(); it_a++) if (*it_a) count++;
00088 return count;
00089 }
00090
00091
00092
00093 template<class T>
00094 T maximum(const std::vector<T> &v, unsigned int *maxIndex = NULL)
00095 {
00096 typename std::vector<T>::const_iterator maxIt = std::max_element(v.begin(),v.end());
00097 if (maxIndex) *maxIndex = static_cast<unsigned int>( std::distance(v.begin(),maxIt) );
00098 return *maxIt;
00099 }
00100
00101
00102
00103 template<class T>
00104 T norm_inf(const std::vector<T> &v, unsigned int *maxIndex = NULL)
00105 {
00106 double M=0;
00107 int i,M_idx=-1;
00108 typename std::vector<T>::const_iterator it;
00109 for (i=0, it=v.begin(); it!=v.end();it++,i++)
00110 {
00111 double it_abs = fabs( static_cast<double>(*it));
00112 if (it_abs>M || M_idx==-1)
00113 {
00114 M = it_abs;
00115 M_idx = i;
00116 }
00117 }
00118 if (maxIndex) *maxIndex = M_idx;
00119 return static_cast<T>(M);
00120 }
00121
00122
00123
00124
00125 template<class T>
00126 T norm(const std::vector<T> &v)
00127 {
00128 T total=0;
00129 typename std::vector<T>::const_iterator it;
00130 for (it=v.begin(); it!=v.end();it++)
00131 total += square(*it);
00132 return ::sqrt(total);
00133 }
00134
00135
00136
00137
00138 template <class T>
00139 T minimum(const std::vector<T> &v, unsigned int *minIndex = NULL)
00140 {
00141 typename std::vector<T>::const_iterator minIt = std::min_element(v.begin(),v.end());
00142 if (minIndex) *minIndex = static_cast<unsigned int>( std::distance(v.begin(),minIt) );
00143 return *minIt;
00144 }
00145
00146
00147
00148
00149 template<class T>
00150 void minimum_maximum(const std::vector<T> &v, T& out_min, T& out_max, unsigned int *minIndex = NULL,unsigned int *maxIndex = NULL)
00151 {
00152 size_t N = v.size();
00153 if (N)
00154 {
00155 out_max = out_min = v[0];
00156 unsigned int min_idx=0,max_idx=0;
00157 for (size_t i=0;i<N;i++)
00158 {
00159 if (v[i]<out_min)
00160 {
00161 out_min=v[i];
00162 min_idx = i;
00163 }
00164 if (v[i]>out_max)
00165 {
00166 out_max=v[i];
00167 max_idx = i;
00168 }
00169 }
00170 if (minIndex) *minIndex = min_idx;
00171 if (maxIndex) *maxIndex = max_idx;
00172 }
00173 }
00174
00175
00176
00177
00178 template<class T>
00179 double mean(const std::vector<T> &v)
00180 {
00181 if (v.empty())
00182 return 0;
00183 else return static_cast<double>( std::accumulate(v.begin(),v.end(), static_cast<T>(0) ) ) / v.size();
00184 }
00185
00186
00187
00188
00189 template<class T>
00190 T sum(const std::vector<T> &v)
00191 {
00192 return std::accumulate(v.begin(),v.end(), static_cast<T>(0) );
00193 }
00194
00195
00196 template<typename T,typename K>
00197 void linspace(T first,T last, size_t count, std::vector<K> &out_vector)
00198 {
00199 if (count<2)
00200 {
00201 out_vector.assign(1,last);
00202 return;
00203 }
00204 else
00205 {
00206 out_vector.resize(count);
00207 const T incr = (last-first)/T(count-1);
00208 T c = first;
00209 for (size_t i=0;i<count;i++,c+=incr)
00210 out_vector[i] = K(c);
00211 }
00212 }
00213
00214
00215 template<class T>
00216 std::vector<T> linspace(T first,T last, size_t count)
00217 {
00218 std::vector<T> ret;
00219 mrpt::math::linspace(first,last,count,ret);
00220 return ret;
00221 }
00222
00223
00224 template<class T>
00225 std::vector<T> ones(size_t count)
00226 {
00227 return std::vector<T>(count,1);
00228 }
00229
00230
00231 template<class T>
00232 std::vector<T> zeros(size_t count)
00233 {
00234 return std::vector<T>(count,0);
00235 }
00236
00237
00238
00239
00240 template<class T>
00241 void normalize(const std::vector<T> &v, std::vector<T> &out_v)
00242 {
00243 T total=0;
00244 typename std::vector<T>::const_iterator it;
00245 for (it=v.begin(); it!=v.end();it++)
00246 total += square(*it);
00247 total = ::sqrt(total);
00248 if (total)
00249 {
00250 out_v.resize(v.size());
00251 typename std::vector<T>::iterator q;
00252 for (it=v.begin(),q=out_v.begin(); q!=out_v.end();it++,q++)
00253 *q = *it / total;
00254 }
00255 else out_v.assign(v.size(),0);
00256 }
00257
00258
00259
00260
00261 template<class T>
00262 std::vector<T> cumsum(const std::vector<T> &v)
00263 {
00264 T last = 0;
00265 std::vector<T> ret(v.size());
00266 typename std::vector<T>::const_iterator it;
00267 typename std::vector<T>::iterator it2;
00268 for (it = v.begin(),it2=ret.begin();it!=v.end();it++,it2++)
00269 last = (*it2) = last + (*it);
00270 return ret;
00271 }
00272
00273
00274
00275
00276 template<class T>
00277 void cumsum(const std::vector<T> &v, std::vector<T> &out_cumsum)
00278 {
00279 T last = 0;
00280 out_cumsum.resize(v.size());
00281 typename std::vector<T>::const_iterator it;
00282 typename std::vector<T>::iterator it2;
00283 for (it = v.begin(),it2=out_cumsum.begin();it!=v.end();it++,it2++)
00284 last = (*it2) = last + (*it);
00285 }
00286
00287
00288
00289
00290
00291
00292 template<class T>
00293 double stddev(const std::vector<T> &v, bool unbiased = true)
00294 {
00295 if (v.size()<2)
00296 return 0;
00297 else
00298 {
00299
00300 typename std::vector<T>::const_iterator it;
00301 double vector_std=0,vector_mean = 0;
00302 for (it = v.begin();it!=v.end();it++) vector_mean += (*it);
00303 vector_mean /= static_cast<double>(v.size());
00304
00305 for (it = v.begin();it!=v.end();it++) vector_std += square((*it)-vector_mean);
00306 vector_std = sqrt(vector_std / static_cast<double>(v.size() - (unbiased ? 1:0)) );
00307
00308 return vector_std;
00309 }
00310 }
00311
00312
00313
00314
00315
00316
00317
00318
00319 template<class T>
00320 void meanAndCov(
00321 const std::vector<std::vector<T> > &v,
00322 vector_double &out_mean,
00323 CMatrixDouble &out_cov
00324 )
00325 {
00326 const size_t N = v.size();
00327 ASSERTMSG_(N>0,"The input vector contains no elements");
00328 const double N_inv = 1.0/N;
00329
00330 const size_t M = v[0].size();
00331 ASSERTMSG_(M>0,"The input vector contains rows of length 0");
00332
00333
00334 out_mean.assign(M,0);
00335 for (size_t i=0;i<N;i++)
00336 for (size_t j=0;j<M;j++)
00337 out_mean[j]+=v[i][j];
00338 out_mean*=N_inv;
00339
00340
00341
00342
00343 out_cov.zeros(M,M);
00344 for (size_t i=0;i<N;i++)
00345 {
00346 for (size_t j=0;j<M;j++)
00347 out_cov.get_unsafe(j,j)+=square(v[i][j]-out_mean[j]);
00348
00349 for (size_t j=0;j<M;j++)
00350 for (size_t k=j+1;k<M;k++)
00351 out_cov.get_unsafe(j,k)+=(v[i][j]-out_mean[j])*(v[i][k]-out_mean[k]);
00352 }
00353 for (size_t j=0;j<M;j++)
00354 for (size_t k=j+1;k<M;k++)
00355 out_cov.get_unsafe(k,j) = out_cov.get_unsafe(j,k);
00356 out_cov*=N_inv;
00357 }
00358
00359
00360
00361
00362
00363
00364 template<class T>
00365 CMatrixDouble cov( const std::vector<std::vector<T> > &v )
00366 {
00367 vector_double m;
00368 CMatrixDouble C;
00369 meanAndCov(v,m,C);
00370 return C;
00371 }
00372
00373
00374
00375
00376
00377
00378
00379
00380 template<class T>
00381 void meanAndStd(
00382 const std::vector<T> &v,
00383 double &out_mean,
00384 double &out_std,
00385 bool unbiased = true)
00386 {
00387 if (v.size()<2)
00388 {
00389 out_std = 0;
00390 if (v.size()==1)
00391 out_mean = v[0];
00392 }
00393 else
00394 {
00395
00396 typename std::vector<T>::const_iterator it;
00397 out_std=0,out_mean = 0;
00398 for (it = v.begin();it!=v.end();it++) out_mean += (*it);
00399 out_mean /= static_cast<double>(v.size());
00400
00401
00402 for (it = v.begin();it!=v.end();it++) out_std += square(static_cast<double>(*it)-out_mean);
00403 out_std = sqrt(out_std / static_cast<double>((v.size() - (unbiased ? 1:0)) ));
00404 }
00405 }
00406
00407
00408
00409
00410
00411
00412
00413
00414 template<class T>
00415 void weightedHistogram(
00416 const std::vector<T> &values,
00417 const std::vector<T> &weights,
00418 float binWidth,
00419 std::vector<float> &out_binCenters,
00420 std::vector<float> &out_binValues )
00421 {
00422 MRPT_TRY_START;
00423
00424 ASSERT_( values.size() == weights.size() );
00425 ASSERT_( binWidth > 0 );
00426 T minBin = minimum( values );
00427 unsigned int nBins = static_cast<unsigned>(ceil((maximum( values )-minBin) / binWidth));
00428
00429
00430 out_binCenters.resize(nBins);
00431 out_binValues.clear(); out_binValues.resize(nBins,0);
00432 float halfBin = 0.5f*binWidth;;
00433 vector_float binBorders(nBins+1,minBin-halfBin);
00434 for (unsigned int i=0;i<nBins;i++)
00435 {
00436 binBorders[i+1] = binBorders[i]+binWidth;
00437 out_binCenters[i] = binBorders[i]+halfBin;
00438 }
00439
00440
00441 float totalSum = 0;
00442 for (typename std::vector<T>::const_iterator itVal = values.begin(), itW = weights.begin(); itVal!=values.end(); ++itVal, ++itW )
00443 {
00444 int idx = round(((*itVal)-minBin)/binWidth);
00445 if (idx>=nBins) idx=nBins-1;
00446 ASSERT_(idx>=0);
00447 out_binValues[idx] += *itW;
00448 totalSum+= *itW;
00449 }
00450
00451 if (totalSum)
00452 out_binValues = out_binValues / totalSum;
00453
00454
00455 MRPT_TRY_END;
00456 }
00457
00458
00459
00460
00461 uint64_t MRPTDLLIMPEXP factorial64(unsigned int n);
00462
00463
00464
00465 double MRPTDLLIMPEXP factorial(unsigned int n);
00466
00467
00468
00469
00470
00471
00472 template <class T>
00473 void wrapTo2PiInPlace(T &a)
00474 {
00475 bool was_neg = a<0;
00476 a = fmod(a, static_cast<T>(M_2PI) );
00477 if (was_neg) a+=static_cast<T>(M_2PI);
00478 }
00479
00480
00481
00482
00483
00484 template <class T>
00485 T wrapTo2Pi(T a)
00486 {
00487 wrapTo2PiInPlace(a);
00488 return a;
00489 }
00490
00491
00492
00493
00494
00495 template <class T>
00496 T wrapToPi(T a)
00497 {
00498 return wrapTo2Pi( a + static_cast<T>(M_PI) )-static_cast<T>(M_PI);
00499 }
00500
00501
00502
00503
00504
00505 template <class T>
00506 void wrapToPiInPlace(T &a)
00507 {
00508 a = wrapToPi(a);
00509 }
00510
00511
00512
00513 template <class T>
00514 T round2up(T val)
00515 {
00516 T n = 1;
00517 while (n < val)
00518 {
00519 n <<= 1;
00520 if (n<=1)
00521 THROW_EXCEPTION("Overflow!");
00522 }
00523 return n;
00524 }
00525
00526
00527
00528
00529 template <class T>
00530 T round_10power(T val, int power10)
00531 {
00532 long double F = ::pow((long double)10.0,-(long double)power10);
00533 long int t = round_long( val * F );
00534 return T(t/F);
00535 }
00536
00537
00538
00539
00540
00541
00542
00543 template<class T>
00544 void chol(const CMatrixTemplateNumeric<T> &in,CMatrixTemplateNumeric<T> &out)
00545 {
00546 if (in.getColCount() != in.getRowCount()) THROW_EXCEPTION("Cholesky factorization error, in matrix not square");
00547 size_t i,j,k;
00548 T sum;
00549 out.setSize(in.getRowCount(),in.getColCount());
00550 for (i=0;i<in.getRowCount();i++)
00551 {
00552 for (j=i;j<in.getColCount();j++)
00553 {
00554 sum=in(i,j);
00555 for (k=i-1;(k>=0)&(k<in.getColCount());k--)
00556 {
00557 sum -= out(k,i)*out(k,j);
00558 }
00559 if (i==j)
00560 {
00561 if (sum<0)
00562 {
00563 THROW_EXCEPTION("Cholesky factorization error, in matrix not defined-positive");
00564 }
00565 out(i,j)=sqrt(sum);
00566 }
00567 else
00568 {
00569 out(i,j)=sum/out(i,i);
00570 out(j,i)=0;
00571 }
00572 }
00573 }
00574 }
00575
00576
00577
00578
00579 template<class T>
00580 double correlate_matrix(const CMatrixTemplateNumeric<T> &a1, const CMatrixTemplateNumeric<T> &a2)
00581 {
00582 if ((a1.getColCount()!=a2.getColCount())|(a1.getRowCount()!=a2.getRowCount()))
00583 THROW_EXCEPTION("Correlation Error!, images with no same size");
00584
00585 int i,j;
00586 T x1,x2;
00587 T syy=0, sxy=0, sxx=0, m1=0, m2=0 ,n=a1.getRowCount()*a2.getColCount();
00588
00589
00590 for (i=0;i<a1.getRowCount();i++)
00591 {
00592 for (j=0;j<a1.getColCount();j++)
00593 {
00594 m1 += a1(i,j);
00595 m2 += a2(i,j);
00596 }
00597 }
00598 m1 /= n;
00599 m2 /= n;
00600
00601 for (i=0;i<a1.getRowCount();i++)
00602 {
00603 for (j=0;j<a1.getColCount();j++)
00604 {
00605 x1 = a1(i,j) - m1;
00606 x2 = a2(i,j) - m2;
00607 sxx += x1*x1;
00608 syy += x2*x2;
00609 sxy += x1*x2;
00610 }
00611 }
00612
00613 return sxy / sqrt(sxx * syy);
00614 }
00615
00616
00617
00618
00619
00620
00621
00622
00623
00624
00625
00626
00627
00628
00629 template<class T>
00630 void MRPTDLLIMPEXP qr_decomposition(
00631 CMatrixTemplateNumeric<T> &A,
00632 CMatrixTemplateNumeric<T> &R,
00633 CMatrixTemplateNumeric<T> &Q,
00634 CVectorTemplate<T> &c,
00635 int &sing);
00636
00637
00638
00639
00640
00641 template<class T>
00642 void MRPTDLLIMPEXP UpdateCholesky(
00643 CMatrixTemplateNumeric<T> &chol,
00644 CVectorTemplate<T> &r1Modification);
00645
00646
00647
00648
00649
00650
00651
00652 void MRPTDLLIMPEXP computeEigenValues2x2(
00653 const CMatrixFloat &in_matrix,
00654 float &min_eigenvalue,
00655 float &max_eigenvalue );
00656
00657
00658
00659
00660 template<class T>
00661 std::vector<T> Exp(const std::vector<T> &v)
00662 {
00663 std::vector<T> ret(v.size());
00664 typename std::vector<T>::const_iterator it;
00665 typename std::vector<T>::iterator it2;
00666 for (it = v.begin(),it2=ret.begin();it!=v.end();it++,it2++) *it2 = ::exp(*it);
00667 return ret;
00668 }
00669
00670
00671
00672
00673 template<class T>
00674 std::vector<T> Log(const std::vector<T> &v)
00675 {
00676 std::vector<T> ret(v.size());
00677 typename std::vector<T>::const_iterator it;
00678 typename std::vector<T>::iterator it2;
00679 for (it = v.begin(),it2=ret.begin();it!=v.end();it++,it2++) *it2 = ::log(*it);
00680 return ret;
00681 }
00682
00683
00684
00685
00686
00687
00688
00689
00690 double MRPTDLLIMPEXP averageLogLikelihood( const vector_double &logLikelihoods );
00691
00692
00693
00694 double MRPTDLLIMPEXP averageWrap2Pi(const vector_double &angles );
00695
00696
00697
00698
00699
00700
00701
00702
00703 double MRPTDLLIMPEXP averageLogLikelihood(
00704 const vector_double &logWeights,
00705 const vector_double &logLikelihoods );
00706
00707
00708
00709
00710
00711
00712
00713
00714 std::string MRPTDLLIMPEXP MATLAB_plotCovariance2D(
00715 const CMatrixFloat &cov22,
00716 const CVectorFloat &mean,
00717 const float &stdCount,
00718 const std::string &style = std::string("b"),
00719 const size_t &nEllipsePoints = 30 );
00720
00721
00722
00723
00724
00725
00726
00727
00728 std::string MRPTDLLIMPEXP MATLAB_plotCovariance2D(
00729 const CMatrixDouble &cov22,
00730 const CVectorDouble &mean,
00731 const float &stdCount,
00732 const std::string &style = std::string("b"),
00733 const size_t &nEllipsePoints = 30 );
00734
00735
00736
00737
00738 void MRPTDLLIMPEXP homogeneousMatrixInverse(
00739 const CMatrixDouble &M,
00740 CMatrixDouble &out_inverse_M);
00741
00742
00743
00744 void MRPTDLLIMPEXP homogeneousMatrixInverse(
00745 const CMatrixDouble44 &M,
00746 CMatrixDouble44 &out_inverse_M);
00747
00748
00749
00750
00751 template<class T>
00752 size_t countCommonElements(
00753 const std::vector<T> &a,
00754 const std::vector<T> &b)
00755 {
00756 size_t ret=0;
00757 typename std::vector<T>::const_iterator it1;
00758 typename std::vector<T>::const_iterator it2;
00759 for (it1 = a.begin();it1!=a.end();it1++)
00760 for (it2 = b.begin();it2!=b.end();it2++)
00761 if ( (*it1) == (*it2) )
00762 ret++;
00763
00764 return ret;
00765 }
00766
00767
00768
00769
00770
00771
00772 template <typename T, class USERPARAM >
00773 void estimateJacobian(
00774 const std::vector<T> &x,
00775 void (*functor)(const std::vector<T> &x,const USERPARAM &y, std::vector<T> &out),
00776 const std::vector<T> &increments,
00777 const USERPARAM &userParam,
00778 CMatrixTemplateNumeric<T> &out_Jacobian )
00779 {
00780 MRPT_TRY_START;
00781 ASSERT_(x.size()>0 && increments.size() == x.size());
00782
00783 size_t m = 0;
00784 const size_t n = x.size();
00785
00786 for (size_t j=0;j<n;j++) { ASSERT_( increments[j]>0 ) }
00787
00788 std::vector<T> f_minus, f_plus;
00789 std::vector<T> x_mod(x);
00790
00791
00792 for (size_t j=0;j<n;j++)
00793 {
00794
00795 x_mod[j]+=increments[j];
00796 functor(x_mod,userParam, f_plus);
00797
00798 x_mod[j]=x[j]-increments[j];
00799 functor(x_mod,userParam, f_minus);
00800
00801 x_mod[j]=x[j];
00802 const T Ax_2_inv = T(0.5)/increments[j];
00803
00804
00805 if (j==0)
00806 {
00807 m = f_plus.size();
00808 out_Jacobian.setSize(m,n);
00809 }
00810
00811 for (size_t i=0;i<m;i++)
00812 {
00813 out_Jacobian(i,j) = Ax_2_inv* (f_plus[i]-f_minus[i]);
00814 }
00815
00816 }
00817
00818 MRPT_TRY_END;
00819 }
00820
00821
00822
00823
00824
00825
00826
00827
00828 template <class T>
00829 T interpolate(
00830 const T &x,
00831 const std::vector<T> &ys,
00832 const T &x0,
00833 const T &x1 )
00834 {
00835 MRPT_TRY_START
00836 ASSERT_(x1>x0); ASSERT_(!ys.empty());
00837 const size_t N = ys.size();
00838 if (x<=x0) return ys[0];
00839 if (x>=x1) return ys[N-1];
00840 const T Ax = (x1-x0)/T(N);
00841 const size_t i = int( (x-x0)/Ax );
00842 if (i>=N-1) return ys[N-1];
00843 const T Ay = ys[i+1]-ys[i];
00844 return ys[i] + (x-(x0+i*Ax))*Ay/Ax;
00845 MRPT_TRY_END
00846 }
00847
00848
00849
00850
00851
00852 double MRPTDLLIMPEXP interpolate2points(const double x, const double x0, const double y0, const double x1, const double y1, bool wrap2pi = false);
00853
00854
00855
00856
00857
00858 double MRPTDLLIMPEXP spline(const double t, const std::vector<double> &x, const std::vector<double> &y, bool wrap2pi = false);
00859
00860
00861
00862
00863
00864
00865 double MRPTDLLIMPEXP leastSquareLinearFit(const double t, const std::vector<double> &x, const std::vector<double> &y, bool wrap2pi = false);
00866
00867
00868
00869
00870 void MRPTDLLIMPEXP leastSquareLinearFit(const std::vector<double> &ts, std::vector<double> &outs, const std::vector<double> &x, const std::vector<double> &y, bool wrap2pi = false);
00871
00872
00873
00874
00875
00876 template<class T>
00877 vector_double histogram(const std::vector<T> &v, double limit_min, double limit_max, size_t number_bins, bool do_normalization = false )
00878 {
00879 mrpt::math::CHistogram H( limit_min, limit_max, number_bins );
00880 vector_double ret(number_bins);
00881 size_t i;
00882 for (i=0;i<v.size();i++) H.add(static_cast<double>( v[i] ));
00883 for (i=0;i<number_bins;i++) ret[i] = do_normalization ? H.getBinRatio(i) : H.getBinCount(i);
00884 return ret;
00885 }
00886
00887
00888
00889
00890
00891
00892
00893
00894
00895 template <typename T, typename At, size_t N>
00896 std::vector<T>& loadVector( std::vector<T> &v, At (&theArray)[N] )
00897 {
00898 MRPT_COMPILE_TIME_ASSERT(N!=0)
00899 v.resize(N);
00900 for (size_t i=0; i < N; i++)
00901 v[i] = static_cast<T>(theArray[i]);
00902 return v;
00903 }
00904
00905
00906 template <class T>
00907 std::vector<T> Abs(const std::vector<T> &a)
00908 {
00909 typename std::vector<T> res(a.size());
00910 for (size_t i=0;i<a.size();i++)
00911 res[i] = static_cast<T>( fabs( static_cast<double>( a[i] ) ) );
00912 return res;
00913 }
00914
00915
00916
00917
00918 void unwrap2PiSequence(vector_double &x);
00919
00920
00921
00922
00923 template<class T>
00924 T MRPTDLLIMPEXP mahalanobisDistance(
00925 const std::vector<T> &X,
00926 const std::vector<T> &MU,
00927 const CMatrixTemplateNumeric<T> &COV_inv );
00928
00929
00930
00931
00932
00933 template<typename T,size_t N,typename U>
00934 void covariancesAndMean(const std::vector<T> &elements,CMatrixTemplateNumeric<U> &covariances,U (&means)[N]) {
00935 size_t nElms=elements.size();
00936 for (size_t i=0;i<N;i++) {
00937 means[i]=0;
00938 for (size_t j=0;j<nElms;j++) means[i]+=elements[j][i];
00939 means[i]/=nElms;
00940 }
00941 covariances.resize(N,N);
00942 for (size_t i=0;i<N;i++) for (size_t j=0;j<=i;j++) {
00943 U elem=0;
00944 for (size_t k=0;k<nElms;k++) elem+=(elements[k][i]-means[i])*(elements[k][j]-means[j]);
00945 elem/=nElms;
00946 covariances.get_unsafe(i,j) = elem;
00947 if (i!=j) covariances.get_unsafe(j,i)=elem;
00948 }
00949 }
00950
00951
00952
00953
00954
00955 template<typename T,size_t N,typename U>
00956 void covariancesAndMean(const std::vector<T> &elements,CMatrixFixedNumeric<U,N,N> &covariances,U (&means)[N]) {
00957 size_t nElms=elements.size();
00958 for (size_t i=0;i<N;i++) {
00959 means[i]=0;
00960 for (size_t j=0;j<nElms;j++) means[i]+=elements[j][i];
00961 means[i]/=nElms;
00962 }
00963 for (size_t i=0;i<N;i++) for (size_t j=0;j<=i;j++) {
00964 U elem=0;
00965 for (size_t k=0;k<nElms;k++) elem+=(elements[k][i]-means[i])*(elements[k][j]-means[j]);
00966 elem/=nElms;
00967 covariances.get_unsafe(i,j) = elem;
00968 if (i!=j) covariances.get_unsafe(j,i)=elem;
00969 }
00970 }
00971
00972
00973
00974
00975
00976 template<typename T,size_t N,typename U>
00977 void covariancesAndMean(const std::vector<T> &elements,CMatrixFixedNumeric<U,N,N> &covariances,std::vector<U> &means ) {
00978 means.resize(N);
00979 size_t nElms=elements.size();
00980 for (size_t i=0;i<N;i++) {
00981 means[i]=0;
00982 for (size_t j=0;j<nElms;j++) means[i]+=elements[j][i];
00983 means[i]/=nElms;
00984 }
00985 for (size_t i=0;i<N;i++) for (size_t j=0;j<=i;j++) {
00986 U elem=0;
00987 for (size_t k=0;k<nElms;k++) elem+=(elements[k][i]-means[i])*(elements[k][j]-means[j]);
00988 elem/=nElms;
00989 covariances.get_unsafe(i,j) = elem;
00990 if (i!=j) covariances.get_unsafe(j,i)=elem;
00991 }
00992 }
00993
00994
00995 template<size_t N,class T,class U,class V>
00996 inline T dotProduct(const U &v1,const V &v2) {
00997 T res=0;
00998 for (size_t i=0;i<N;i++) res+=v1[i]*v2[i];
00999 return res;
01000 }
01001
01002
01003 template<size_t N,class T,class U>
01004 inline T squareNorm(const U &v) {
01005 T res=0;
01006 for (size_t i=0;i<N;i++) res+=v[i]*v[i];
01007 return res;
01008 }
01009 }
01010
01011 }
01012
01013 #endif