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 in.saveToTextFile("c:\\cholesky_non_defined_positive.txt");
00502 THROW_EXCEPTION("Cholesky factorization error, in matrix not defined-positive");
00503 }
00504 out(i,j)=sqrt(sum);
00505 }
00506 else
00507 {
00508 out(i,j)=sum/out(i,i);
00509 out(j,i)=0;
00510 }
00511 }
00512 }
00513 }
00514
00518 template<class T>
00519 double correlate_matrix(const CMatrixTemplateNumeric<T> &a1, const CMatrixTemplateNumeric<T> &a2)
00520 {
00521 if ((a1.getColCount()!=a2.getColCount())|(a1.getRowCount()!=a2.getRowCount()))
00522 THROW_EXCEPTION("Correlation Error!, images with no same size");
00523
00524 int i,j;
00525 T x1,x2;
00526 T syy=0, sxy=0, sxx=0, m1=0, m2=0 ,n=a1.getRowCount()*a2.getColCount();
00527
00528
00529 for (i=0;i<a1.getRowCount();i++)
00530 {
00531 for (j=0;j<a1.getColCount();j++)
00532 {
00533 m1 += a1(i,j);
00534 m2 += a2(i,j);
00535 }
00536 }
00537 m1 /= n;
00538 m2 /= n;
00539
00540 for (i=0;i<a1.getRowCount();i++)
00541 {
00542 for (j=0;j<a1.getColCount();j++)
00543 {
00544 x1 = a1(i,j) - m1;
00545 x2 = a2(i,j) - m2;
00546 sxx += x1*x1;
00547 syy += x2*x2;
00548 sxy += x1*x2;
00549 }
00550 }
00551
00552 return sxy / sqrt(sxx * syy);
00553 }
00554
00555
00568 template<class T>
00569 void MRPTDLLIMPEXP qr_decomposition(
00570 CMatrixTemplateNumeric<T> &A,
00571 CMatrixTemplateNumeric<T> &R,
00572 CMatrixTemplateNumeric<T> &Q,
00573 CVectorTemplate<T> &c,
00574 int &sing);
00575
00576
00580 template<class T>
00581 void MRPTDLLIMPEXP UpdateCholesky(
00582 CMatrixTemplateNumeric<T> &chol,
00583 CVectorTemplate<T> &r1Modification);
00584
00591 void MRPTDLLIMPEXP computeEigenValues2x2(
00592 const CMatrixFloat &in_matrix,
00593 float &min_eigenvalue,
00594 float &max_eigenvalue );
00595
00599 template<class T>
00600 std::vector<T> Exp(const std::vector<T> &v)
00601 {
00602 std::vector<T> ret(v.size());
00603 typename std::vector<T>::const_iterator it;
00604 typename std::vector<T>::iterator it2;
00605 for (it = v.begin(),it2=ret.begin();it!=v.end();it++,it2++) *it2 = ::exp(*it);
00606 return ret;
00607 }
00608
00612 template<class T>
00613 std::vector<T> Log(const std::vector<T> &v)
00614 {
00615 std::vector<T> ret(v.size());
00616 typename std::vector<T>::const_iterator it;
00617 typename std::vector<T>::iterator it2;
00618 for (it = v.begin(),it2=ret.begin();it!=v.end();it++,it2++) *it2 = ::log(*it);
00619 return ret;
00620 }
00621
00629 double MRPTDLLIMPEXP averageLogLikelihood( const vector_double &logLikelihoods );
00630
00638 double MRPTDLLIMPEXP averageLogLikelihood(
00639 const vector_double &logWeights,
00640 const vector_double &logLikelihoods );
00641
00649 std::string MRPTDLLIMPEXP MATLAB_plotCovariance2D(
00650 const CMatrixFloat &cov22,
00651 const CVectorFloat &mean,
00652 const float &stdCount,
00653 const std::string &style = std::string("b"),
00654 const size_t &nEllipsePoints = 30 );
00655
00663 std::string MRPTDLLIMPEXP MATLAB_plotCovariance2D(
00664 const CMatrixDouble &cov22,
00665 const CVectorDouble &mean,
00666 const float &stdCount,
00667 const std::string &style = std::string("b"),
00668 const size_t &nEllipsePoints = 30 );
00669
00670
00673 void MRPTDLLIMPEXP homogeneousMatrixInverse(
00674 const CMatrixDouble &M,
00675 CMatrixDouble &out_inverse_M);
00676
00680 template<class T>
00681 size_t countCommonElements(
00682 const std::vector<T> &a,
00683 const std::vector<T> &b)
00684 {
00685 size_t ret=0;
00686 typename std::vector<T>::const_iterator it1;
00687 typename std::vector<T>::const_iterator it2;
00688 for (it1 = a.begin();it1!=a.end();it1++)
00689 for (it2 = b.begin();it2!=b.end();it2++)
00690 if ( (*it1) == (*it2) )
00691 ret++;
00692
00693 return ret;
00694 }
00695
00699 template <class T>
00700 T interpolate(
00701 const T &x,
00702 const std::vector<T> &ys,
00703 const T &x0,
00704 const T &x1 )
00705 {
00706 MRPT_TRY_START
00707 ASSERT_(x1>x0); ASSERT_(!ys.empty());
00708 const size_t N = ys.size();
00709 if (x<=x0) return ys[0];
00710 if (x>=x1) return ys[N-1];
00711 const T Ax = (x1-x0)/T(N);
00712 const size_t i = int( (x-x0)/Ax );
00713 if (i>=N-1) return ys[N-1];
00714 const T Ay = ys[i+1]-ys[i];
00715 return ys[i] + (x-(x0+i*Ax))*Ay/Ax;
00716 MRPT_TRY_END
00717 }
00718
00721 void MRPTDLLIMPEXP estimateJacobian(
00722 const vector_double &x,
00723 utils::TFunctor_retVecDbl_inp2VecDbl functor,
00724 const vector_double &increments,
00725 const vector_double &userParam,
00726 math::CMatrixDouble &out_Jacobian );
00727
00731 double MRPTDLLIMPEXP spline(const double t, const std::vector<double> &x, const std::vector<double> &y, bool wrap2pi = false);
00732
00735 template<class T>
00736 vector_double histogram(const std::vector<T> &v, double limit_min, double limit_max, size_t number_bins, bool do_normalization = false )
00737 {
00738 mrpt::math::CHistogram H( limit_min, limit_max, number_bins );
00739 vector_double ret(number_bins);
00740 size_t i;
00741 for (i=0;i<v.size();i++) H.add(static_cast<double>( v[i] ));
00742 for (i=0;i<number_bins;i++) ret[i] = do_normalization ? H.getBinRatio(i) : H.getBinCount(i);
00743 return ret;
00744 }
00745
00754 template <typename T, typename At, size_t N>
00755 std::vector<T>& loadVector( std::vector<T> &v, At (&theArray)[N] )
00756 {
00757 MRPT_COMPILE_TIME_ASSERT(N!=0)
00758 v.resize(N);
00759 for (size_t i=0; i < N; i++)
00760 v[i] = static_cast<T>(theArray[i]);
00761 return v;
00762 }
00763
00765 template <class T>
00766 std::vector<T> Abs(const std::vector<T> &a)
00767 {
00768 typename std::vector<T> res(a.size());
00769 for (size_t i=0;i<a.size();i++)
00770 res[i] = static_cast<T>( fabs( static_cast<double>( a[i] ) ) );
00771 return res;
00772 }
00773
00774 }
00775
00776 }
00777
00778 #endif