Main MRPT website > C++ reference
MRPT logo
ops_containers.h
Go to the documentation of this file.
1 /* +---------------------------------------------------------------------------+
2  | The Mobile Robot Programming Toolkit (MRPT) C++ library |
3  | |
4  | http://www.mrpt.org/ |
5  | |
6  | Copyright (C) 2005-2012 University of Malaga |
7  | |
8  | This software was written by the Machine Perception and Intelligent |
9  | Robotics Lab, University of Malaga (Spain). |
10  | Contact: Jose-Luis Blanco <jlblanco@ctima.uma.es> |
11  | |
12  | This file is part of the MRPT project. |
13  | |
14  | MRPT is free software: you can redistribute it and/or modify |
15  | it under the terms of the GNU General Public License as published by |
16  | the Free Software Foundation, either version 3 of the License, or |
17  | (at your option) any later version. |
18  | |
19  | MRPT is distributed in the hope that it will be useful, |
20  | but WITHOUT ANY WARRANTY; without even the implied warranty of |
21  | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the |
22  | GNU General Public License for more details. |
23  | |
24  | You should have received a copy of the GNU General Public License |
25  | along with MRPT. If not, see <http://www.gnu.org/licenses/>. |
26  | |
27  +---------------------------------------------------------------------------+ */
28 #ifndef mrpt_math_container_ops_H
29 #define mrpt_math_container_ops_H
30 
31 #include <mrpt/math/math_frwds.h> // Fordward declarations
32 
33 #include <mrpt/math/lightweight_geom_data.h> // Fordward declarations
34 
35 #include <functional>
36 #include <algorithm>
37 #include <cmath>
38 
39 /** \addtogroup container_ops_grp Vector and matrices mathematical operations and other utilities
40  * \ingroup mrpt_base_grp
41  * @{ */
42 
43 /** \file ops_containers.h
44  * This file implements several operations that operate element-wise on individual or pairs of containers.
45  * Containers here means any of: mrpt::math::CVectorTemplace, mrpt::math::CArray, mrpt::math::CMatrixFixedNumeric, mrpt::math::CMatrixTemplate.
46  *
47  * In general, any container having a type "mrpt_autotype" self-referencing to the type itself, and a dummy struct mrpt_container<>
48  * which is only used as a way to force the compiler to assure that BOTH containers are valid ones in binary operators.
49  * This restrictions
50  * have been designed as a way to provide "polymorphism" at a template level, so the "+,-,..." operators do not
51  * generate ambiguities for ANY type, and limiting them to MRPT containers.
52  *
53  * In some cases, the containers provide specializations of some operations, for increased performance.
54  */
55 
56 #include <algorithm>
57 #include <numeric>
58 #include <functional>
59 
60 #include <mrpt/math/CHistogram.h> // Used in ::histogram()
61 
62 
63 namespace mrpt
64 {
65  namespace math
66  {
67  /** Computes the normalized or normal histogram of a sequence of numbers given the number of bins and the limits.
68  * 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.
69  * If desired, out_bin_centers can be set to receive the bins centers.
70  */
71  template<class CONTAINER>
72  vector_double histogram(
73  const CONTAINER &v,
74  double limit_min,
75  double limit_max,
76  size_t number_bins,
77  bool do_normalization = false,
78  vector_double *out_bin_centers = NULL)
79  {
80  mrpt::math::CHistogram H( limit_min, limit_max, number_bins );
81  vector_double ret(number_bins);
82  vector_double dummy_ret_bins;
83  H.add(v);
84  if (do_normalization)
85  H.getHistogramNormalized( out_bin_centers ? *out_bin_centers : dummy_ret_bins, ret );
86  else H.getHistogram( out_bin_centers ? *out_bin_centers : dummy_ret_bins, ret );
87  return ret;
88  }
89 
90  /** Computes the cumulative sum of all the elements, saving the result in another container.
91  * This works for both matrices (even mixing their types) and vectores/arrays (even mixing types),
92  * and even to store the cumsum of any matrix into any vector/array, but not in opposite direction.
93  * \sa sum */
94  template <class CONTAINER1,class CONTAINER2>
95  inline void cumsum(const CONTAINER1 &in_data, CONTAINER2 &out_cumsum)
96  {
97  out_cumsum.resizeLike(in_data);
98  typename CONTAINER1::value_type last=0;
99  const size_t N = in_data.size();
100  for (size_t i=0;i<N;i++)
101  last = out_cumsum[i] = last + in_data[i];
102  }
103 
104  /** Computes the cumulative sum of all the elements
105  * \sa sum */
106  template<class CONTAINER>
107  inline CONTAINER cumsum(const CONTAINER &in_data)
108  {
109  CONTAINER ret;
110  cumsum(in_data,ret);
111  return ret;
112  }
113 
114  template <class CONTAINER> inline typename CONTAINER::value_type norm_inf(const CONTAINER &v) { return v.norm_inf(); }
115  template <class CONTAINER> inline typename CONTAINER::value_type norm(const CONTAINER &v) { return v.norm(); }
116  template <class CONTAINER> inline typename CONTAINER::value_type maximum(const CONTAINER &v) { return v.maximum(); }
117  template <class CONTAINER> inline typename CONTAINER::value_type minimum(const CONTAINER &v) { return v.minimum(); }
118 
119  template <typename T> inline T maximum(const std::vector<T> &v)
120  {
121  ASSERT_(!v.empty())
122  T m = v[0];
123  for (size_t i=0;i<v.size();i++) mrpt::utils::keep_max(m,v[i]);
124  return m;
125  }
126  template <typename T> inline T minimum(const std::vector<T> &v)
127  {
128  ASSERT_(!v.empty())
129  T m = v[0];
130  for (size_t i=0;i<v.size();i++) mrpt::utils::keep_min(m,v[i]);
131  return m;
132  }
133 
134  /** \name Container initializer from pose classes
135  * @{
136  */
137 
138  /** Conversion of poses to MRPT containers (vector/matrix) */
139  template <class CONTAINER> CONTAINER & containerFromPoseOrPoint(CONTAINER &C, const TPoint2D &p) {
140  C.resize(2,1);
141  for (size_t i=0;i<2;i++) C.coeffRef(i,0)=p[i];
142  return C;
143  }
144  template <class CONTAINER> CONTAINER & containerFromPoseOrPoint(CONTAINER &C, const TPoint3D &p) {
145  C.resize(3,1);
146  for (size_t i=0;i<3;i++) C.coeffRef(i,0)=p[i];
147  return C;
148  }
149  template <class CONTAINER> CONTAINER & containerFromPoseOrPoint(CONTAINER &C, const TPose2D &p) {
150  C.resize(3,1);
151  for (size_t i=0;i<3;i++) C.coeffRef(i,0)=p[i];
152  return C;
153  }
154  template <class CONTAINER> CONTAINER & containerFromPoseOrPoint(CONTAINER &C, const TPose3D &p) {
155  C.resize(6,1);
156  for (size_t i=0;i<6;i++) C.coeffRef(i,0)=p[i];
157  return C;
158  }
159  template <class CONTAINER> CONTAINER & containerFromPoseOrPoint(CONTAINER &C, const TPose3DQuat &p) {
160  C.resize(7,1);
161  for (size_t i=0;i<7;i++) C.coeffRef(i,0)=p[i];
162  return C;
163  }
164 
165  /** @} */
166 
167 
168 
169  /** \name Generic container element-wise operations - Miscelaneous
170  * @{
171  */
172 
173  /** Accumulate the squared-norm of a vector/array/matrix into "total" (this function is compatible with std::accumulate). */
174  template <class CONTAINER>
175  typename CONTAINER::value_type squareNorm_accum(const typename CONTAINER::value_type total, const CONTAINER &v);
176  template <class CONTAINER> typename CONTAINER::value_type squareNorm_accum(const typename CONTAINER::value_type total, const CONTAINER &v) {
177  return total+v.squaredNorm();
178  }
179 
180  /** Compute the square norm of anything implementing [].
181  \sa norm */
182  template<size_t N,class T,class U>
183  inline T squareNorm(const U &v) {
184  T res=0;
185  for (size_t i=0;i<N;i++) res+=square(v[i]);
186  return res;
187  }
188 
189  /** v1·v2: The dot product of two containers (vectors/arrays/matrices) */
190  template <class CONTAINER1,class CONTAINER2>
191  inline typename CONTAINER1::value_type
192  dotProduct(const CONTAINER1 &v1,const CONTAINER1 &v2)
193  {
194  return v1.dot(v2);
195  }
196 
197  /** v1·v2: The dot product of any two objects supporting [] */
198  template<size_t N,class T,class U,class V>
199  inline T dotProduct(const U &v1,const V &v2) {
200  T res=0;
201  for (size_t i=0;i<N;i++) res+=v1[i]*v2[i];
202  return res;
203  }
204 
205  /** Computes the sum of all the elements.
206  * \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.
207  \sa cumsum */
208  template <class CONTAINER> inline typename CONTAINER::value_type sum(const CONTAINER &v) { return v.sum(); }
209 
210  /// \overload
211  template <typename T> inline T sum(const std::vector<T> &v) { return std::accumulate(v.begin(),v.end(),T(0)); }
212 
213  /** Computes the sum of all the elements, with a custom return type.
214  \sa sum, cumsum */
215  template <class CONTAINER,typename RET> inline RET sumRetType(const CONTAINER &v) { return v.sumRetType<RET>(); }
216 
217  /** Computes the mean value of a vector \return The mean, as a double number.
218  * \sa math::stddev,math::meanAndStd */
219  template <class CONTAINER>
220  inline double mean(const CONTAINER &v)
221  {
222  if (v.empty())
223  return 0;
224  else return sum(v)/static_cast<double>(v.size());
225  }
226 
227  /** Return the maximum and minimum values of a std::vector */
228  template <typename T>
229  inline void minimum_maximum(const std::vector<T> &V, T&curMin,T&curMax)
230  {
231  ASSERT_(V.size()!=0)
232  const size_t N=V.size();
233  curMin=curMax=V[0];
234  for (size_t i=1;i<N;i++)
235  {
236  mrpt::utils::keep_min(curMin,V[i]);
237  mrpt::utils::keep_max(curMax,V[i]);
238  }
239  }
240 
241  /** Return the maximum and minimum values of a Eigen-based vector or matrix */
242  template <class Derived>
243  inline void minimum_maximum(
244  const Eigen::MatrixBase<Derived> &V,
245  typename Eigen::MatrixBase<Derived>::value_type &curMin,
246  typename Eigen::MatrixBase<Derived>::value_type &curMax)
247  {
248  V.minimum_maximum(curMin,curMax);
249  }
250 
251  /** Counts the number of elements that appear in both STL-like containers (comparison through the == operator)
252  * It is assumed that no repeated elements appear within each of the containers. */
253  template <class CONTAINER1,class CONTAINER2>
254  size_t countCommonElements(const CONTAINER1 &a,const CONTAINER2 &b)
255  {
256  size_t ret=0;
257  for (typename CONTAINER1::const_iterator it1 = a.begin();it1!=a.end();it1++)
258  for (typename CONTAINER2::const_iterator it2 = b.begin();it2!=b.end();it2++)
259  if ( (*it1) == (*it2) )
260  ret++;
261  return ret;
262  }
263 
264  /** Adjusts the range of all the elements such as the minimum and maximum values being those supplied by the user. */
265  template <class CONTAINER>
266  void adjustRange(CONTAINER &m, const typename CONTAINER::value_type minVal,const typename CONTAINER::value_type maxVal)
267  {
268  if (size_t(m.size())==0) return;
269  typename CONTAINER::value_type curMin,curMax;
270  minimum_maximum(m,curMin,curMax);
271  const typename CONTAINER::value_type curRan = curMax-curMin;
272  m -= (curMin+minVal);
273  if (curRan!=0) m *= (maxVal-minVal)/curRan;
274  }
275 
276 
277  /** Computes the standard deviation of a vector
278  * \param v The set of data
279  * \param out_mean The output for the estimated mean
280  * \param out_std The output for the estimated standard deviation
281  * \param unbiased If set to true or false the std is normalized by "N-1" or "N", respectively.
282  * \sa math::mean,math::stddev
283  */
284  template<class VECTORLIKE>
285  void meanAndStd(
286  const VECTORLIKE &v,
287  double &out_mean,
288  double &out_std,
289  bool unbiased = true)
290  {
291  if (v.size()<2)
292  {
293  out_std = 0;
294  out_mean = (v.size()==1) ? *v.begin() : 0;
295  }
296  else
297  {
298  // Compute the mean:
299  const size_t N = v.size();
300  out_mean = mrpt::math::sum(v) / static_cast<double>(N);
301  // Compute the std:
302  double vector_std=0;
303  for (size_t i=0;i<N;i++) vector_std += mrpt::utils::square( v[i]-out_mean);
304  out_std = std::sqrt(vector_std / static_cast<double>(N - (unbiased ? 1:0)) );
305  }
306  }
307 
308 
309  /** Computes the standard deviation of a vector
310  * \param v The set of data
311  * \param unbiased If set to true or false the std is normalized by "N-1" or "N", respectively.
312  * \sa math::mean,math::meanAndStd
313  */
314  template<class VECTORLIKE>
315  inline double stddev(const VECTORLIKE &v, bool unbiased = true)
316  {
317  double m,s;
318  meanAndStd(v,m,s,unbiased);
319  return s;
320  }
321 
322  /** Computes the mean vector and covariance from a list of values given as a vector of vectors, where each row is a sample.
323  * \param v The set of data, as a vector of N vectors of M elements.
324  * \param out_mean The output M-vector for the estimated mean.
325  * \param out_cov The output MxM matrix for the estimated covariance matrix.
326  * \sa mrpt::math::meanAndCovMat, math::mean,math::stddev, math::cov
327  */
328  template<class VECTOR_OF_VECTOR, class VECTORLIKE, class MATRIXLIKE>
329  void meanAndCovVec(
330  const VECTOR_OF_VECTOR &v,
331  VECTORLIKE &out_mean,
332  MATRIXLIKE &out_cov
333  )
334  {
335  const size_t N = v.size();
336  ASSERTMSG_(N>0,"The input vector contains no elements");
337  const double N_inv = 1.0/N;
338 
339  const size_t M = v[0].size();
340  ASSERTMSG_(M>0,"The input vector contains rows of length 0");
341 
342  // First: Compute the mean
343  out_mean.assign(M,0);
344  for (size_t i=0;i<N;i++)
345  for (size_t j=0;j<M;j++)
346  out_mean[j]+=v[i][j];
347  out_mean*=N_inv;
348 
349  // Second: Compute the covariance
350  // Save only the above-diagonal part, then after averaging
351  // duplicate that part to the other half.
352  out_cov.zeros(M,M);
353  for (size_t i=0;i<N;i++)
354  {
355  for (size_t j=0;j<M;j++)
356  out_cov.get_unsafe(j,j)+=square(v[i][j]-out_mean[j]);
357 
358  for (size_t j=0;j<M;j++)
359  for (size_t k=j+1;k<M;k++)
360  out_cov.get_unsafe(j,k)+=(v[i][j]-out_mean[j])*(v[i][k]-out_mean[k]);
361  }
362  for (size_t j=0;j<M;j++)
363  for (size_t k=j+1;k<M;k++)
364  out_cov.get_unsafe(k,j) = out_cov.get_unsafe(j,k);
365  out_cov*=N_inv;
366  }
367 
368  /** Computes the covariance matrix from a list of values given as a vector of vectors, where each row is a sample.
369  * \param v The set of data, as a vector of N vectors of M elements.
370  * \param out_cov The output MxM matrix for the estimated covariance matrix.
371  * \sa math::mean,math::stddev, math::cov, meanAndCovVec
372  */
373  template<class VECTOR_OF_VECTOR>
374  inline Eigen::MatrixXd covVector( const VECTOR_OF_VECTOR &v )
375  {
376  vector_double m;
377  Eigen::MatrixXd C;
378  meanAndCovVec(v,m,C);
379  return C;
380  }
381 
382 
383  /** Normalised Cross Correlation between two vector patches
384  * The Matlab code for this is
385  * a = a - mean2(a);
386  * b = b - mean2(b);
387  * r = sum(sum(a.*b))/sqrt(sum(sum(a.*a))*sum(sum(b.*b)));
388  */
389  template <class CONT1,class CONT2>
390  double ncc_vector( const CONT1 &patch1, const CONT2 &patch2 )
391  {
392  ASSERT_( patch1.size()==patch2.size() )
393 
394  double numerator = 0, sum_a = 0, sum_b = 0, result, a_mean, b_mean;
395  a_mean = patch1.mean();
396  b_mean = patch2.mean();
397 
398  const size_t N = patch1.size();
399  for(size_t i=0;i<N;++i)
400  {
401  numerator += (patch1[i]-a_mean)*(patch2[i]-b_mean);
402  sum_a += mrpt::utils::square(patch1[i]-a_mean);
403  sum_b += mrpt::utils::square(patch2[i]-b_mean);
404  }
405  ASSERTMSG_(sum_a*sum_b!=0,"Divide by zero when normalizing.")
406  result=numerator/std::sqrt(sum_a*sum_b);
407  return result;
408  }
409 
410  /** @} Misc ops */
411 
412  } // End of math namespace
413 } // End of mrpt namespace
414 
415 /** @} */ // end of grouping
416 
417 #endif


Page generated by Doxygen 1.8.3 for MRPT 0.9.6 SVN: at Fri Feb 15 22:05:02 EST 2013