MRPT  2.0.1
transform_gaussian.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 
12 #include <mrpt/math/CMatrixFixed.h>
13 #include <mrpt/math/data_utils.h>
14 #include <mrpt/math/math_frwds.h>
15 #include <mrpt/math/num_jacobian.h>
16 #include <mrpt/math/ops_matrices.h>
17 #include <mrpt/random.h>
18 #include <functional>
19 #include <vector>
20 
21 namespace mrpt::math
22 {
23 /** @addtogroup gausspdf_transform_grp Gaussian PDF transformation functions
24  * \ingroup mrpt_math_grp
25  * @{ */
26 
27 /** Scaled unscented transformation (SUT) for estimating the Gaussian
28  * distribution of a variable Y=f(X) for an arbitrary function f() provided by
29  * the user.
30  * The user must supply the function in "functor" which takes points in the X
31  * space and returns the mapped point in Y, optionally using an extra, constant
32  * parameter ("fixed_param") which remains constant.
33  *
34  * The parameters alpha, K and beta are the common names of the SUT method,
35  * and the default values are those recommended in most papers.
36  *
37  * \param elem_do_wrap2pi If !=nullptr; it must point to an array of "bool" of
38  * size()==dimension of each element, stating if it's needed to do a wrap to
39  * [-pi,pi] to each dimension.
40  * \sa The example in MRPT/samples/unscented_transform_test
41  * \sa transform_gaussian_montecarlo, transform_gaussian_linear
42  */
43 template <
44  class VECTORLIKE1, class MATLIKE1, class USERPARAM, class VECTORLIKE2,
45  class VECTORLIKE3, class MATLIKE2>
47  const VECTORLIKE1& x_mean, const MATLIKE1& x_cov,
48  void (*functor)(
49  const VECTORLIKE1& x, const USERPARAM& fixed_param, VECTORLIKE3& y),
50  const USERPARAM& fixed_param, VECTORLIKE2& y_mean, MATLIKE2& y_cov,
51  const bool* elem_do_wrap2pi = nullptr, const double alpha = 1e-3,
52  const double K = 0, const double beta = 2.0)
53 {
55  const size_t Nx = x_mean.size(); // Dimensionality of inputs X
56  const double lambda = alpha * alpha * (Nx + K) - Nx;
57  const double c = Nx + lambda;
58 
59  // Generate weights:
60  const double Wi = 0.5 / c;
61  std::vector<double> W_mean(1 + 2 * Nx, Wi), W_cov(1 + 2 * Nx, Wi);
62  W_mean[0] = lambda / c;
63  W_cov[0] = W_mean[0] + (1 - alpha * alpha + beta);
64 
65  // Generate X_i samples:
66  MATLIKE1 L;
67  const bool valid = x_cov.chol(L);
68  if (!valid)
69  throw std::runtime_error(
70  "transform_gaussian_unscented: Singular covariance matrix in "
71  "Cholesky.");
72  L *= sqrt(c);
73 
74  // Propagate the samples X_i -> Y_i:
75  // We don't need to store the X sigma points: just use one vector to compute
76  // all the Y sigma points:
77  std::vector<VECTORLIKE3> Y(1 + 2 * Nx); // 2Nx+1 sigma points
78  VECTORLIKE1 X = x_mean;
79  functor(X, fixed_param, Y[0]);
80  VECTORLIKE1 delta; // i'th row of L:
81  delta.resize(Nx);
82  size_t row = 1;
83  for (size_t i = 0; i < Nx; i++)
84  {
85  for (size_t k = 0; k < Nx; k++) delta[k] = L(i, k);
86  X = x_mean;
87  X -= delta;
88  functor(X, fixed_param, Y[row++]);
89  X = x_mean;
90  X += delta;
91  functor(X, fixed_param, Y[row++]);
92  }
93 
94  // Estimate weighted cov & mean:
96  Y, y_cov, y_mean, &W_mean, &W_cov, elem_do_wrap2pi);
97  MRPT_END
98 }
99 
100 /** Simple Montecarlo-base estimation of the Gaussian distribution of a variable
101  * Y=f(X) for an arbitrary function f() provided by the user.
102  * The user must supply the function in "functor" which takes points in the X
103  * space and returns the mapped point in Y, optionally using an extra, constant
104  * parameter ("fixed_param") which remains constant.
105  * \param out_samples_y If !=nullptr, this vector will contain, upon return,
106  * the sequence of random samples generated and propagated through the
107  * functor().
108  * \sa The example in MRPT/samples/unscented_transform_test
109  * \sa transform_gaussian_unscented, transform_gaussian_linear
110  */
111 template <
112  class VECTORLIKE1, class MATLIKE1, class USERPARAM, class VECTORLIKE2,
113  class VECTORLIKE3, class MATLIKE2>
115  const VECTORLIKE1& x_mean, const MATLIKE1& x_cov,
116  void (*functor)(
117  const VECTORLIKE1& x, const USERPARAM& fixed_param, VECTORLIKE3& y),
118  const USERPARAM& fixed_param, VECTORLIKE2& y_mean, MATLIKE2& y_cov,
119  const size_t num_samples = 1000,
120  std::vector<VECTORLIKE3>* out_samples_y = nullptr)
121 {
122  MRPT_START
123  std::vector<VECTORLIKE1> samples_x;
125  samples_x, num_samples, x_cov, &x_mean);
126  std::vector<VECTORLIKE3> samples_y(num_samples);
127  for (size_t i = 0; i < num_samples; i++)
128  functor(samples_x[i], fixed_param, samples_y[i]);
129  mrpt::math::covariancesAndMean(samples_y, y_cov, y_mean);
130  if (out_samples_y)
131  {
132  out_samples_y->clear();
133  samples_y.swap(*out_samples_y);
134  }
135  MRPT_END
136 }
137 
138 /** First order uncertainty propagation estimator of the Gaussian distribution
139  * of a variable Y=f(X) for an arbitrary function f() provided by the user.
140  * The user must supply the function in "functor" which takes points in the X
141  * space and returns the mapped point in Y, optionally using an extra, constant
142  * parameter ("fixed_param") which remains constant.
143  * The Jacobians are estimated numerically using the vector of small
144  * increments "x_increments".
145  * \sa The example in MRPT/samples/unscented_transform_test
146  * \sa transform_gaussian_unscented, transform_gaussian_montecarlo
147  * \note This function requires `#include <Eigen/Dense>`
148  */
149 template <
150  class VECTORLIKE1, class MATLIKE1, class USERPARAM, class VECTORLIKE2,
151  class VECTORLIKE3, class MATLIKE2>
153  const VECTORLIKE1& x_mean, const MATLIKE1& x_cov,
154  void (*functor)(
155  const VECTORLIKE1& x, const USERPARAM& fixed_param, VECTORLIKE3& y),
156  const USERPARAM& fixed_param, VECTORLIKE2& y_mean, MATLIKE2& y_cov,
157  const VECTORLIKE1& x_increments)
158 {
159  MRPT_START
160  // Mean: simple propagation:
161  functor(x_mean, fixed_param, y_mean);
162  // Cov: COV = H C Ht
163  CMatrixFixed<
164  double, VECTORLIKE3::RowsAtCompileTime, VECTORLIKE1::RowsAtCompileTime>
165  H;
167  x_mean,
168  std::function<void(
169  const VECTORLIKE1& x, const USERPARAM& fixed_param,
170  VECTORLIKE3& y)>(functor),
171  x_increments, fixed_param, H);
172  mrpt::math::multiply_HCHt(H, x_cov, y_cov);
173  MRPT_END
174 }
175 
176 /** @} */
177 
178 } // namespace mrpt::math
A compile-time fixed-size numeric matrix container.
Definition: CMatrixFixed.h:33
#define MRPT_START
Definition: exceptions.h:241
This file implements miscelaneous matrix and matrix/vector operations, and internal functions in mrpt...
void transform_gaussian_montecarlo(const VECTORLIKE1 &x_mean, const MATLIKE1 &x_cov, void(*functor)(const VECTORLIKE1 &x, const USERPARAM &fixed_param, VECTORLIKE3 &y), const USERPARAM &fixed_param, VECTORLIKE2 &y_mean, MATLIKE2 &y_cov, const size_t num_samples=1000, std::vector< VECTORLIKE3 > *out_samples_y=nullptr)
Simple Montecarlo-base estimation of the Gaussian distribution of a variable Y=f(X) for an arbitrary ...
This base provides a set of functions for maths stuff.
void covariancesAndMean(const VECTOR_OF_VECTORS &elements, MATRIXLIKE &covariances, VECTORLIKE &means, const bool *elem_do_wrap2pi=nullptr)
Computes covariances and mean of any vector of containers.
Definition: data_utils.h:381
void drawGaussianMultivariateMany(VECTOR_OF_VECTORS &ret, size_t desiredSamples, const COVMATRIX &cov, const typename VECTOR_OF_VECTORS::value_type *mean=nullptr)
Generate a given number of multidimensional random samples according to a given covariance matrix...
void transform_gaussian_linear(const VECTORLIKE1 &x_mean, const MATLIKE1 &x_cov, void(*functor)(const VECTORLIKE1 &x, const USERPARAM &fixed_param, VECTORLIKE3 &y), const USERPARAM &fixed_param, VECTORLIKE2 &y_mean, MATLIKE2 &y_cov, const VECTORLIKE1 &x_increments)
First order uncertainty propagation estimator of the Gaussian distribution of a variable Y=f(X) for a...
void transform_gaussian_unscented(const VECTORLIKE1 &x_mean, const MATLIKE1 &x_cov, void(*functor)(const VECTORLIKE1 &x, const USERPARAM &fixed_param, VECTORLIKE3 &y), const USERPARAM &fixed_param, VECTORLIKE2 &y_mean, MATLIKE2 &y_cov, const bool *elem_do_wrap2pi=nullptr, const double alpha=1e-3, const double K=0, const double beta=2.0)
Scaled unscented transformation (SUT) for estimating the Gaussian distribution of a variable Y=f(X) f...
void estimateJacobian(const VECTORLIKE &x, const std::function< void(const VECTORLIKE &x, const USERPARAM &y, VECTORLIKE3 &out)> &functor, const VECTORLIKE2 &increments, const USERPARAM &userParam, MATRIXLIKE &out_Jacobian)
Estimate the Jacobian of a multi-dimensional function around a point "x", using finite differences of...
Definition: num_jacobian.h:31
void covariancesAndMeanWeighted(const VECTOR_OF_VECTORS &elements, MATRIXLIKE &covariances, VECTORLIKE &means, const VECTORLIKE2 *weights_mean, const VECTORLIKE3 *weights_cov, const bool *elem_do_wrap2pi=nullptr)
Computes covariances and mean of any vector of containers, given optional weights for the different s...
Definition: data_utils.h:261
#define MRPT_END
Definition: exceptions.h:245
CRandomGenerator & getRandomGenerator()
A static instance of a CRandomGenerator class, for use in single-thread applications.
void multiply_HCHt(const MAT_H &H, const MAT_C &C, MAT_R &R, bool accumResultInOutput=false)
R = H * C * H^t.
Definition: ops_matrices.h:28



Page generated by Doxygen 1.8.14 for MRPT 2.0.1 Git: 0fef1a6d7 Fri Apr 3 23:00:21 2020 +0200 at vie abr 3 23:20:28 CEST 2020