MRPT  1.9.9
ops_containers.h
Go to the documentation of this file.
1 /* +------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | https://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2019, Individual contributors, see AUTHORS file |
6  | See: https://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See: https://www.mrpt.org/License |
8  +------------------------------------------------------------------------+ */
9 #pragma once
10 
11 #include <mrpt/core/bits_math.h> // keep_max(),...
12 #include <mrpt/math/CHistogram.h>
13 #include <mrpt/math/math_frwds.h>
14 #include <algorithm>
15 #include <cmath>
16 #include <functional>
17 #include <numeric>
18 #include <type_traits>
19 
20 #include "ops_vectors.h"
21 
22 /** \addtogroup container_ops_grp Vector and matrices mathematical operations
23  * and other utilities
24  * \ingroup mrpt_math_grp
25  * @{ */
26 
27 /** \file ops_containers.h
28  * This file implements several operations that operate element-wise on
29  * individual or pairs of containers.
30  * Containers here means any of: mrpt::math::CVectorTemplace,
31  * mrpt::math::CArray, mrpt::math::CMatrixFixed,
32  * mrpt::math::CMatrixDynamic.
33  *
34  */
35 
36 namespace mrpt::math
37 {
38 /** ContainerType<T>::element_t exposes the value of any STL or Eigen container
39  */
40 template <typename CONTAINER>
42 
43 /** Specialization for Eigen containers */
44 template <typename Derived>
45 struct ContainerType<Eigen::EigenBase<Derived>>
46 {
47  using element_t = typename Derived::Scalar;
48 };
49 /** Specialization for MRPT containers */
50 template <typename Scalar, typename Derived>
51 struct ContainerType<mrpt::math::MatrixVectorBase<Scalar, Derived>>
52 {
53  using element_t = Scalar;
54 };
55 
56 /** Computes the normalized or normal histogram of a sequence of numbers given
57  * the number of bins and the limits.
58  * In any case this is a "linear" histogram, i.e. for matrices, all the
59  * elements are taken as if they were a plain sequence, not taking into account
60  * they were in columns or rows.
61  * If desired, out_bin_centers can be set to receive the bins centers.
62  */
63 template <class CONTAINER>
64 std::vector<double> histogram(
65  const CONTAINER& v, double limit_min, double limit_max, size_t number_bins,
66  bool do_normalization = false,
67  std::vector<double>* out_bin_centers = nullptr)
68 {
69  mrpt::math::CHistogram H(limit_min, limit_max, number_bins);
70  std::vector<double> ret(number_bins);
71  std::vector<double> dummy_ret_bins;
72  H.add(v);
73  if (do_normalization)
75  out_bin_centers ? *out_bin_centers : dummy_ret_bins, ret);
76  else
77  H.getHistogram(
78  out_bin_centers ? *out_bin_centers : dummy_ret_bins, ret);
79  return ret;
80 }
81 
82 template <class EIGEN_CONTAINER>
83 void resizeLike(EIGEN_CONTAINER& trg, const EIGEN_CONTAINER& src)
84 {
85  trg.resizeLike(src);
86 }
87 template <typename T>
88 void resizeLike(std::vector<T>& trg, const std::vector<T>& src)
89 {
90  trg.resize(src.size());
91 }
92 
93 /** Computes the cumulative sum of all the elements, saving the result in
94  * another container.
95  * This works for both matrices (even mixing their types) and vectores/arrays
96  * (even mixing types),
97  * and even to store the cumsum of any matrix into any vector/array, but not
98  * in opposite direction.
99  * \sa sum */
100 template <class CONTAINER1, class CONTAINER2>
101 inline void cumsum_tmpl(const CONTAINER1& in_data, CONTAINER2& out_cumsum)
102 {
103  resizeLike(out_cumsum, in_data);
104  using T =
105  std::remove_const_t<std::remove_reference_t<decltype(in_data[0])>>;
106  T last = 0;
107  const size_t N = in_data.size();
108  for (size_t i = 0; i < N; i++) last = out_cumsum[i] = last + in_data[i];
109 }
110 
111 template <class CONTAINER1, class CONTAINER2>
112 inline void cumsum(const CONTAINER1& in_data, CONTAINER2& out_cumsum)
113 {
114  cumsum_tmpl<CONTAINER1, CONTAINER2>(in_data, out_cumsum);
115 }
116 
117 /** Computes the cumulative sum of all the elements
118  * \sa sum */
119 template <class CONTAINER>
120 inline CONTAINER cumsum(const CONTAINER& in_data)
121 {
122  CONTAINER ret;
123  cumsum(in_data, ret);
124  return ret;
125 }
126 
127 template <class CONTAINER>
128 inline typename CONTAINER::Scalar norm_inf(const CONTAINER& v)
129 {
130  return v.norm_inf();
131 }
132 template <class CONTAINER>
133 inline typename CONTAINER::Scalar norm(const CONTAINER& v)
134 {
135  return v.norm();
136 }
137 template <class CONTAINER, int = CONTAINER::is_mrpt_type>
138 inline typename CONTAINER::Scalar maximum(const CONTAINER& v)
139 {
140  return v.maxCoeff();
141 }
142 template <class CONTAINER, int = CONTAINER::is_mrpt_type>
143 inline typename CONTAINER::Scalar minimum(const CONTAINER& v)
144 {
145  return v.minCoeff();
146 }
147 
148 template <class Derived>
150 {
151  return v.maxCoeff();
152 }
153 template <class Derived>
155 {
156  return v.minCoeff();
157 }
158 
159 template <typename T>
160 inline T maximum(const std::vector<T>& v)
161 {
162  ASSERT_(!v.empty());
163  T m = v[0];
164  for (size_t i = 0; i < v.size(); i++) mrpt::keep_max(m, v[i]);
165  return m;
166 }
167 template <typename T>
168 inline T minimum(const std::vector<T>& v)
169 {
170  ASSERT_(!v.empty());
171  T m = v[0];
172  for (size_t i = 0; i < v.size(); i++) mrpt::keep_min(m, v[i]);
173  return m;
174 }
175 
176 /** \name Generic container element-wise operations - Miscelaneous
177  * @{
178  */
179 
180 /** Accumulate the squared-norm of a vector/array/matrix into "total" (this
181  * function is compatible with std::accumulate). */
182 template <class CONTAINER, typename VALUE>
183 VALUE squareNorm_accum(const VALUE total, const CONTAINER& v)
184 {
185  return total + v.squaredNorm();
186 }
187 
188 /** Compute the square norm of anything implementing [].
189  \sa norm */
190 template <size_t N, class T, class U>
191 inline T squareNorm(const U& v)
192 {
193  T res = 0;
194  for (size_t i = 0; i < N; i++) res += square(v[i]);
195  return res;
196 }
197 
198 /** v1*v2: The dot product of two containers (vectors/arrays/matrices) */
199 template <class CONTAINER1, class CONTAINER2>
201  const CONTAINER1& v1, const CONTAINER1& v2)
202 {
203  return v1.dot(v2);
204 }
205 
206 /** v1*v2: The dot product of any two objects supporting [] */
207 template <size_t N, class T, class U, class V>
208 inline T dotProduct(const U& v1, const V& v2)
209 {
210  T res = 0;
211  for (size_t i = 0; i < N; i++) res += v1[i] * v2[i];
212  return res;
213 }
214 
215 /** Computes the sum of all the elements.
216  * \note If used with containers of integer types (uint8_t, int, etc...) this
217  could overflow. In those cases, use sumRetType the second argument RET to
218  specify a larger type to hold the sum.
219  \sa cumsum */
220 template <class CONTAINER>
221 inline typename CONTAINER::Scalar sum(const CONTAINER& v)
222 {
223  return v.sum();
224 }
225 
226 /// \overload
227 template <typename T>
228 inline T sum(const std::vector<T>& v)
229 {
230  return std::accumulate(v.begin(), v.end(), T(0));
231 }
232 
233 /** Computes the sum of all the elements, with a custom return type.
234  \sa sum, cumsum */
235 template <class CONTAINER, typename RET>
236 inline RET sumRetType(const CONTAINER& v)
237 {
238  return v.template sumRetType<RET>();
239 }
240 
241 /** Computes the mean value of a vector \return The mean, as a double number.
242  * \sa math::stddev,math::meanAndStd */
243 template <class CONTAINER>
244 inline double mean(const CONTAINER& v)
245 {
246  if (v.empty())
247  return 0;
248  else
249  return sum(v) / static_cast<double>(v.size());
250 }
251 
252 /** Return the maximum and minimum values of a std::vector */
253 template <typename T>
254 inline void minimum_maximum(const std::vector<T>& V, T& curMin, T& curMax)
255 {
256  ASSERT_(V.size() != 0);
257  const size_t N = V.size();
258  curMin = curMax = V[0];
259  for (size_t i = 1; i < N; i++)
260  {
261  mrpt::keep_min(curMin, V[i]);
262  mrpt::keep_max(curMax, V[i]);
263  }
264 }
265 
266 /** Return the maximum and minimum values of a Eigen-based vector or matrix */
267 template <class Derived>
268 inline void minimum_maximum(
270  typename Eigen::MatrixBase<Derived>::Scalar& curMin,
271  typename Eigen::MatrixBase<Derived>::Scalar& curMax)
272 {
273  V.minimum_maximum(curMin, curMax);
274 }
275 
276 /** Scales all elements such as the minimum & maximum values are shifted to the
277  * given values */
278 template <class CONTAINER, typename Scalar>
279 void normalize(CONTAINER& c, Scalar valMin, Scalar valMax)
280 {
281  if (c.empty()) return;
282  const Scalar curMin = c.minCoeff();
283  const Scalar curMax = c.maxCoeff();
284  Scalar minMaxDelta = curMax - curMin;
285  if (minMaxDelta == 0) minMaxDelta = 1;
286  const Scalar minMaxDelta_ = (valMax - valMin) / minMaxDelta;
287  c.array() = (c.array() - curMin) * minMaxDelta_ + valMin;
288 }
289 
290 /** Counts the number of elements that appear in both STL-like containers
291  * (comparison through the == operator)
292  * It is assumed that no repeated elements appear within each of the
293  * containers. */
294 template <class CONTAINER1, class CONTAINER2>
295 size_t countCommonElements(const CONTAINER1& a, const CONTAINER2& b)
296 {
297  size_t ret = 0;
298  for (auto it1 = a.begin(); it1 != a.end(); ++it1)
299  for (auto it2 = b.begin(); it2 != b.end(); ++it2)
300  if ((*it1) == (*it2)) ret++;
301  return ret;
302 }
303 
304 /** Adjusts the range of all the elements such as the minimum and maximum values
305  * being those supplied by the user. */
306 template <class CONTAINER>
308  CONTAINER& m, const typename CONTAINER::Scalar minVal,
309  const typename CONTAINER::Scalar maxVal)
310 {
311  if (size_t(m.size()) == 0) return;
312  typename CONTAINER::Scalar curMin, curMax;
313  minimum_maximum(m, curMin, curMax);
314  const typename CONTAINER::Scalar curRan = curMax - curMin;
315  m -= (curMin + minVal);
316  if (curRan != 0) m *= (maxVal - minVal) / curRan;
317 }
318 
319 /** Computes the standard deviation of a vector (or all elements of a matrix)
320  * \param v The set of data, either as a vector, or a matrix (arrangement of
321  * data is ignored in this function).
322  * \param out_mean The output for the estimated mean
323  * \param out_std The output for the estimated standard deviation
324  * \param unbiased If set to true or false the std is normalized by "N-1" or
325  * "N", respectively.
326  * \sa math::mean,math::stddev
327  */
328 template <class VECTORLIKE>
330  const VECTORLIKE& v, double& out_mean, double& out_std,
331  bool unbiased = true)
332 {
333  if (v.size() < 2)
334  {
335  out_std = 0;
336  out_mean = (v.size() == 1) ? *v.begin() : 0;
337  }
338  else
339  {
340  // Compute the mean:
341  const size_t N = v.size();
342  out_mean = mrpt::math::sum(v) / static_cast<double>(N);
343  // Compute the std:
344  double vector_std = 0;
345  for (size_t i = 0; i < N; i++)
346  vector_std += mrpt::square(v[i] - out_mean);
347  out_std =
348  std::sqrt(vector_std / static_cast<double>(N - (unbiased ? 1 : 0)));
349  }
350 }
351 
352 /** Computes the standard deviation of a vector
353  * \param v The set of data
354  * \param unbiased If set to true or false the std is normalized by "N-1" or
355  * "N", respectively.
356  * \sa math::mean,math::meanAndStd
357  */
358 template <class VECTORLIKE>
359 inline double stddev(const VECTORLIKE& v, bool unbiased = true)
360 {
361  double m, s;
362  meanAndStd(v, m, s, unbiased);
363  return s;
364 }
365 
366 /** Computes the mean vector and covariance from a list of values given as a
367  * vector of vectors, where each row is a sample.
368  * \param v The set of data, as a vector of N vectors of M elements.
369  * \param out_mean The output M-vector for the estimated mean.
370  * \param out_cov The output MxM matrix for the estimated covariance matrix.
371  * \sa mrpt::math::meanAndCovMat, math::mean,math::stddev, math::cov
372  */
373 template <class VECTOR_OF_VECTOR, class VECTORLIKE, class MATRIXLIKE>
375  const VECTOR_OF_VECTOR& v, VECTORLIKE& out_mean, MATRIXLIKE& out_cov)
376 {
377  const size_t N = v.size();
378  ASSERTMSG_(N > 0, "The input vector contains no elements");
379  const double N_inv = 1.0 / N;
380 
381  const size_t M = v[0].size();
382  ASSERTMSG_(M > 0, "The input vector contains rows of length 0");
383 
384  // First: Compute the mean
385  out_mean.assign(M, 0);
386  for (size_t i = 0; i < N; i++)
387  for (size_t j = 0; j < M; j++) out_mean[j] += v[i][j];
388 
389  for (size_t j = 0; j < M; j++) out_mean[j] *= N_inv;
390 
391  // Second: Compute the covariance
392  // Save only the above-diagonal part, then after averaging
393  // duplicate that part to the other half.
394  out_cov.setZero(M, M);
395  for (size_t i = 0; i < N; i++)
396  {
397  for (size_t j = 0; j < M; j++)
398  out_cov(j, j) += square(v[i][j] - out_mean[j]);
399 
400  for (size_t j = 0; j < M; j++)
401  for (size_t k = j + 1; k < M; k++)
402  out_cov(j, k) +=
403  (v[i][j] - out_mean[j]) * (v[i][k] - out_mean[k]);
404  }
405  for (size_t j = 0; j < M; j++)
406  for (size_t k = j + 1; k < M; k++) out_cov(k, j) = out_cov(j, k);
407  out_cov *= N_inv;
408 }
409 
410 /** Computes the covariance matrix from a list of values given as a vector of
411  * vectors, where each row is a sample.
412  * \param v The set of data, as a vector of N vectors of M elements.
413  * \param out_cov The output MxM matrix for the estimated covariance matrix.
414  * \tparam RETURN_MATRIX The type of the returned matrix, e.g. Eigen::MatrixXd
415  * \sa math::mean,math::stddev, math::cov, meanAndCovVec
416  */
417 template <class VECTOR_OF_VECTOR, class RETURN_MATRIX>
418 inline RETURN_MATRIX covVector(const VECTOR_OF_VECTOR& v)
419 {
420  std::vector<double> m;
421  RETURN_MATRIX C;
422  meanAndCovVec(v, m, C);
423  return C;
424 }
425 
426 /** Normalised Cross Correlation between two vector patches
427  * The Matlab code for this is
428  * a = a - mean2(a);
429  * b = b - mean2(b);
430  * r = sum(sum(a.*b))/sqrt(sum(sum(a.*a))*sum(sum(b.*b)));
431  */
432 template <class CONT1, class CONT2>
433 double ncc_vector(const CONT1& patch1, const CONT2& patch2)
434 {
435  ASSERT_(patch1.size() == patch2.size());
436 
437  double numerator = 0, sum_a = 0, sum_b = 0, result, a_mean, b_mean;
438  a_mean = patch1.mean();
439  b_mean = patch2.mean();
440 
441  const size_t N = patch1.size();
442  for (size_t i = 0; i < N; ++i)
443  {
444  numerator += (patch1[i] - a_mean) * (patch2[i] - b_mean);
445  sum_a += mrpt::square(patch1[i] - a_mean);
446  sum_b += mrpt::square(patch2[i] - b_mean);
447  }
448  ASSERTMSG_(sum_a * sum_b != 0, "Divide by zero when normalizing.");
449  result = numerator / std::sqrt(sum_a * sum_b);
450  return result;
451 }
452 
453 /** @} Misc ops */
454 
455 } // namespace mrpt::math
456 /** @} */ // end of grouping
This class provides an easy way of computing histograms for unidimensional real valued variables...
Definition: CHistogram.h:33
double Scalar
Definition: KmUtils.h:43
size_t countCommonElements(const CONTAINER1 &a, const CONTAINER2 &b)
Counts the number of elements that appear in both STL-like containers (comparison through the == oper...
double stddev(const VECTORLIKE &v, bool unbiased=true)
Computes the standard deviation of a vector.
T squareNorm(const U &v)
Compute the square norm of anything implementing [].
void keep_min(T &var, const K test_val)
If the second argument is below the first one, set the first argument to this lower value...
void resizeLike(EIGEN_CONTAINER &trg, const EIGEN_CONTAINER &src)
GLdouble s
Definition: glext.h:3682
GLuint src
Definition: glext.h:7397
T square(const T x)
Inline function for the square of a number.
#define ASSERT_(f)
Defines an assertion mechanism.
Definition: exceptions.h:120
This base provides a set of functions for maths stuff.
void keep_max(T &var, const K test_val)
If the second argument is above the first one, set the first argument to this higher value...
void normalize(CONTAINER &c, Scalar valMin, Scalar valMax)
Scales all elements such as the minimum & maximum values are shifted to the given values...
const GLubyte * c
Definition: glext.h:6406
void add(const double x)
Add an element to the histogram.
Definition: CHistogram.cpp:42
CONTAINER::Scalar sum(const CONTAINER &v)
Computes the sum of all the elements.
CONTAINER::Scalar maximum(const CONTAINER &v)
GLubyte GLubyte b
Definition: glext.h:6372
#define ASSERTMSG_(f, __ERROR_MSG)
Defines an assertion mechanism.
Definition: exceptions.h:108
VALUE squareNorm_accum(const VALUE total, const CONTAINER &v)
Accumulate the squared-norm of a vector/array/matrix into "total" (this function is compatible with s...
void getHistogram(std::vector< double > &x, std::vector< double > &hits) const
Returns the list of bin centers & hit counts.
Definition: CHistogram.cpp:77
void minimum_maximum(const std::vector< T > &V, T &curMin, T &curMax)
Return the maximum and minimum values of a std::vector.
void cumsum(const CONTAINER1 &in_data, CONTAINER2 &out_cumsum)
ContainerType<T>::element_t exposes the value of any STL or Eigen container.
CONTAINER::Scalar norm_inf(const CONTAINER &v)
CONTAINER::Scalar minimum(const CONTAINER &v)
const GLdouble * v
Definition: glext.h:3684
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
void meanAndStd(const VECTORLIKE &v, double &out_mean, double &out_std, bool unbiased=true)
Computes the standard deviation of a vector (or all elements of a matrix)
double ncc_vector(const CONT1 &patch1, const CONT2 &patch2)
Normalised Cross Correlation between two vector patches The Matlab code for this is a = a - mean2(a);...
GLfloat GLfloat v1
Definition: glext.h:4121
RETURN_MATRIX covVector(const VECTOR_OF_VECTOR &v)
Computes the covariance matrix from a list of values given as a vector of vectors, where each row is a sample.
RET sumRetType(const CONTAINER &v)
Computes the sum of all the elements, with a custom return type.
CONTAINER1::Scalar dotProduct(const CONTAINER1 &v1, const CONTAINER1 &v2)
v1*v2: The dot product of two containers (vectors/arrays/matrices)
std::vector< double > histogram(const CONTAINER &v, double limit_min, double limit_max, size_t number_bins, bool do_normalization=false, std::vector< double > *out_bin_centers=nullptr)
Computes the normalized or normal histogram of a sequence of numbers given the number of bins and the...
double mean(const CONTAINER &v)
Computes the mean value of a vector.
void cumsum_tmpl(const CONTAINER1 &in_data, CONTAINER2 &out_cumsum)
Computes the cumulative sum of all the elements, saving the result in another container.
void meanAndCovVec(const VECTOR_OF_VECTOR &v, VECTORLIKE &out_mean, MATRIXLIKE &out_cov)
Computes the mean vector and covariance from a list of values given as a vector of vectors...
GLfloat GLfloat GLfloat v2
Definition: glext.h:4123
Base CRTP class for all MRPT vectors and matrices.
GLuint res
Definition: glext.h:7385
void adjustRange(CONTAINER &m, const typename CONTAINER::Scalar minVal, const typename CONTAINER::Scalar maxVal)
Adjusts the range of all the elements such as the minimum and maximum values being those supplied by ...
GLubyte GLubyte GLubyte a
Definition: glext.h:6372
void getHistogramNormalized(std::vector< double > &x, std::vector< double > &hits) const
Returns the list of bin centers & hit counts, normalized such as the integral of the histogram...
Definition: CHistogram.cpp:86
CONTAINER::Scalar norm(const CONTAINER &v)



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: dad381fd7 Sun Oct 20 13:36:46 2019 +0200 at dom oct 20 13:40:10 CEST 2019