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_container_ops_H
00029 #define mrpt_math_container_ops_H
00030
00031 #include <mrpt/math/math_frwds.h>
00032
00033 #include <mrpt/math/lightweight_geom_data.h>
00034
00035 #include <functional>
00036 #include <algorithm>
00037 #include <cmath>
00038
00039
00040
00041
00042
00043
00044
00045
00046
00047
00048
00049
00050
00051
00052 #include <algorithm>
00053 #include <numeric>
00054 #include <functional>
00055
00056 #include <mrpt/math/CHistogram.h>
00057
00058
00059 namespace mrpt
00060 {
00061 namespace math
00062 {
00063
00064
00065
00066
00067 template<class CONTAINER>
00068 vector_double histogram(
00069 const CONTAINER &v,
00070 double limit_min,
00071 double limit_max,
00072 size_t number_bins,
00073 bool do_normalization = false,
00074 vector_double *out_bin_centers = NULL)
00075 {
00076 mrpt::math::CHistogram H( limit_min, limit_max, number_bins );
00077 vector_double ret(number_bins);
00078 vector_double dummy_ret_bins;
00079 H.add(v);
00080 if (do_normalization)
00081 H.getHistogramNormalized( out_bin_centers ? *out_bin_centers : dummy_ret_bins, ret );
00082 else H.getHistogram( out_bin_centers ? *out_bin_centers : dummy_ret_bins, ret );
00083 return ret;
00084 }
00085
00086
00087
00088
00089
00090 template <class CONTAINER1,class CONTAINER2>
00091 inline void cumsum(const CONTAINER1 &in_data, CONTAINER2 &out_cumsum)
00092 {
00093 out_cumsum.resizeLike(in_data);
00094 typename CONTAINER1::value_type last=0;
00095 const size_t N = in_data.size();
00096 for (size_t i=0;i<N;i++)
00097 last = out_cumsum[i] = last + in_data[i];
00098 }
00099
00100
00101
00102 template<class CONTAINER>
00103 inline CONTAINER cumsum(const CONTAINER &in_data)
00104 {
00105 CONTAINER ret;
00106 cumsum(in_data,ret);
00107 return ret;
00108 }
00109
00110 template <class CONTAINER> inline typename CONTAINER::value_type norm_inf(const CONTAINER &v) { return v.norm_inf(); }
00111 template <class CONTAINER> inline typename CONTAINER::value_type norm(const CONTAINER &v) { return v.norm(); }
00112 template <class CONTAINER> inline typename CONTAINER::value_type maximum(const CONTAINER &v) { return v.maximum(); }
00113 template <class CONTAINER> inline typename CONTAINER::value_type minimum(const CONTAINER &v) { return v.minimum(); }
00114
00115 template <typename T> inline T maximum(const std::vector<T> &v)
00116 {
00117 ASSERT_(!v.empty())
00118 T m = v[0];
00119 for (size_t i=0;i<v.size();i++) mrpt::utils::keep_max(m,v[i]);
00120 return m;
00121 }
00122 template <typename T> inline T minimum(const std::vector<T> &v)
00123 {
00124 ASSERT_(!v.empty())
00125 T m = v[0];
00126 for (size_t i=0;i<v.size();i++) mrpt::utils::keep_min(m,v[i]);
00127 return m;
00128 }
00129
00130
00131
00132
00133
00134
00135 template <class CONTAINER> CONTAINER & containerFromPoseOrPoint(CONTAINER &C, const TPoint2D &p) {
00136 C.resize(2,1);
00137 for (size_t i=0;i<2;i++) C.coeffRef(i,0)=p[i];
00138 return C;
00139 }
00140 template <class CONTAINER> CONTAINER & containerFromPoseOrPoint(CONTAINER &C, const TPoint3D &p) {
00141 C.resize(3,1);
00142 for (size_t i=0;i<3;i++) C.coeffRef(i,0)=p[i];
00143 return C;
00144 }
00145 template <class CONTAINER> CONTAINER & containerFromPoseOrPoint(CONTAINER &C, const TPose2D &p) {
00146 C.resize(3,1);
00147 for (size_t i=0;i<3;i++) C.coeffRef(i,0)=p[i];
00148 return C;
00149 }
00150 template <class CONTAINER> CONTAINER & containerFromPoseOrPoint(CONTAINER &C, const TPose3D &p) {
00151 C.resize(6,1);
00152 for (size_t i=0;i<6;i++) C.coeffRef(i,0)=p[i];
00153 return C;
00154 }
00155 template <class CONTAINER> CONTAINER & containerFromPoseOrPoint(CONTAINER &C, const TPose3DQuat &p) {
00156 C.resize(7,1);
00157 for (size_t i=0;i<7;i++) C.coeffRef(i,0)=p[i];
00158 return C;
00159 }
00160
00161
00162
00163
00164
00165
00166
00167
00168
00169
00170 template <class CONTAINER>
00171 typename CONTAINER::value_type squareNorm_accum(const typename CONTAINER::value_type total, const CONTAINER &v);
00172 template <class CONTAINER> typename CONTAINER::value_type squareNorm_accum(const typename CONTAINER::value_type total, const CONTAINER &v) {
00173 return total+v.squaredNorm();
00174 }
00175
00176
00177
00178 template<size_t N,class T,class U>
00179 inline T squareNorm(const U &v) {
00180 T res=0;
00181 for (size_t i=0;i<N;i++) res+=square(v[i]);
00182 return res;
00183 }
00184
00185
00186 template <class CONTAINER1,class CONTAINER2>
00187 inline typename CONTAINER1::value_type
00188 dotProduct(const CONTAINER1 &v1,const CONTAINER1 &v2)
00189 {
00190 return v1.dot(v2);
00191 }
00192
00193
00194 template<size_t N,class T,class U,class V>
00195 inline T dotProduct(const U &v1,const V &v2) {
00196 T res=0;
00197 for (size_t i=0;i<N;i++) res+=v1[i]*v2[i];
00198 return res;
00199 }
00200
00201
00202
00203
00204 template <class CONTAINER> inline typename CONTAINER::value_type sum(const CONTAINER &v) { return v.sum(); }
00205
00206
00207 template <typename T> inline T sum(const std::vector<T> &v) { return std::accumulate(v.begin(),v.end(),T(0)); }
00208
00209
00210
00211 template <class CONTAINER,typename RET> inline RET sumRetType(const CONTAINER &v) { return v.sumRetType<RET>(); }
00212
00213
00214
00215 template <class CONTAINER>
00216 inline double mean(const CONTAINER &v)
00217 {
00218 if (v.empty())
00219 return 0;
00220 else return sum(v)/static_cast<double>(v.size());
00221 }
00222
00223
00224 template <typename T>
00225 inline void minimum_maximum(const std::vector<T> &V, T&curMin,T&curMax)
00226 {
00227 ASSERT_(V.size()!=0)
00228 const size_t N=V.size();
00229 curMin=curMax=V[0];
00230 for (size_t i=1;i<N;i++)
00231 {
00232 mrpt::utils::keep_min(curMin,V[i]);
00233 mrpt::utils::keep_max(curMax,V[i]);
00234 }
00235 }
00236
00237
00238 template <class Derived>
00239 inline void minimum_maximum(
00240 const Eigen::MatrixBase<Derived> &V,
00241 typename Eigen::MatrixBase<Derived>::value_type &curMin,
00242 typename Eigen::MatrixBase<Derived>::value_type &curMax)
00243 {
00244 V.minimum_maximum(curMin,curMax);
00245 }
00246
00247
00248
00249 template <class CONTAINER1,class CONTAINER2>
00250 size_t countCommonElements(const CONTAINER1 &a,const CONTAINER2 &b)
00251 {
00252 size_t ret=0;
00253 for (typename CONTAINER1::const_iterator it1 = a.begin();it1!=a.end();it1++)
00254 for (typename CONTAINER2::const_iterator it2 = b.begin();it2!=b.end();it2++)
00255 if ( (*it1) == (*it2) )
00256 ret++;
00257 return ret;
00258 }
00259
00260
00261 template <class CONTAINER>
00262 void adjustRange(CONTAINER &m, const typename CONTAINER::value_type minVal,const typename CONTAINER::value_type maxVal)
00263 {
00264 if (size_t(m.size())==0) return;
00265 typename CONTAINER::value_type curMin,curMax;
00266 minimum_maximum(m,curMin,curMax);
00267 const typename CONTAINER::value_type curRan = curMax-curMin;
00268 m -= (curMin+minVal);
00269 if (curRan!=0) m *= (maxVal-minVal)/curRan;
00270 }
00271
00272
00273
00274
00275
00276
00277
00278
00279
00280 template<class VECTORLIKE>
00281 void meanAndStd(
00282 const VECTORLIKE &v,
00283 double &out_mean,
00284 double &out_std,
00285 bool unbiased = true)
00286 {
00287 if (v.size()<2)
00288 {
00289 out_std = 0;
00290 out_mean = (v.size()==1) ? *v.begin() : 0;
00291 }
00292 else
00293 {
00294
00295 const size_t N = v.size();
00296 out_mean = mrpt::math::sum(v) / static_cast<double>(N);
00297
00298 double vector_std=0;
00299 for (size_t i=0;i<N;i++) vector_std += mrpt::utils::square( v[i]-out_mean);
00300 out_std = std::sqrt(vector_std / static_cast<double>(N - (unbiased ? 1:0)) );
00301 }
00302 }
00303
00304
00305
00306
00307
00308
00309
00310 template<class VECTORLIKE>
00311 inline double stddev(const VECTORLIKE &v, bool unbiased = true)
00312 {
00313 double m,s;
00314 meanAndStd(v,m,s,unbiased);
00315 return s;
00316 }
00317
00318
00319
00320
00321
00322
00323
00324 template<class VECTOR_OF_VECTOR, class VECTORLIKE, class MATRIXLIKE>
00325 void meanAndCovVec(
00326 const VECTOR_OF_VECTOR &v,
00327 VECTORLIKE &out_mean,
00328 MATRIXLIKE &out_cov
00329 )
00330 {
00331 const size_t N = v.size();
00332 ASSERTMSG_(N>0,"The input vector contains no elements");
00333 const double N_inv = 1.0/N;
00334
00335 const size_t M = v[0].size();
00336 ASSERTMSG_(M>0,"The input vector contains rows of length 0");
00337
00338
00339 out_mean.assign(M,0);
00340 for (size_t i=0;i<N;i++)
00341 for (size_t j=0;j<M;j++)
00342 out_mean[j]+=v[i][j];
00343 out_mean*=N_inv;
00344
00345
00346
00347
00348 out_cov.zeros(M,M);
00349 for (size_t i=0;i<N;i++)
00350 {
00351 for (size_t j=0;j<M;j++)
00352 out_cov.get_unsafe(j,j)+=square(v[i][j]-out_mean[j]);
00353
00354 for (size_t j=0;j<M;j++)
00355 for (size_t k=j+1;k<M;k++)
00356 out_cov.get_unsafe(j,k)+=(v[i][j]-out_mean[j])*(v[i][k]-out_mean[k]);
00357 }
00358 for (size_t j=0;j<M;j++)
00359 for (size_t k=j+1;k<M;k++)
00360 out_cov.get_unsafe(k,j) = out_cov.get_unsafe(j,k);
00361 out_cov*=N_inv;
00362 }
00363
00364
00365
00366
00367
00368
00369 template<class VECTOR_OF_VECTOR>
00370 inline Eigen::MatrixXd covVector( const VECTOR_OF_VECTOR &v )
00371 {
00372 vector_double m;
00373 Eigen::MatrixXd C;
00374 meanAndCovVec(v,m,C);
00375 return C;
00376 }
00377
00378
00379
00380
00381
00382
00383
00384
00385 template <class CONT1,class CONT2>
00386 double ncc_vector( const CONT1 &patch1, const CONT2 &patch2 )
00387 {
00388 ASSERT_( patch1.size()==patch2.size() )
00389
00390 double numerator = 0, sum_a = 0, sum_b = 0, result, a_mean, b_mean;
00391 a_mean = patch1.mean();
00392 b_mean = patch2.mean();
00393
00394 const size_t N = patch1.size();
00395 for(size_t i=0;i<N;++i)
00396 {
00397 numerator += (patch1[i]-a_mean)*(patch2[i]-b_mean);
00398 sum_a += mrpt::utils::square(patch1[i]-a_mean);
00399 sum_b += mrpt::utils::square(patch2[i]-b_mean);
00400 }
00401 ASSERTMSG_(sum_a*sum_b!=0,"Divide by zero when normalizing.")
00402 result=numerator/std::sqrt(sum_a*sum_b);
00403 return result;
00404 }
00405
00406
00407
00408
00409
00410
00411 #if 0
00412 namespace detail
00413 {
00414 template <class CONTAINER>
00415 void saveToTextFileAsVector(
00416 const CONTAINER &M,
00417 const std::string &file,
00418 mrpt::math::TMatrixTextFileFormat fileFormat,
00419 bool asColumnVector)
00420 {
00421 using namespace mrpt::system;
00422 MRPT_START
00423 FILE *f=os::fopen(file.c_str(),"wt");
00424 if (!f) THROW_EXCEPTION_CUSTOM_MSG1("saveToTextFile: Error opening file '%s' for writing a matrix as text.", file.c_str());
00425
00426 for (typename CONTAINER::const_iterator it=M.begin();it!=M.end();++it)
00427 {
00428 switch(fileFormat)
00429 {
00430 case MATRIX_FORMAT_ENG: os::fprintf(f,"%.16e ",static_cast<double>(*it)); break;
00431 case MATRIX_FORMAT_FIXED: os::fprintf(f,"%.16f ",static_cast<double>(*it)); break;
00432 case MATRIX_FORMAT_INT: os::fprintf(f,"%i ",static_cast<int>(*it)); break;
00433 default:
00434 THROW_EXCEPTION("Unsupported value for the parameter 'fileFormat'!");
00435 };
00436
00437 if (asColumnVector) os::fprintf(f,"\n");
00438 }
00439 os::fclose(f);
00440 MRPT_END
00441 }
00442 }
00443
00444
00445
00446
00447
00448
00449 template <class CONTAINER1,class CONTAINER2>
00450 void Abs(const CONTAINER1 &dat_in, CONTAINER2 &dat_out) {
00451 dat_out.resize(dat_in.size());
00452 std::transform<typename CONTAINER1::const_iterator,typename CONTAINER2::iterator,typename CONTAINER1::value_type (*)(typename CONTAINER1::value_type)>
00453 (dat_in.begin(),dat_in.end(), dat_out.begin(), &::abs );
00454 }
00455
00456 template <class CONTAINER> inline CONTAINER Abs(const CONTAINER &dat_in)
00457 { CONTAINER ret; Abs(dat_in,ret); return ret; }
00458
00459
00460 template <class CONTAINER1,class CONTAINER2>
00461 void Log(const CONTAINER1 &dat_in, CONTAINER2 &dat_out) {
00462 dat_out.resize(dat_in.size());
00463 std::transform<typename CONTAINER1::const_iterator,typename CONTAINER2::iterator,typename CONTAINER1::value_type (*)(typename CONTAINER1::value_type)>
00464 (dat_in.begin(),dat_in.end(), dat_out.begin(), &::log );
00465 }
00466
00467 template <class CONTAINER> inline CONTAINER Log(const CONTAINER &dat_in)
00468 { CONTAINER ret; Log(dat_in,ret); return ret; }
00469
00470
00471 template <class CONTAINER1,class CONTAINER2>
00472 void Exp(const CONTAINER1 &dat_in, CONTAINER2 &dat_out) {
00473 dat_out.resize(dat_in.size());
00474 std::transform<typename CONTAINER1::const_iterator,typename CONTAINER2::iterator,typename CONTAINER1::value_type (*)(typename CONTAINER1::value_type)>
00475 (dat_in.begin(),dat_in.end(), dat_out.begin(), &::exp );
00476 }
00477
00478 template <class CONTAINER> inline CONTAINER Exp(const CONTAINER &dat_in)
00479 { CONTAINER ret; Exp(dat_in,ret); return ret; }
00480
00481
00482
00483 template <class CONTAINER1,class CONTAINER2>
00484 RET_TYPE_ASSERT_MRPTCONTAINER(CONTAINER1, bool)
00485 operator == (const CONTAINER1 &m1, const CONTAINER2 &m2)
00486 {
00487 if (m1.size()!=m2.size()) return false;
00488 typename CONTAINER1::const_iterator it1=m1.begin();
00489 typename CONTAINER2::const_iterator it2=m2.begin();
00490 const size_t N = m1.size();
00491 for(size_t i=0;i<N;++i, ++it1,++it2)
00492 if (*it1!=*it2)
00493 return false;
00494 return true;
00495 }
00496
00497
00498 template <class CONTAINER1,class CONTAINER2>
00499 inline RET_TYPE_ASSERT_MRPTCONTAINER(CONTAINER1, bool)
00500 operator != (const CONTAINER1 &m1, const CONTAINER2 &m2)
00501 {
00502 return !(m1 == m2);
00503 }
00504
00505
00506 #endif
00507
00508 }
00509 }
00510
00511
00512 #endif