MRPT  2.0.4
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-2020, 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.size()) return; // empty() is not defined for Eigen classes
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
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...
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 resizeLike(EIGEN_CONTAINER &trg, const EIGEN_CONTAINER &src)
#define ASSERT_(f)
Defines an assertion mechanism.
Definition: exceptions.h:120
This base provides a set of functions for maths stuff.
void normalize(CONTAINER &c, Scalar valMin, Scalar valMax)
Scales all elements such as the minimum & maximum values are shifted to the given values...
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)
#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.
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...
CONTAINER::Scalar norm_inf(const CONTAINER &v)
return_t square(const num_t x)
Inline function for the square of a number.
CONTAINER::Scalar minimum(const CONTAINER &v)
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);...
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...
Base CRTP class for all MRPT vectors and matrices.
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 ...
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 2.0.4 Git: 33de1d0ad Sat Jun 20 11:02:42 2020 +0200 at sáb jun 20 17:35:17 CEST 2020