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/CHistogram.h>
00035
00036 #include <mrpt/math/ops_vectors.h>
00037 #include <mrpt/math/ops_matrices.h>
00038
00039 #include <numeric>
00040 #include <cmath>
00041
00042
00043
00044
00045 namespace mrpt
00046 {
00047
00048
00049 namespace math
00050 {
00051 using namespace mrpt::utils;
00052
00053
00054
00055
00056
00057 bool BASE_IMPEXP loadVector( utils::CFileStream &f, std::vector<int> &d);
00058
00059
00060
00061
00062
00063 bool BASE_IMPEXP loadVector( utils::CFileStream &f, std::vector<double> &d);
00064
00065
00066
00067 bool BASE_IMPEXP isNaN(float f) MRPT_NO_THROWS;
00068
00069
00070 bool BASE_IMPEXP isNaN(double f) MRPT_NO_THROWS;
00071
00072
00073 bool BASE_IMPEXP isFinite(float f) MRPT_NO_THROWS;
00074
00075
00076 bool BASE_IMPEXP isFinite(double f) MRPT_NO_THROWS;
00077
00078 void BASE_IMPEXP medianFilter( const std::vector<double> &inV, std::vector<double> &outV, const int &winSize, const int &numberOfSigmas = 2 );
00079
00080 #ifdef HAVE_LONG_DOUBLE
00081
00082 bool BASE_IMPEXP isNaN(long double f) MRPT_NO_THROWS;
00083
00084
00085 bool BASE_IMPEXP isFinite(long double f) MRPT_NO_THROWS;
00086 #endif
00087
00088
00089
00090 template<typename T,typename VECTOR>
00091 void linspace(T first,T last, size_t count, VECTOR &out_vector)
00092 {
00093 if (count<2)
00094 {
00095 out_vector.assign(count,last);
00096 return;
00097 }
00098 else
00099 {
00100 out_vector.resize(count);
00101 const T incr = (last-first)/T(count-1);
00102 T c = first;
00103 for (size_t i=0;i<count;i++,c+=incr)
00104 out_vector[i] = static_cast<typename VECTOR::value_type>(c);
00105 }
00106 }
00107
00108
00109
00110 template<class T>
00111 inline Eigen::Matrix<T,Eigen::Dynamic,1> linspace(T first,T last, size_t count)
00112 {
00113 Eigen::Matrix<T,Eigen::Dynamic,1> ret;
00114 mrpt::math::linspace(first,last,count,ret);
00115 return ret;
00116 }
00117
00118
00119 template<class T,T STEP>
00120 inline Eigen::Matrix<T,Eigen::Dynamic,1> sequence(T first,size_t length)
00121 {
00122 Eigen::Matrix<T,Eigen::Dynamic,1> ret(length);
00123 if (!length) return ret;
00124 size_t i=0;
00125 while (length--) { ret[i++]=first; first+=STEP; }
00126 return ret;
00127 }
00128
00129
00130 template<class T,T STEP>
00131 inline std::vector<T> sequenceStdVec(T first,size_t length)
00132 {
00133 std::vector<T> ret(length);
00134 if (!length) return ret;
00135 size_t i=0;
00136 while (length--) { ret[i++]=first; first+=STEP; }
00137 return ret;
00138 }
00139
00140
00141 template<class T> inline Eigen::Matrix<T,Eigen::Dynamic,1> ones(size_t count)
00142 {
00143 Eigen::Matrix<T,Eigen::Dynamic,1> v(count);
00144 v.setOnes();
00145 return v;
00146 }
00147
00148
00149 template<class T> inline Eigen::Matrix<T,Eigen::Dynamic,1> zeros(size_t count)
00150 {
00151 Eigen::Matrix<T,Eigen::Dynamic,1> v(count);
00152 v.setZero();
00153 return v;
00154 }
00155
00156
00157
00158
00159
00160
00161 template <class T>
00162 inline void wrapTo2PiInPlace(T &a)
00163 {
00164 bool was_neg = a<0;
00165 a = fmod(a, static_cast<T>(M_2PI) );
00166 if (was_neg) a+=static_cast<T>(M_2PI);
00167 }
00168
00169
00170
00171
00172
00173 template <class T>
00174 inline T wrapTo2Pi(T a)
00175 {
00176 wrapTo2PiInPlace(a);
00177 return a;
00178 }
00179
00180
00181
00182
00183
00184 template <class T>
00185 inline T wrapToPi(T a)
00186 {
00187 return wrapTo2Pi( a + static_cast<T>(M_PI) )-static_cast<T>(M_PI);
00188 }
00189
00190
00191
00192
00193
00194 template <class T>
00195 inline void wrapToPiInPlace(T &a)
00196 {
00197 a = wrapToPi(a);
00198 }
00199
00200
00201
00202
00203
00204 template<class VEC1,class VEC2>
00205 void normalize(const VEC1 &v, VEC2 &out_v)
00206 {
00207 typename VEC1::value_type total=0;
00208 const size_t N = v.size();
00209 for (size_t i=0;i<N;i++)
00210 total += square(v[i]);
00211 total = std::sqrt(total);
00212 if (total)
00213 {
00214 out_v = v * (1.0/total);
00215 }
00216 else out_v.assign(v.size(),0);
00217 }
00218
00219
00220
00221
00222
00223
00224
00225
00226
00227
00228
00229 template<class VECTOR_OF_VECTORS, class MATRIXLIKE,class VECTORLIKE,class VECTORLIKE2,class VECTORLIKE3>
00230 inline void covariancesAndMeanWeighted(
00231 const VECTOR_OF_VECTORS &elements,
00232 MATRIXLIKE &covariances,
00233 VECTORLIKE &means,
00234 const VECTORLIKE2 *weights_mean,
00235 const VECTORLIKE3 *weights_cov,
00236 const bool *elem_do_wrap2pi = NULL
00237 )
00238 {
00239 ASSERTMSG_(elements.size()!=0,"No samples provided, so there is no way to deduce the output size.")
00240 typedef typename VECTORLIKE::value_type T;
00241 const size_t DIM = elements[0].size();
00242 means.resize(DIM);
00243 covariances.setSize(DIM,DIM);
00244 const size_t nElms=elements.size();
00245 const T NORM=1.0/nElms;
00246 if (weights_mean) { ASSERTDEB_(size_t(weights_mean->size())==size_t(nElms)) }
00247
00248 for (size_t i=0;i<DIM;i++)
00249 {
00250 T accum = 0;
00251 if (!elem_do_wrap2pi || !elem_do_wrap2pi[i])
00252 {
00253 if (weights_mean)
00254 {
00255 for (size_t j=0;j<nElms;j++)
00256 accum+= (*weights_mean)[j] * elements[j][i];
00257 }
00258 else
00259 {
00260 for (size_t j=0;j<nElms;j++) accum+=elements[j][i];
00261 accum*=NORM;
00262 }
00263 }
00264 else
00265 {
00266 double accum_L=0,accum_R=0;
00267 double Waccum_L=0,Waccum_R=0;
00268 for (size_t j=0;j<nElms;j++)
00269 {
00270 double ang = elements[j][i];
00271 const double w = weights_mean!=NULL ? (*weights_mean)[j] : NORM;
00272 if (fabs( ang )>0.5*M_PI)
00273 {
00274 if (ang<0) ang = (M_2PI + ang);
00275 accum_L += ang * w;
00276 Waccum_L += w;
00277 }
00278 else
00279 {
00280 accum_R += ang * w;
00281 Waccum_R += w;
00282 }
00283 }
00284 if (Waccum_L>0) accum_L /= Waccum_L;
00285 if (Waccum_R>0) accum_R /= Waccum_R;
00286 if (accum_L>M_PI) accum_L -= M_2PI;
00287 accum = (accum_L* Waccum_L + accum_R * Waccum_R );
00288 }
00289 means[i]=accum;
00290 }
00291
00292 for (size_t i=0;i<DIM;i++)
00293 for (size_t j=0;j<=i;j++)
00294 {
00295 typename MATRIXLIKE::value_type elem=0;
00296 if (weights_cov)
00297 {
00298 ASSERTDEB_(size_t(weights_cov->size())==size_t(nElms))
00299 for (size_t k=0;k<nElms;k++)
00300 {
00301 const T Ai = (elements[k][i]-means[i]);
00302 const T Aj = (elements[k][j]-means[j]);
00303 if (!elem_do_wrap2pi || !elem_do_wrap2pi[i])
00304 elem+= (*weights_cov)[k] * Ai * Aj;
00305 else elem+= (*weights_cov)[k] * mrpt::math::wrapToPi(Ai) * mrpt::math::wrapToPi(Aj);
00306 }
00307 }
00308 else
00309 {
00310 for (size_t k=0;k<nElms;k++)
00311 {
00312 const T Ai = (elements[k][i]-means[i]);
00313 const T Aj = (elements[k][j]-means[j]);
00314 if (!elem_do_wrap2pi || !elem_do_wrap2pi[i])
00315 elem+= Ai * Aj;
00316 else elem+= mrpt::math::wrapToPi(Ai) * mrpt::math::wrapToPi(Aj);
00317 }
00318 elem*=NORM;
00319 }
00320 covariances.get_unsafe(i,j) = elem;
00321 if (i!=j) covariances.get_unsafe(j,i)=elem;
00322 }
00323 }
00324
00325
00326
00327
00328
00329
00330
00331 template<class VECTOR_OF_VECTORS, class MATRIXLIKE,class VECTORLIKE>
00332 void covariancesAndMean(const VECTOR_OF_VECTORS &elements,MATRIXLIKE &covariances,VECTORLIKE &means, const bool *elem_do_wrap2pi = NULL)
00333 {
00334 covariancesAndMeanWeighted<VECTOR_OF_VECTORS,MATRIXLIKE,VECTORLIKE,vector_double,vector_double>(elements,covariances,means,NULL,NULL,elem_do_wrap2pi);
00335 }
00336
00337
00338
00339
00340
00341
00342
00343
00344
00345
00346 template<class VECTORLIKE1,class VECTORLIKE2>
00347 void weightedHistogram(
00348 const VECTORLIKE1 &values,
00349 const VECTORLIKE1 &weights,
00350 float binWidth,
00351 VECTORLIKE2 &out_binCenters,
00352 VECTORLIKE2 &out_binValues )
00353 {
00354 MRPT_START;
00355 typedef typename VECTORLIKE1::value_type TNum;
00356
00357 ASSERT_( values.size() == weights.size() );
00358 ASSERT_( binWidth > 0 );
00359 TNum minBin = minimum( values );
00360 unsigned int nBins = static_cast<unsigned>(ceil((maximum( values )-minBin) / binWidth));
00361
00362
00363 out_binCenters.resize(nBins);
00364 out_binValues.clear(); out_binValues.resize(nBins,0);
00365 TNum halfBin = TNum(0.5)*binWidth;;
00366 VECTORLIKE2 binBorders(nBins+1,minBin-halfBin);
00367 for (unsigned int i=0;i<nBins;i++)
00368 {
00369 binBorders[i+1] = binBorders[i]+binWidth;
00370 out_binCenters[i] = binBorders[i]+halfBin;
00371 }
00372
00373
00374 TNum totalSum = 0;
00375 typename VECTORLIKE1::const_iterator itVal, itW;
00376 for (itVal = values.begin(), itW = weights.begin(); itVal!=values.end(); ++itVal, ++itW )
00377 {
00378 int idx = round(((*itVal)-minBin)/binWidth);
00379 if (idx>=int(nBins)) idx=nBins-1;
00380 ASSERTDEB_(idx>=0);
00381 out_binValues[idx] += *itW;
00382 totalSum+= *itW;
00383 }
00384
00385 if (totalSum)
00386 out_binValues /= totalSum;
00387
00388
00389 MRPT_END;
00390 }
00391
00392
00393
00394
00395
00396
00397
00398
00399
00400 template<class VECTORLIKE1,class VECTORLIKE2>
00401 void weightedHistogramLog(
00402 const VECTORLIKE1 &values,
00403 const VECTORLIKE1 &log_weights,
00404 float binWidth,
00405 VECTORLIKE2 &out_binCenters,
00406 VECTORLIKE2 &out_binValues )
00407 {
00408 MRPT_START;
00409 typedef typename VECTORLIKE1::value_type TNum;
00410
00411 ASSERT_( values.size() == log_weights.size() );
00412 ASSERT_( binWidth > 0 );
00413 TNum minBin = minimum( values );
00414 unsigned int nBins = static_cast<unsigned>(ceil((maximum( values )-minBin) / binWidth));
00415
00416
00417 out_binCenters.resize(nBins);
00418 out_binValues.clear(); out_binValues.resize(nBins,0);
00419 TNum halfBin = TNum(0.5)*binWidth;;
00420 VECTORLIKE2 binBorders(nBins+1,minBin-halfBin);
00421 for (unsigned int i=0;i<nBins;i++)
00422 {
00423 binBorders[i+1] = binBorders[i]+binWidth;
00424 out_binCenters[i] = binBorders[i]+halfBin;
00425 }
00426
00427
00428 const TNum max_log_weight = maximum(log_weights);
00429 TNum totalSum = 0;
00430 typename VECTORLIKE1::const_iterator itVal, itW;
00431 for (itVal = values.begin(), itW = log_weights.begin(); itVal!=values.end(); ++itVal, ++itW )
00432 {
00433 int idx = round(((*itVal)-minBin)/binWidth);
00434 if (idx>=int(nBins)) idx=nBins-1;
00435 ASSERTDEB_(idx>=0);
00436 const TNum w = exp(*itW-max_log_weight);
00437 out_binValues[idx] += w;
00438 totalSum+= w;
00439 }
00440
00441 if (totalSum)
00442 out_binValues /= totalSum;
00443
00444 MRPT_END;
00445 }
00446
00447
00448
00449
00450
00451
00452
00453
00454 template <class VECTOR_OF_VECTORS, class VECTORLIKE>
00455 inline void extractColumnFromVectorOfVectors(const size_t colIndex, const VECTOR_OF_VECTORS &data, VECTORLIKE &out_column)
00456 {
00457 const size_t N = data.size();
00458 out_column.resize(N);
00459 for (size_t i=0;i<N;i++)
00460 out_column[i]=data[i][colIndex];
00461 }
00462
00463
00464
00465 uint64_t BASE_IMPEXP factorial64(unsigned int n);
00466
00467
00468
00469 double BASE_IMPEXP factorial(unsigned int n);
00470
00471
00472
00473 template <class T>
00474 T round2up(T val)
00475 {
00476 T n = 1;
00477 while (n < val)
00478 {
00479 n <<= 1;
00480 if (n<=1)
00481 THROW_EXCEPTION("Overflow!");
00482 }
00483 return n;
00484 }
00485
00486
00487
00488
00489 template <class T>
00490 T round_10power(T val, int power10)
00491 {
00492 long double F = ::pow((long double)10.0,-(long double)power10);
00493 long int t = round_long( val * F );
00494 return T(t/F);
00495 }
00496
00497
00498
00499
00500 template<class T>
00501 double correlate_matrix(const CMatrixTemplateNumeric<T> &a1, const CMatrixTemplateNumeric<T> &a2)
00502 {
00503 if ((a1.getColCount()!=a2.getColCount())|(a1.getRowCount()!=a2.getRowCount()))
00504 THROW_EXCEPTION("Correlation Error!, images with no same size");
00505
00506 int i,j;
00507 T x1,x2;
00508 T syy=0, sxy=0, sxx=0, m1=0, m2=0 ,n=a1.getRowCount()*a2.getColCount();
00509
00510
00511 for (i=0;i<a1.getRowCount();i++)
00512 {
00513 for (j=0;j<a1.getColCount();j++)
00514 {
00515 m1 += a1(i,j);
00516 m2 += a2(i,j);
00517 }
00518 }
00519 m1 /= n;
00520 m2 /= n;
00521
00522 for (i=0;i<a1.getRowCount();i++)
00523 {
00524 for (j=0;j<a1.getColCount();j++)
00525 {
00526 x1 = a1(i,j) - m1;
00527 x2 = a2(i,j) - m2;
00528 sxx += x1*x1;
00529 syy += x2*x2;
00530 sxy += x1*x2;
00531 }
00532 }
00533
00534 return sxy / sqrt(sxx * syy);
00535 }
00536
00537
00538
00539
00540
00541
00542
00543
00544
00545
00546
00547
00548
00549
00550
00551
00552
00553
00554
00555
00556
00557
00558
00559
00560
00561
00562
00563
00564
00565
00566
00567
00568
00569
00570
00571
00572
00573
00574
00575
00576
00577
00578
00579
00580
00581
00582
00583
00584
00585 double BASE_IMPEXP averageLogLikelihood( const vector_double &logLikelihoods );
00586
00587
00588
00589 double BASE_IMPEXP averageWrap2Pi(const vector_double &angles );
00590
00591
00592
00593
00594
00595
00596
00597
00598 double BASE_IMPEXP averageLogLikelihood(
00599 const vector_double &logWeights,
00600 const vector_double &logLikelihoods );
00601
00602
00603
00604
00605
00606
00607
00608
00609 std::string BASE_IMPEXP MATLAB_plotCovariance2D(
00610 const CMatrixFloat &cov22,
00611 const vector_float &mean,
00612 const float &stdCount,
00613 const std::string &style = std::string("b"),
00614 const size_t &nEllipsePoints = 30 );
00615
00616
00617
00618
00619
00620
00621
00622
00623 std::string BASE_IMPEXP MATLAB_plotCovariance2D(
00624 const CMatrixDouble &cov22,
00625 const vector_double &mean,
00626 const float &stdCount,
00627 const std::string &style = std::string("b"),
00628 const size_t &nEllipsePoints = 30 );
00629
00630
00631
00632
00633
00634
00635 template <class MATRIXLIKE1,class MATRIXLIKE2>
00636 void homogeneousMatrixInverse(const MATRIXLIKE1 &M, MATRIXLIKE2 &out_inverse_M)
00637 {
00638 MRPT_START;
00639 ASSERT_( M.isSquare() && size(M,1)==4);
00640
00641
00642
00643
00644
00645
00646
00647
00648
00649
00650
00651
00652 out_inverse_M.setSize(4,4);
00653
00654
00655 out_inverse_M.set_unsafe(0,0, M.get_unsafe(0,0));
00656 out_inverse_M.set_unsafe(0,1, M.get_unsafe(1,0));
00657 out_inverse_M.set_unsafe(0,2, M.get_unsafe(2,0));
00658
00659 out_inverse_M.set_unsafe(1,0, M.get_unsafe(0,1));
00660 out_inverse_M.set_unsafe(1,1, M.get_unsafe(1,1));
00661 out_inverse_M.set_unsafe(1,2, M.get_unsafe(2,1));
00662
00663 out_inverse_M.set_unsafe(2,0, M.get_unsafe(0,2));
00664 out_inverse_M.set_unsafe(2,1, M.get_unsafe(1,2));
00665 out_inverse_M.set_unsafe(2,2, M.get_unsafe(2,2));
00666
00667 const double tx = -M.get_unsafe(0,3);
00668 const double ty = -M.get_unsafe(1,3);
00669 const double tz = -M.get_unsafe(2,3);
00670
00671 const double tx_ = tx*M.get_unsafe(0,0)+ty*M.get_unsafe(1,0)+tz*M.get_unsafe(2,0);
00672 const double ty_ = tx*M.get_unsafe(0,1)+ty*M.get_unsafe(1,1)+tz*M.get_unsafe(2,1);
00673 const double tz_ = tx*M.get_unsafe(0,2)+ty*M.get_unsafe(1,2)+tz*M.get_unsafe(2,2);
00674
00675 out_inverse_M.set_unsafe(0,3, tx_ );
00676 out_inverse_M.set_unsafe(1,3, ty_ );
00677 out_inverse_M.set_unsafe(2,3, tz_ );
00678
00679 out_inverse_M.set_unsafe(3,0, 0);
00680 out_inverse_M.set_unsafe(3,1, 0);
00681 out_inverse_M.set_unsafe(3,2, 0);
00682 out_inverse_M.set_unsafe(3,3, 1);
00683
00684 MRPT_END;
00685 }
00686
00687 template <class IN_ROTMATRIX,class IN_XYZ, class OUT_ROTMATRIX, class OUT_XYZ>
00688 void homogeneousMatrixInverse(
00689 const IN_ROTMATRIX & in_R,
00690 const IN_XYZ & in_xyz,
00691 OUT_ROTMATRIX & out_R,
00692 OUT_XYZ & out_xyz
00693 )
00694 {
00695 MRPT_START
00696 ASSERT_( in_R.isSquare() && size(in_R,1)==3 && in_xyz.size()==3)
00697 out_R.setSize(3,3);
00698 out_xyz.resize(3);
00699
00700
00701 const double tx = -in_xyz[0];
00702 const double ty = -in_xyz[1];
00703 const double tz = -in_xyz[2];
00704
00705 out_xyz[0] = tx*in_R.get_unsafe(0,0)+ty*in_R.get_unsafe(1,0)+tz*in_R.get_unsafe(2,0);
00706 out_xyz[1] = tx*in_R.get_unsafe(0,1)+ty*in_R.get_unsafe(1,1)+tz*in_R.get_unsafe(2,1);
00707 out_xyz[2] = tx*in_R.get_unsafe(0,2)+ty*in_R.get_unsafe(1,2)+tz*in_R.get_unsafe(2,2);
00708
00709
00710 out_R = in_R.adjoint();
00711
00712 MRPT_END
00713 }
00714
00715 template <class MATRIXLIKE>
00716 inline void homogeneousMatrixInverse(MATRIXLIKE &M)
00717 {
00718 ASSERTDEB_( M.isSquare() && size(M,1)==4);
00719
00720 const double tx = -M(0,3);
00721 const double ty = -M(1,3);
00722 const double tz = -M(2,3);
00723 M(0,3) = tx*M(0,0)+ty*M(1,0)+tz*M(2,0);
00724 M(1,3) = tx*M(0,1)+ty*M(1,1)+tz*M(2,1);
00725 M(2,3) = tx*M(0,2)+ty*M(1,2)+tz*M(2,2);
00726
00727 std::swap( M(1,0),M(0,1) );
00728 std::swap( M(2,0),M(0,2) );
00729 std::swap( M(1,2),M(2,1) );
00730 }
00731
00732
00733
00734
00735
00736
00737
00738
00739
00740
00741 template <class VECTORLIKE,class VECTORLIKE2, class VECTORLIKE3, class MATRIXLIKE, class USERPARAM >
00742 void estimateJacobian(
00743 const VECTORLIKE &x,
00744 void (*functor) (const VECTORLIKE &x,const USERPARAM &y, VECTORLIKE3 &out),
00745 const VECTORLIKE2 &increments,
00746 const USERPARAM &userParam,
00747 MATRIXLIKE &out_Jacobian )
00748 {
00749 MRPT_START
00750 ASSERT_(x.size()>0 && increments.size() == x.size());
00751
00752 size_t m = 0;
00753 const size_t n = x.size();
00754
00755 for (size_t j=0;j<n;j++) { ASSERT_( increments[j]>0 ) }
00756
00757 VECTORLIKE3 f_minus, f_plus;
00758 VECTORLIKE x_mod(x);
00759
00760
00761 for (size_t j=0;j<n;j++)
00762 {
00763
00764 x_mod[j]=x[j]+increments[j];
00765 functor(x_mod,userParam, f_plus);
00766
00767 x_mod[j]=x[j]-increments[j];
00768 functor(x_mod,userParam, f_minus);
00769
00770 x_mod[j]=x[j];
00771 const double Ax_2_inv = 0.5/increments[j];
00772
00773
00774 if (j==0)
00775 {
00776 m = f_plus.size();
00777 out_Jacobian.setSize(m,n);
00778 }
00779
00780 for (size_t i=0;i<m;i++)
00781 out_Jacobian.get_unsafe(i,j) = Ax_2_inv* (f_plus[i]-f_minus[i]);
00782
00783 }
00784
00785 MRPT_END
00786 }
00787
00788
00789
00790
00791
00792
00793
00794
00795 template <class T,class VECTOR>
00796 T interpolate(
00797 const T &x,
00798 const VECTOR &ys,
00799 const T &x0,
00800 const T &x1 )
00801 {
00802 MRPT_START
00803 ASSERT_(x1>x0); ASSERT_(!ys.empty());
00804 const size_t N = ys.size();
00805 if (x<=x0) return ys[0];
00806 if (x>=x1) return ys[N-1];
00807 const T Ax = (x1-x0)/T(N);
00808 const size_t i = int( (x-x0)/Ax );
00809 if (i>=N-1) return ys[N-1];
00810 const T Ay = ys[i+1]-ys[i];
00811 return ys[i] + (x-(x0+i*Ax))*Ay/Ax;
00812 MRPT_END
00813 }
00814
00815
00816
00817
00818
00819 double BASE_IMPEXP interpolate2points(const double x, const double x0, const double y0, const double x1, const double y1, bool wrap2pi = false);
00820
00821
00822
00823
00824
00825 double BASE_IMPEXP spline(const double t, const vector_double &x, const vector_double &y, bool wrap2pi = false);
00826
00827
00828
00829
00830
00831
00832
00833 template <typename NUMTYPE,class VECTORLIKE>
00834 NUMTYPE leastSquareLinearFit(const NUMTYPE t, const VECTORLIKE &x, const VECTORLIKE &y, bool wrap2pi = false)
00835 {
00836 MRPT_START
00837
00838
00839 ASSERT_(x.size()==y.size());
00840 ASSERT_(x.size()>1);
00841
00842 const size_t N = x.size();
00843
00844 typedef typename VECTORLIKE::value_type NUM;
00845
00846
00847 const NUM x_min = x.minimum();
00848 CMatrixTemplateNumeric<NUM> Xt(2,N);
00849 for (size_t i=0;i<N;i++)
00850 {
00851 Xt.set_unsafe(0,i, 1);
00852 Xt.set_unsafe(1,i, x[i]-x_min);
00853 }
00854
00855 CMatrixTemplateNumeric<NUM> XtX;
00856 XtX.multiply_AAt(Xt);
00857
00858 CMatrixTemplateNumeric<NUM> XtXinv;
00859 XtX.inv_fast(XtXinv);
00860
00861 CMatrixTemplateNumeric<NUM> XtXinvXt;
00862 XtXinvXt.multiply(XtXinv,Xt);
00863
00864 VECTORLIKE B;
00865 XtXinvXt.multiply_Ab(y,B);
00866
00867 ASSERT_(B.size()==2)
00868
00869 NUM ret = B[0] + B[1]*(t-x_min);
00870
00871
00872 if (!wrap2pi)
00873 return ret;
00874 else return mrpt::math::wrapToPi(ret);
00875
00876 MRPT_END
00877 }
00878
00879
00880
00881
00882
00883 template <class VECTORLIKE1,class VECTORLIKE2,class VECTORLIKE3>
00884 void leastSquareLinearFit(
00885 const VECTORLIKE1 &ts,
00886 VECTORLIKE2 &outs,
00887 const VECTORLIKE3 &x,
00888 const VECTORLIKE3 &y,
00889 bool wrap2pi = false)
00890 {
00891 MRPT_START
00892
00893
00894 ASSERT_(x.size()==y.size());
00895 ASSERT_(x.size()>1);
00896
00897 const size_t N = x.size();
00898
00899
00900 typedef typename VECTORLIKE3::value_type NUM;
00901 const NUM x_min = x.minimum();
00902 CMatrixTemplateNumeric<NUM> Xt(2,N);
00903 for (size_t i=0;i<N;i++)
00904 {
00905 Xt.set_unsafe(0,i, 1);
00906 Xt.set_unsafe(1,i, x[i]-x_min);
00907 }
00908
00909 CMatrixTemplateNumeric<NUM> XtX;
00910 XtX.multiply_AAt(Xt);
00911
00912 CMatrixTemplateNumeric<NUM> XtXinv;
00913 XtX.inv_fast(XtXinv);
00914
00915 CMatrixTemplateNumeric<NUM> XtXinvXt;
00916 XtXinvXt.multiply(XtXinv,Xt);
00917
00918 VECTORLIKE3 B;
00919 XtXinvXt.multiply_Ab(y,B);
00920
00921 ASSERT_(B.size()==2)
00922
00923 const size_t tsN = size_t(ts.size());
00924 outs.resize(tsN);
00925 if (!wrap2pi)
00926 for (size_t k=0;k<tsN;k++)
00927 outs[k] = B[0] + B[1]*(ts[k]-x_min);
00928 else
00929 for (size_t k=0;k<tsN;k++)
00930 outs[k] = mrpt::math::wrapToPi( B[0] + B[1]*(ts[k]-x_min) );
00931 MRPT_END
00932 }
00933
00934
00935
00936
00937
00938
00939
00940
00941
00942
00943
00944
00945 template <typename T, typename At, size_t N>
00946 std::vector<T>& loadVector( std::vector<T> &v, At (&theArray)[N] )
00947 {
00948 MRPT_COMPILE_TIME_ASSERT(N!=0)
00949 v.resize(N);
00950 for (size_t i=0; i < N; i++)
00951 v[i] = static_cast<T>(theArray[i]);
00952 return v;
00953 }
00954
00955 template <typename Derived, typename At, size_t N>
00956 Eigen::EigenBase<Derived>& loadVector( Eigen::EigenBase<Derived> &v, At (&theArray)[N] )
00957 {
00958 MRPT_COMPILE_TIME_ASSERT(N!=0)
00959 v.derived().resize(N);
00960 for (size_t i=0; i < N; i++)
00961 (v.derived())[i] = static_cast<typename Derived::Scalar>(theArray[i]);
00962 return v;
00963 }
00964
00965
00966
00967
00968 void unwrap2PiSequence(vector_double &x);
00969
00970
00971
00972
00973
00974
00975
00976 template<class VECTORLIKE1,class VECTORLIKE2,class MAT>
00977 typename VECTORLIKE1::value_type mahalanobisDistance2(
00978 const VECTORLIKE1 &X,
00979 const VECTORLIKE2 &MU,
00980 const MAT &COV )
00981 {
00982 MRPT_START
00983 #if defined(_DEBUG) || (MRPT_ALWAYS_CHECKS_DEBUG_MATRICES)
00984 ASSERT_( !X.empty() );
00985 ASSERT_( X.size()==MU.size() );
00986 ASSERT_( X.size()==size(COV,1) && COV.isSquare() );
00987 #endif
00988 const size_t N = X.size();
00989 mrpt::dynamicsize_vector<typename VECTORLIKE1::value_type> X_MU(N);
00990 for (size_t i=0;i<N;i++) X_MU[i]=X[i]-MU[i];
00991 return multiply_HCHt_scalar(X_MU, COV.inv() );
00992 MRPT_END
00993 }
00994
00995
00996
00997
00998
00999 template<class VECTORLIKE1,class VECTORLIKE2,class MAT>
01000 inline typename VECTORLIKE1::value_type mahalanobisDistance(
01001 const VECTORLIKE1 &X,
01002 const VECTORLIKE2 &MU,
01003 const MAT &COV )
01004 {
01005 return std::sqrt( mahalanobisDistance2(X,MU,COV) );
01006 }
01007
01008
01009
01010
01011
01012 template<class VECTORLIKE,class MAT1,class MAT2,class MAT3>
01013 typename VECTORLIKE::value_type
01014 mahalanobisDistance2(
01015 const VECTORLIKE &mean_diffs,
01016 const MAT1 &COV1,
01017 const MAT2 &COV2,
01018 const MAT3 &CROSS_COV12 )
01019 {
01020 MRPT_START
01021 #if defined(_DEBUG) || (MRPT_ALWAYS_CHECKS_DEBUG_MATRICES)
01022 ASSERT_( !mean_diffs.empty() );
01023 ASSERT_( mean_diffs.size()==size(COV1,1));
01024 ASSERT_( COV1.isSquare() && COV2.isSquare() );
01025 ASSERT_( size(COV1,1)==size(COV2,1));
01026 #endif
01027 const size_t N = size(COV1,1);
01028 MAT1 COV = COV1;
01029 COV+=COV2;
01030 COV.substract_An(CROSS_COV12,2);
01031 MAT1 COV_inv;
01032 COV.inv_fast(COV_inv);
01033 return multiply_HCHt_scalar(mean_diffs,COV_inv);
01034 MRPT_END
01035 }
01036
01037
01038
01039
01040 template<class VECTORLIKE,class MAT1,class MAT2,class MAT3> inline typename VECTORLIKE::value_type
01041 mahalanobisDistance(
01042 const VECTORLIKE &mean_diffs,
01043 const MAT1 &COV1,
01044 const MAT2 &COV2,
01045 const MAT3 &CROSS_COV12 )
01046 {
01047 return std::sqrt( mahalanobisDistance( mean_diffs, COV1,COV2,CROSS_COV12 ));
01048 }
01049
01050
01051
01052
01053 template<class VECTORLIKE,class MATRIXLIKE>
01054 inline typename MATRIXLIKE::value_type
01055 mahalanobisDistance2(const VECTORLIKE &delta_mu,const MATRIXLIKE &cov)
01056 {
01057 ASSERTDEB_(cov.isSquare())
01058 ASSERTDEB_(cov.getColCount()==delta_mu.size())
01059 return multiply_HCHt_scalar(delta_mu,cov.inverse());
01060 }
01061
01062
01063
01064
01065 template<class VECTORLIKE,class MATRIXLIKE>
01066 inline typename MATRIXLIKE::value_type
01067 mahalanobisDistance(const VECTORLIKE &delta_mu,const MATRIXLIKE &cov)
01068 {
01069 return std::sqrt(mahalanobisDistance2(delta_mu,cov));
01070 }
01071
01072
01073
01074
01075 template <typename T>
01076 T productIntegralTwoGaussians(
01077 const std::vector<T> &mean_diffs,
01078 const CMatrixTemplateNumeric<T> &COV1,
01079 const CMatrixTemplateNumeric<T> &COV2
01080 )
01081 {
01082 const size_t vector_dim = mean_diffs.size();
01083 ASSERT_(vector_dim>=1)
01084
01085 CMatrixTemplateNumeric<T> C = COV1;
01086 C+= COV2;
01087 const T cov_det = C.det();
01088 CMatrixTemplateNumeric<T> C_inv;
01089 C.inv_fast(C_inv);
01090
01091 return std::pow( M_2PI, -0.5*vector_dim ) * (1.0/std::sqrt( cov_det ))
01092 * exp( -0.5 * mean_diffs.multiply_HCHt_scalar(C_inv) );
01093 }
01094
01095
01096
01097
01098 template <typename T, size_t DIM>
01099 T productIntegralTwoGaussians(
01100 const std::vector<T> &mean_diffs,
01101 const CMatrixFixedNumeric<T,DIM,DIM> &COV1,
01102 const CMatrixFixedNumeric<T,DIM,DIM> &COV2
01103 )
01104 {
01105 ASSERT_(mean_diffs.size()==DIM);
01106
01107 CMatrixFixedNumeric<T,DIM,DIM> C = COV1;
01108 C+= COV2;
01109 const T cov_det = C.det();
01110 CMatrixFixedNumeric<T,DIM,DIM> C_inv(UNINITIALIZED_MATRIX);
01111 C.inv_fast(C_inv);
01112
01113 return std::pow( M_2PI, -0.5*DIM ) * (1.0/std::sqrt( cov_det ))
01114 * exp( -0.5 * mean_diffs.multiply_HCHt_scalar(C_inv) );
01115 }
01116
01117
01118
01119
01120 template <typename T, class VECLIKE,class MATLIKE1, class MATLIKE2>
01121 void productIntegralAndMahalanobisTwoGaussians(
01122 const VECLIKE &mean_diffs,
01123 const MATLIKE1 &COV1,
01124 const MATLIKE2 &COV2,
01125 T &maha2_out,
01126 T &intprod_out,
01127 const MATLIKE1 *CROSS_COV12=NULL
01128 )
01129 {
01130 const size_t vector_dim = mean_diffs.size();
01131 ASSERT_(vector_dim>=1)
01132
01133 MATLIKE1 C = COV1;
01134 C+= COV2;
01135 if (CROSS_COV12) { C-=*CROSS_COV12; C-=*CROSS_COV12; }
01136 const T cov_det = C.det();
01137 MATLIKE1 C_inv;
01138 C.inv_fast(C_inv);
01139
01140 maha2_out = mean_diffs.multiply_HCHt_scalar(C_inv);
01141 intprod_out = std::pow( M_2PI, -0.5*vector_dim ) * (1.0/std::sqrt( cov_det ))*exp(-0.5*maha2_out);
01142 }
01143
01144
01145
01146
01147 template <typename T, class VECLIKE,class MATRIXLIKE>
01148 void mahalanobisDistance2AndLogPDF(
01149 const VECLIKE &diff_mean,
01150 const MATRIXLIKE &cov,
01151 T &maha2_out,
01152 T &log_pdf_out)
01153 {
01154 MRPT_START
01155 ASSERTDEB_(cov.isSquare())
01156 ASSERTDEB_(size_t(cov.getColCount())==size_t(diff_mean.size()))
01157 MATRIXLIKE C_inv;
01158 cov.inv(C_inv);
01159 maha2_out = multiply_HCHt_scalar(diff_mean,C_inv);
01160 log_pdf_out = static_cast<typename MATRIXLIKE::value_type>(-0.5)* (
01161 maha2_out+
01162 static_cast<typename MATRIXLIKE::value_type>(cov.getColCount())*::log(static_cast<typename MATRIXLIKE::value_type>(M_2PI))+
01163 ::log(cov.det())
01164 );
01165 MRPT_END
01166 }
01167
01168
01169
01170
01171 template <typename T, class VECLIKE,class MATRIXLIKE>
01172 inline void mahalanobisDistance2AndPDF(
01173 const VECLIKE &diff_mean,
01174 const MATRIXLIKE &cov,
01175 T &maha2_out,
01176 T &pdf_out)
01177 {
01178 mahalanobisDistance2AndLogPDF(diff_mean,cov,maha2_out,pdf_out);
01179 pdf_out = std::exp(pdf_out);
01180 }
01181
01182
01183
01184
01185
01186
01187
01188
01189
01190
01191
01192
01193 template <size_t N, typename T>
01194 std::vector<T> make_vector(const T val1, ...)
01195 {
01196 MRPT_COMPILE_TIME_ASSERT( N>0 )
01197 std::vector<T> ret;
01198 ret.reserve(N);
01199
01200 ret.push_back(val1);
01201
01202 va_list args;
01203 va_start(args,val1);
01204 for (size_t i=0;i<N-1;i++)
01205 ret.push_back( va_arg(args,T) );
01206
01207 va_end(args);
01208 return ret;
01209 }
01210
01211 }
01212
01213 }
01214
01215 #endif