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/CVectorTemplate.h>
00034 #include <mrpt/math/vector_ops.h>
00035 #include <mrpt/math/CHistogram.h>
00036
00037 #include <numeric>
00038 #include <cmath>
00039
00040
00041
00042
00043 namespace mrpt
00044 {
00047 namespace math
00048 {
00049 using namespace mrpt::utils;
00050
00055 bool MRPTDLLIMPEXP loadVector( utils::CFileStream &f, std::vector<int> &d);
00056
00061 bool MRPTDLLIMPEXP loadVector( utils::CFileStream &f, std::vector<double> &d);
00062
00065 bool MRPTDLLIMPEXP isNan(float v);
00066
00069 bool MRPTDLLIMPEXP isNan(double v);
00070
00073 bool MRPTDLLIMPEXP isFinite(float v);
00074
00077 bool MRPTDLLIMPEXP isFinite(double v);
00078
00081 template <class T>
00082 size_t countNonZero(const std::vector<T> &a)
00083 {
00084 typename std::vector<T>::const_iterator it_a;
00085 size_t count=0;
00086 for (it_a=a.begin(); it_a!=a.end(); it_a++) if (*it_a) count++;
00087 return count;
00088 }
00089
00092 template<class T>
00093 T maximum(const std::vector<T> &v, unsigned int *maxIndex = NULL)
00094 {
00095 typename std::vector<T>::const_iterator maxIt = std::max_element(v.begin(),v.end());
00096 if (maxIndex) *maxIndex = static_cast<unsigned int>( std::distance(v.begin(),maxIt) );
00097 return *maxIt;
00098 }
00099
00102 template<class T>
00103 T norm_inf(const std::vector<T> &v, unsigned int *maxIndex = NULL)
00104 {
00105 double M=0;
00106 int i,M_idx=-1;
00107 typename std::vector<T>::const_iterator it;
00108 for (i=0, it=v.begin(); it!=v.end();it++,i++)
00109 {
00110 double it_abs = fabs( static_cast<double>(*it));
00111 if (it_abs>M || M_idx==-1)
00112 {
00113 M = it_abs;
00114 M_idx = i;
00115 }
00116 }
00117 if (maxIndex) *maxIndex = M_idx;
00118 return static_cast<T>(M);
00119 }
00120
00121
00124 template<class T>
00125 T norm(const std::vector<T> &v)
00126 {
00127 T total=0;
00128 typename std::vector<T>::const_iterator it;
00129 for (it=v.begin(); it!=v.end();it++)
00130 total += square(*it);
00131 return ::sqrt(total);
00132 }
00133
00137 template <class T>
00138 T minimum(const std::vector<T> &v, unsigned int *minIndex = NULL)
00139 {
00140 typename std::vector<T>::const_iterator minIt = std::min_element(v.begin(),v.end());
00141 if (minIndex) *minIndex = static_cast<unsigned int>( std::distance(v.begin(),minIt) );
00142 return *minIt;
00143 }
00144
00148 template<class T>
00149 void minimum_maximum(const std::vector<T> &v, T& out_min, T& out_max, unsigned int *minIndex = NULL,unsigned int *maxIndex = NULL)
00150 {
00151 size_t N = v.size();
00152 if (N)
00153 {
00154 out_max = out_min = v[0];
00155 unsigned int min_idx=0,max_idx=0;
00156 for (size_t i=0;i<N;i++)
00157 {
00158 if (v[i]<out_min)
00159 {
00160 out_min=v[i];
00161 min_idx = i;
00162 }
00163 if (v[i]>out_max)
00164 {
00165 out_max=v[i];
00166 max_idx = i;
00167 }
00168 }
00169 if (minIndex) *minIndex = min_idx;
00170 if (maxIndex) *maxIndex = max_idx;
00171 }
00172 }
00173
00177 template<class T>
00178 double mean(const std::vector<T> &v)
00179 {
00180 if (v.empty())
00181 return 0;
00182 else return static_cast<double>( std::accumulate(v.begin(),v.end(), static_cast<T>(0) ) ) / v.size();
00183 }
00184
00188 template<class T>
00189 T sum(const std::vector<T> &v)
00190 {
00191 return std::accumulate(v.begin(),v.end(), static_cast<T>(0) );
00192 }
00193
00195 template<typename T,typename K>
00196 void linspace(T first,T last, size_t count, std::vector<K> &out_vector)
00197 {
00198 if (count<2)
00199 {
00200 out_vector.assign(1,last);
00201 return;
00202 }
00203 else
00204 {
00205 out_vector.resize(count);
00206 const T incr = (last-first)/T(count-1);
00207 T c = first;
00208 for (size_t i=0;i<count;i++,c+=incr)
00209 out_vector[i] = K(c);
00210 }
00211 }
00212
00214 template<class T>
00215 std::vector<T> linspace(T first,T last, size_t count)
00216 {
00217 std::vector<T> ret;
00218 mrpt::math::linspace(first,last,count,ret);
00219 return ret;
00220 }
00221
00223 template<class T>
00224 std::vector<T> ones(size_t count)
00225 {
00226 return std::vector<T>(count,1);
00227 }
00228
00230 template<class T>
00231 std::vector<T> zeros(size_t count)
00232 {
00233 return std::vector<T>(count,0);
00234 }
00235
00239 template<class T>
00240 void normalize(const std::vector<T> &v, std::vector<T> &out_v)
00241 {
00242 T total=0;
00243 typename std::vector<T>::const_iterator it;
00244 for (it=v.begin(); it!=v.end();it++)
00245 total += square(*it);
00246 total = ::sqrt(total);
00247 if (total)
00248 {
00249 out_v.resize(v.size());
00250 typename std::vector<T>::iterator q;
00251 for (it=v.begin(),q=out_v.begin(); q!=out_v.end();it++,q++)
00252 *q = *it / total;
00253 }
00254 else out_v.assign(v.size(),0);
00255 }
00256
00260 template<class T>
00261 std::vector<T> cumsum(const std::vector<T> &v)
00262 {
00263 T last = 0;
00264 std::vector<T> ret(v.size());
00265 typename std::vector<T>::const_iterator it;
00266 typename std::vector<T>::iterator it2;
00267 for (it = v.begin(),it2=ret.begin();it!=v.end();it++,it2++)
00268 last = (*it2) = last + (*it);
00269 return ret;
00270 }
00271
00275 template<class T>
00276 void cumsum(const std::vector<T> &v, std::vector<T> &out_cumsum)
00277 {
00278 T last = 0;
00279 out_cumsum.resize(v.size());
00280 typename std::vector<T>::const_iterator it;
00281 typename std::vector<T>::iterator it2;
00282 for (it = v.begin(),it2=out_cumsum.begin();it!=v.end();it++,it2++)
00283 last = (*it2) = last + (*it);
00284 }
00285
00291 template<class T>
00292 double stddev(const std::vector<T> &v, bool unbiased = true)
00293 {
00294 if (v.size()<2)
00295 return 0;
00296 else
00297 {
00298
00299 typename std::vector<T>::const_iterator it;
00300 double vector_std=0,vector_mean = 0;
00301 for (it = v.begin();it!=v.end();it++) vector_mean += (*it);
00302 vector_mean /= static_cast<double>(v.size());
00303
00304 for (it = v.begin();it!=v.end();it++) vector_std += square((*it)-vector_mean);
00305 vector_std = sqrt(vector_std / static_cast<double>(v.size() - (unbiased ? 1:0)) );
00306
00307 return vector_std;
00308 }
00309 }
00310
00318 template<class T>
00319 void meanAndStd(
00320 const std::vector<T> &v,
00321 double &out_mean,
00322 double &out_std,
00323 bool unbiased = true)
00324 {
00325 if (v.size()<2)
00326 {
00327 out_std = 0;
00328 if (v.size()==1)
00329 out_mean = v[0];
00330 }
00331 else
00332 {
00333
00334 typename std::vector<T>::const_iterator it;
00335 out_std=0,out_mean = 0;
00336 for (it = v.begin();it!=v.end();it++) out_mean += (*it);
00337 out_mean /= static_cast<double>(v.size());
00338
00339
00340 for (it = v.begin();it!=v.end();it++) out_std += square(static_cast<double>(*it)-out_mean);
00341 out_std = sqrt(out_std / static_cast<double>((v.size() - (unbiased ? 1:0)) ));
00342 }
00343 }
00344
00352 template<class T>
00353 void weightedHistogram(
00354 const std::vector<T> &values,
00355 const std::vector<T> &weights,
00356 float binWidth,
00357 std::vector<float> &out_binCenters,
00358 std::vector<float> &out_binValues )
00359 {
00360 MRPT_TRY_START;
00361
00362 ASSERT_( values.size() == weights.size() );
00363 ASSERT_( binWidth > 0 );
00364 T minBin = minimum( values );
00365 unsigned int nBins = static_cast<unsigned>(ceil((maximum( values )-minBin) / binWidth));
00366
00367
00368 out_binCenters.resize(nBins);
00369 out_binValues.clear(); out_binValues.resize(nBins,0);
00370 float halfBin = 0.5f*binWidth;;
00371 vector_float binBorders(nBins+1,minBin-halfBin);
00372 for (unsigned int i=0;i<nBins;i++)
00373 {
00374 binBorders[i+1] = binBorders[i]+binWidth;
00375 out_binCenters[i] = binBorders[i]+halfBin;
00376 }
00377
00378
00379 float totalSum = 0;
00380 for (typename std::vector<T>::const_iterator itVal = values.begin(), itW = weights.begin(); itVal!=values.end(); itVal++, itW++ )
00381 {
00382 int idx = round(((*itVal)-minBin)/binWidth);
00383 if (idx>=nBins) idx=nBins-1;
00384 ASSERT_(idx>=0);
00385 out_binValues[idx] += *itW;
00386 totalSum+= *itW;
00387 }
00388
00389 if (totalSum)
00390 out_binValues = out_binValues / totalSum;
00391
00392
00393 MRPT_TRY_END;
00394 }
00395
00396
00399 uint64_t MRPTDLLIMPEXP factorial64(unsigned int n);
00400
00403 double MRPTDLLIMPEXP factorial(unsigned int n);
00404
00405
00410 template <class T>
00411 void wrapTo2PiInPlace(T &a)
00412 {
00413 bool was_neg = a<0;
00414 a = fmod(a, static_cast<T>(M_2PI) );
00415 if (was_neg) a+=static_cast<T>(M_2PI);
00416 }
00417
00422 template <class T>
00423 T wrapTo2Pi(T a)
00424 {
00425 wrapTo2PiInPlace(a);
00426 return a;
00427 }
00428
00433 template <class T>
00434 T wrapToPi(T a)
00435 {
00436 return wrapTo2Pi( a + static_cast<T>(M_PI) )-static_cast<T>(M_PI);
00437 }
00438
00443 template <class T>
00444 void wrapToPiInPlace(T &a)
00445 {
00446 a = wrapToPi(a);
00447 }
00448
00451 template <class T>
00452 T round2up(T val)
00453 {
00454 T n = 1;
00455 while (n < val)
00456 {
00457 n <<= 1;
00458 if (n<=1)
00459 THROW_EXCEPTION("Overflow!");
00460 }
00461 return n;
00462 }
00463
00467 template <class T>
00468 T round_10power(T val, int power10)
00469 {
00470 long double F = ::pow((long double)10.0,-(long double)power10);
00471 long int t = round_long( val * F );
00472 return T(t/F);
00473 }
00474
00481 template<class T>
00482 void chol(const CMatrixTemplateNumeric<T> &in,CMatrixTemplateNumeric<T> &out)
00483 {
00484 if (in.getColCount() != in.getRowCount()) THROW_EXCEPTION("Cholesky factorization error, in matrix not square");
00485 size_t i,j,k;
00486 T sum;
00487 out.setSize(in.getRowCount(),in.getColCount());
00488 for (i=0;i<in.getRowCount();i++)
00489 {
00490 for (j=i;j<in.getColCount();j++)
00491 {
00492 sum=in(i,j);
00493 for (k=i-1;(k>=0)&(k<in.getColCount());k--)
00494 {
00495 sum -= out(k,i)*out(k,j);
00496 }
00497 if (i==j)
00498 {
00499 if (sum<0)
00500 {
00501 THROW_EXCEPTION("Cholesky factorization error, in matrix not defined-positive");
00502 }
00503 out(i,j)=sqrt(sum);
00504 }
00505 else
00506 {
00507 out(i,j)=sum/out(i,i);
00508 out(j,i)=0;
00509 }
00510 }
00511 }
00512 }
00513
00517 template<class T>
00518 double correlate_matrix(const CMatrixTemplateNumeric<T> &a1, const CMatrixTemplateNumeric<T> &a2)
00519 {
00520 if ((a1.getColCount()!=a2.getColCount())|(a1.getRowCount()!=a2.getRowCount()))
00521 THROW_EXCEPTION("Correlation Error!, images with no same size");
00522
00523 int i,j;
00524 T x1,x2;
00525 T syy=0, sxy=0, sxx=0, m1=0, m2=0 ,n=a1.getRowCount()*a2.getColCount();
00526
00527
00528 for (i=0;i<a1.getRowCount();i++)
00529 {
00530 for (j=0;j<a1.getColCount();j++)
00531 {
00532 m1 += a1(i,j);
00533 m2 += a2(i,j);
00534 }
00535 }
00536 m1 /= n;
00537 m2 /= n;
00538
00539 for (i=0;i<a1.getRowCount();i++)
00540 {
00541 for (j=0;j<a1.getColCount();j++)
00542 {
00543 x1 = a1(i,j) - m1;
00544 x2 = a2(i,j) - m2;
00545 sxx += x1*x1;
00546 syy += x2*x2;
00547 sxy += x1*x2;
00548 }
00549 }
00550
00551 return sxy / sqrt(sxx * syy);
00552 }
00553
00554
00567 template<class T>
00568 void MRPTDLLIMPEXP qr_decomposition(
00569 CMatrixTemplateNumeric<T> &A,
00570 CMatrixTemplateNumeric<T> &R,
00571 CMatrixTemplateNumeric<T> &Q,
00572 CVectorTemplate<T> &c,
00573 int &sing);
00574
00575
00579 template<class T>
00580 void MRPTDLLIMPEXP UpdateCholesky(
00581 CMatrixTemplateNumeric<T> &chol,
00582 CVectorTemplate<T> &r1Modification);
00583
00590 void MRPTDLLIMPEXP computeEigenValues2x2(
00591 const CMatrixFloat &in_matrix,
00592 float &min_eigenvalue,
00593 float &max_eigenvalue );
00594
00598 template<class T>
00599 std::vector<T> Exp(const std::vector<T> &v)
00600 {
00601 std::vector<T> ret(v.size());
00602 typename std::vector<T>::const_iterator it;
00603 typename std::vector<T>::iterator it2;
00604 for (it = v.begin(),it2=ret.begin();it!=v.end();it++,it2++) *it2 = ::exp(*it);
00605 return ret;
00606 }
00607
00611 template<class T>
00612 std::vector<T> Log(const std::vector<T> &v)
00613 {
00614 std::vector<T> ret(v.size());
00615 typename std::vector<T>::const_iterator it;
00616 typename std::vector<T>::iterator it2;
00617 for (it = v.begin(),it2=ret.begin();it!=v.end();it++,it2++) *it2 = ::log(*it);
00618 return ret;
00619 }
00620
00628 double MRPTDLLIMPEXP averageLogLikelihood( const vector_double &logLikelihoods );
00629
00632 double MRPTDLLIMPEXP averageWrap2Pi(const vector_double &angles );
00633
00641 double MRPTDLLIMPEXP averageLogLikelihood(
00642 const vector_double &logWeights,
00643 const vector_double &logLikelihoods );
00644
00652 std::string MRPTDLLIMPEXP MATLAB_plotCovariance2D(
00653 const CMatrixFloat &cov22,
00654 const CVectorFloat &mean,
00655 const float &stdCount,
00656 const std::string &style = std::string("b"),
00657 const size_t &nEllipsePoints = 30 );
00658
00666 std::string MRPTDLLIMPEXP MATLAB_plotCovariance2D(
00667 const CMatrixDouble &cov22,
00668 const CVectorDouble &mean,
00669 const float &stdCount,
00670 const std::string &style = std::string("b"),
00671 const size_t &nEllipsePoints = 30 );
00672
00673
00676 void MRPTDLLIMPEXP homogeneousMatrixInverse(
00677 const CMatrixDouble &M,
00678 CMatrixDouble &out_inverse_M);
00679
00683 template<class T>
00684 size_t countCommonElements(
00685 const std::vector<T> &a,
00686 const std::vector<T> &b)
00687 {
00688 size_t ret=0;
00689 typename std::vector<T>::const_iterator it1;
00690 typename std::vector<T>::const_iterator it2;
00691 for (it1 = a.begin();it1!=a.end();it1++)
00692 for (it2 = b.begin();it2!=b.end();it2++)
00693 if ( (*it1) == (*it2) )
00694 ret++;
00695
00696 return ret;
00697 }
00698
00699
00702 template <typename T>
00703 void MRPTDLLIMPEXP estimateJacobian(
00704 const std::vector<T> &x,
00705 void (*functor)(const std::vector<T> &x,const std::vector<T> &y, std::vector<T> &out),
00706 const std::vector<T> &increments,
00707 const std::vector<T> &userParam,
00708 CMatrixTemplateNumeric<T> &out_Jacobian );
00709
00717 template <class T>
00718 T interpolate(
00719 const T &x,
00720 const std::vector<T> &ys,
00721 const T &x0,
00722 const T &x1 )
00723 {
00724 MRPT_TRY_START
00725 ASSERT_(x1>x0); ASSERT_(!ys.empty());
00726 const size_t N = ys.size();
00727 if (x<=x0) return ys[0];
00728 if (x>=x1) return ys[N-1];
00729 const T Ax = (x1-x0)/T(N);
00730 const size_t i = int( (x-x0)/Ax );
00731 if (i>=N-1) return ys[N-1];
00732 const T Ay = ys[i+1]-ys[i];
00733 return ys[i] + (x-(x0+i*Ax))*Ay/Ax;
00734 MRPT_TRY_END
00735 }
00736
00741 double MRPTDLLIMPEXP interpolate2points(const double x, const double x0, const double y0, const double x1, const double y1, bool wrap2pi = false);
00742
00747 double MRPTDLLIMPEXP spline(const double t, const std::vector<double> &x, const std::vector<double> &y, bool wrap2pi = false);
00748
00754 double MRPTDLLIMPEXP leastSquareLinearFit(const double t, const std::vector<double> &x, const std::vector<double> &y, bool wrap2pi = false);
00755
00759 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);
00760
00765 template<class T>
00766 vector_double histogram(const std::vector<T> &v, double limit_min, double limit_max, size_t number_bins, bool do_normalization = false )
00767 {
00768 mrpt::math::CHistogram H( limit_min, limit_max, number_bins );
00769 vector_double ret(number_bins);
00770 size_t i;
00771 for (i=0;i<v.size();i++) H.add(static_cast<double>( v[i] ));
00772 for (i=0;i<number_bins;i++) ret[i] = do_normalization ? H.getBinRatio(i) : H.getBinCount(i);
00773 return ret;
00774 }
00775
00784 template <typename T, typename At, size_t N>
00785 std::vector<T>& loadVector( std::vector<T> &v, At (&theArray)[N] )
00786 {
00787 MRPT_COMPILE_TIME_ASSERT(N!=0)
00788 v.resize(N);
00789 for (size_t i=0; i < N; i++)
00790 v[i] = static_cast<T>(theArray[i]);
00791 return v;
00792 }
00793
00795 template <class T>
00796 std::vector<T> Abs(const std::vector<T> &a)
00797 {
00798 typename std::vector<T> res(a.size());
00799 for (size_t i=0;i<a.size();i++)
00800 res[i] = static_cast<T>( fabs( static_cast<double>( a[i] ) ) );
00801 return res;
00802 }
00803
00807 void unwrap2PiSequence(vector_double &x);
00808
00812 template<class T>
00813 T MRPTDLLIMPEXP mahalanobisDistance(
00814 const std::vector<T> &X,
00815 const std::vector<T> &MU,
00816 const CMatrixTemplateNumeric<T> &COV_inv );
00817
00818
00819 }
00820
00821 }
00822
00823 #endif