Main MRPT website > C++ reference
MRPT logo

ops_containers.h

Go to the documentation of this file.
00001 /* +---------------------------------------------------------------------------+
00002    |          The Mobile Robot Programming Toolkit (MRPT) C++ library          |
00003    |                                                                           |
00004    |                   http://mrpt.sourceforge.net/                            |
00005    |                                                                           |
00006    |   Copyright (C) 2005-2011  University of Malaga                           |
00007    |                                                                           |
00008    |    This software was written by the Machine Perception and Intelligent    |
00009    |      Robotics Lab, University of Malaga (Spain).                          |
00010    |    Contact: Jose-Luis Blanco  <jlblanco@ctima.uma.es>                     |
00011    |                                                                           |
00012    |  This file is part of the MRPT project.                                   |
00013    |                                                                           |
00014    |     MRPT is free software: you can redistribute it and/or modify          |
00015    |     it under the terms of the GNU General Public License as published by  |
00016    |     the Free Software Foundation, either version 3 of the License, or     |
00017    |     (at your option) any later version.                                   |
00018    |                                                                           |
00019    |   MRPT is distributed in the hope that it will be useful,                 |
00020    |     but WITHOUT ANY WARRANTY; without even the implied warranty of        |
00021    |     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         |
00022    |     GNU General Public License for more details.                          |
00023    |                                                                           |
00024    |     You should have received a copy of the GNU General Public License     |
00025    |     along with MRPT.  If not, see <http://www.gnu.org/licenses/>.         |
00026    |                                                                           |
00027    +---------------------------------------------------------------------------+ */
00028 #ifndef  mrpt_math_container_ops_H
00029 #define  mrpt_math_container_ops_H
00030 
00031 #include <mrpt/math/math_frwds.h>  // Fordward declarations
00032 
00033 #include <mrpt/math/lightweight_geom_data.h>  // Fordward declarations
00034 
00035 #include <functional>
00036 #include <algorithm>
00037 #include <cmath>
00038 
00039 /** \file ops_containers.h
00040   * This file implements several operations that operate element-wise on individual or pairs of containers.
00041   *  Containers here means any of: mrpt::math::CVectorTemplace, mrpt::math::CArray, mrpt::math::CMatrixFixedNumeric, mrpt::math::CMatrixTemplate.
00042   *
00043   *  In general, any container having a type "mrpt_autotype" self-referencing to the type itself, and a dummy struct mrpt_container<>
00044   *   which is only used as a way to force the compiler to assure that BOTH containers are valid ones in binary operators.
00045   *   This restrictions
00046   *   have been designed as a way to provide "polymorphism" at a template level, so the "+,-,..." operators do not
00047   *   generate ambiguities for ANY type, and limiting them to MRPT containers.
00048   *
00049   *   In some cases, the containers provide specializations of some operations, for increased performance.
00050   */
00051 
00052 #include <algorithm>
00053 #include <numeric>
00054 #include <functional>
00055 
00056 #include <mrpt/math/CHistogram.h>  // Used in ::histogram()
00057 
00058 
00059 namespace mrpt
00060 {
00061         namespace math
00062         {
00063                 /** Computes the normalized or normal histogram of a sequence of numbers given the number of bins and the limits.
00064                   *  In any case this is a "linear" histogram, i.e. for matrices, all the elements are taken as if they were a plain sequence, not taking into account they were in columns or rows.
00065                   *  If desired, out_bin_centers can be set to receive the bins centers.
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                 /** Computes the cumulative sum of all the elements, saving the result in another container.
00087                   *  This works for both matrices (even mixing their types) and vectores/arrays (even mixing types),
00088                   *  and even to store the cumsum of any matrix into any vector/array, but not in opposite direction.
00089                   * \sa sum */
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                 /** Computes the cumulative sum of all the elements
00101                   * \sa sum  */
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                 /** \name Container initializer from pose classes
00131                   * @{
00132                   */
00133 
00134                 /** Conversion of poses to MRPT containers (vector/matrix) */
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                 /** \name Generic container element-wise operations - Miscelaneous
00166                   * @{
00167                   */
00168 
00169                 /** Accumulate the squared-norm of a vector/array/matrix into "total" (this function is compatible with std::accumulate). */
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                 /** Compute the square norm of anything implementing [].
00177                   \sa norm */
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                 /** v1·v2: The dot product of two containers (vectors/arrays/matrices) */
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                 /** v1·v2: The dot product of any two objects supporting []  */
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                 /** Computes the sum of all the elements.
00202                   * \note If used with containers of integer types (uint8_t, int, etc...) this could overflow. In those cases, use sumRetType the second argument RET to specify a larger type to hold the sum.
00203                    \sa cumsum  */
00204                 template <class CONTAINER> inline typename CONTAINER::value_type sum(const CONTAINER &v) { return v.sum(); }
00205 
00206                 /// \overload
00207                 template <typename T> inline T sum(const std::vector<T> &v) { return std::accumulate(v.begin(),v.end(),T(0)); }
00208 
00209                 /** Computes the sum of all the elements, with a custom return type.
00210                    \sa sum, cumsum  */
00211                 template <class CONTAINER,typename RET> inline RET sumRetType(const CONTAINER &v) { return v.sumRetType<RET>(); }
00212 
00213                 /** Computes the mean value of a vector  \return The mean, as a double number.
00214                   * \sa math::stddev,math::meanAndStd  */
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                 /** Return the maximum and minimum values of a std::vector */
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                 /** Return the maximum and minimum values of a Eigen-based vector or matrix */
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                 /** Counts the number of elements that appear in both STL-like containers (comparison through the == operator)
00248                   *  It is assumed that no repeated elements appear within each of the containers.  */
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                 /** Adjusts the range of all the elements such as the minimum and maximum values being those supplied by the user.  */
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                 /** Computes the standard deviation of a vector
00274                   * \param v The set of data
00275                   * \param out_mean The output for the estimated mean
00276                   * \param out_std The output for the estimated standard deviation
00277                   * \param unbiased If set to true or false the std is normalized by "N-1" or "N", respectively.
00278                   * \sa math::mean,math::stddev
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                                 // Compute the mean:
00295                                 const size_t N = v.size();
00296                                 out_mean = mrpt::math::sum(v) / static_cast<double>(N);
00297                                 // Compute the std:
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                 /** Computes the standard deviation of a vector
00306                   * \param v The set of data
00307                   * \param unbiased If set to true or false the std is normalized by "N-1" or "N", respectively.
00308                   * \sa math::mean,math::meanAndStd
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                 /** Computes the mean vector and covariance from a list of values given as a vector of vectors, where each row is a sample.
00319                   * \param v The set of data, as a vector of N vectors of M elements.
00320                   * \param out_mean The output M-vector for the estimated mean.
00321                   * \param out_cov The output MxM matrix for the estimated covariance matrix.
00322                   * \sa mrpt::math::meanAndCovMat, math::mean,math::stddev, math::cov
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                         // First: Compute the mean
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                         // Second: Compute the covariance
00346                         //  Save only the above-diagonal part, then after averaging
00347                         //  duplicate that part to the other half.
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                 /** Computes the covariance matrix from a list of values given as a vector of vectors, where each row is a sample.
00365                   * \param v The set of data, as a vector of N vectors of M elements.
00366                   * \param out_cov The output MxM matrix for the estimated covariance matrix.
00367                   * \sa math::mean,math::stddev, math::cov, meanAndCovVec
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                 /** Normalised Cross Correlation between two vector patches
00380                   * The Matlab code for this is
00381                   * a = a - mean2(a);
00382                   * b = b - mean2(b);
00383                   * r = sum(sum(a.*b))/sqrt(sum(sum(a.*a))*sum(sum(b.*b)));
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                 /** @} Misc ops */
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                                         // If saving as a column, save a newline char:
00437                                         if (asColumnVector) os::fprintf(f,"\n");
00438                                 }
00439                                 os::fclose(f);
00440                                 MRPT_END
00441                         }
00442                 }
00443 
00444 
00445                 /** \name Generic container element-wise operations - Mathematical functions
00446                   * @{
00447                   */
00448                 /** out = abs(in), element by element */
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                 /** return abs(in), element by element */
00456                 template <class CONTAINER>  inline CONTAINER Abs(const CONTAINER &dat_in)
00457                 { CONTAINER ret; Abs(dat_in,ret); return ret; }
00458 
00459                 /** out = log(in), element by element */
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                 /** return log(in), element by element */
00467                 template <class CONTAINER>  inline CONTAINER Log(const CONTAINER &dat_in)
00468                 { CONTAINER ret; Log(dat_in,ret); return ret; }
00469 
00470                 /** out = exp(in), element by element */
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                 /** return exp(in), element by element */
00478                 template <class CONTAINER>  inline CONTAINER Exp(const CONTAINER &dat_in)
00479                 { CONTAINER ret; Exp(dat_in,ret); return ret; }
00480 
00481 
00482                 /** Logical equal-to == operator between any pair of matrix or vectors */
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                 /** Logical not-equal-to != operator between any pair of matrix or vectors  */
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                 /** @} Math ops */
00506 #endif
00507 
00508         } // End of math namespace
00509 } // End of mrpt namespace
00510 
00511 
00512 #endif



Page generated by Doxygen 1.7.3 for MRPT 0.9.4 SVN:exported at Tue Jan 25 21:56:31 UTC 2011