MRPT  1.9.9
transform_gaussian.h
Go to the documentation of this file.
1 /* +------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | http://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2018, Individual contributors, see AUTHORS file |
8  +------------------------------------------------------------------------+ */
9 #pragma once
10
11 #include <mrpt/math/math_frwds.h>
14 #include <mrpt/math/ops_matrices.h>
15 #include <mrpt/math/num_jacobian.h>
16 #include <mrpt/math/data_utils.h>
18 #include <mrpt/random.h>
19 #include <functional>
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 <class VECTORLIKE1, class MATLIKE1, class USERPARAM, class VECTORLIKE2,
44  class VECTORLIKE3, class MATLIKE2>
46  const VECTORLIKE1& x_mean, const MATLIKE1& x_cov,
47  void (*functor)(
48  const VECTORLIKE1& x, const USERPARAM& fixed_param, VECTORLIKE3& y),
49  const USERPARAM& fixed_param, VECTORLIKE2& y_mean, MATLIKE2& y_cov,
50  const bool* elem_do_wrap2pi = nullptr, const double alpha = 1e-3,
51  const double K = 0, const double beta = 2.0)
52 {
54  const size_t Nx = x_mean.size(); // Dimensionality of inputs X
55  const double lambda = alpha * alpha * (Nx + K) - Nx;
56  const double c = Nx + lambda;
57
58  // Generate weights:
59  const double Wi = 0.5 / c;
60  std::vector<double> W_mean(1 + 2 * Nx, Wi), W_cov(1 + 2 * Nx, Wi);
61  W_mean[0] = lambda / c;
62  W_cov[0] = W_mean[0] + (1 - alpha * alpha + beta);
63
64  // Generate X_i samples:
65  MATLIKE1 L;
66  const bool valid = x_cov.chol(L);
67  if (!valid)
68  throw std::runtime_error(
69  "transform_gaussian_unscented: Singular covariance matrix in "
70  "Cholesky.");
71  L *= sqrt(c);
72
73  // Propagate the samples X_i -> Y_i:
74  // We don't need to store the X sigma points: just use one vector to compute
75  // all the Y sigma points:
76  mrpt::aligned_std_vector<VECTORLIKE3> Y(1 + 2 * Nx); // 2Nx+1 sigma points
77  VECTORLIKE1 X = x_mean;
78  functor(X, fixed_param, Y[0]);
79  VECTORLIKE1 delta; // i'th row of L:
80  delta.resize(Nx);
81  size_t row = 1;
82  for (size_t i = 0; i < Nx; i++)
83  {
84  L.extractRowAsCol(i, delta);
85  X = x_mean;
86  X -= delta;
87  functor(X, fixed_param, Y[row++]);
88  X = x_mean;
89  X += delta;
90  functor(X, fixed_param, Y[row++]);
91  }
92
93  // Estimate weighted cov & mean:
95  Y, y_cov, y_mean, &W_mean, &W_cov, elem_do_wrap2pi);
96  MRPT_END
97 }
98
99 /** Simple Montecarlo-base estimation of the Gaussian distribution of a variable
100  * Y=f(X) for an arbitrary function f() provided by the user.
101  * The user must supply the function in "functor" which takes points in the X
102  * space and returns the mapped point in Y, optionally using an extra, constant
103  * parameter ("fixed_param") which remains constant.
104  * \param out_samples_y If !=nullptr, this vector will contain, upon return,
105  * the sequence of random samples generated and propagated through the
106  * functor().
107  * \sa The example in MRPT/samples/unscented_transform_test
108  * \sa transform_gaussian_unscented, transform_gaussian_linear
109  */
110 template <class VECTORLIKE1, class MATLIKE1, class USERPARAM, class VECTORLIKE2,
111  class VECTORLIKE3, class MATLIKE2>
113  const VECTORLIKE1& x_mean, const MATLIKE1& x_cov,
114  void (*functor)(
115  const VECTORLIKE1& x, const USERPARAM& fixed_param, VECTORLIKE3& y),
116  const USERPARAM& fixed_param, VECTORLIKE2& y_mean, MATLIKE2& y_cov,
117  const size_t num_samples = 1000,
118  mrpt::aligned_std_vector<VECTORLIKE3>* out_samples_y = nullptr)
119 {
120  MRPT_START
123  samples_x, num_samples, x_cov, &x_mean);
124  mrpt::aligned_std_vector<VECTORLIKE3> samples_y(num_samples);
125  for (size_t i = 0; i < num_samples; i++)
126  functor(samples_x[i], fixed_param, samples_y[i]);
127  mrpt::math::covariancesAndMean(samples_y, y_cov, y_mean);
128  if (out_samples_y)
129  {
130  out_samples_y->clear();
131  samples_y.swap(*out_samples_y);
132  }
133  MRPT_END
134 }
135
136 /** First order uncertainty propagation estimator of the Gaussian distribution
137  * of a variable Y=f(X) for an arbitrary function f() provided by the user.
138  * The user must supply the function in "functor" which takes points in the X
139  * space and returns the mapped point in Y, optionally using an extra, constant
140  * parameter ("fixed_param") which remains constant.
141  * The Jacobians are estimated numerically using the vector of small
142  * increments "x_increments".
143  * \sa The example in MRPT/samples/unscented_transform_test
144  * \sa transform_gaussian_unscented, transform_gaussian_montecarlo
145  */
146 template <class VECTORLIKE1, class MATLIKE1, class USERPARAM, class VECTORLIKE2,
147  class VECTORLIKE3, class MATLIKE2>
149  const VECTORLIKE1& x_mean, const MATLIKE1& x_cov,
150  void (*functor)(
151  const VECTORLIKE1& x, const USERPARAM& fixed_param, VECTORLIKE3& y),
152  const USERPARAM& fixed_param, VECTORLIKE2& y_mean, MATLIKE2& y_cov,
153  const VECTORLIKE1& x_increments)
154 {
155  MRPT_START
156  // Mean: simple propagation:
157  functor(x_mean, fixed_param, y_mean);
158  // Cov: COV = H C Ht
159  Eigen::Matrix<double, VECTORLIKE3::RowsAtCompileTime,
160  VECTORLIKE1::RowsAtCompileTime>
161  H;
163  x_mean, std::function<void(
164  const VECTORLIKE1& x, const USERPARAM& fixed_param,
165  VECTORLIKE3& y)>(functor),
166  x_increments, fixed_param, H);
167  H.multiply_HCHt(x_cov, y_cov);
168  MRPT_END
169 }
170
171 /** @} */
172
173 }
174
GLclampf GLclampf GLclampf alpha
Definition: glext.h:3525
#define MRPT_START
Definition: exceptions.h:262
This file implements miscelaneous matrix and matrix/vector operations, and internal functions in mrpt...
std::vector< T, mrpt::aligned_allocator_cpp11< T > > aligned_std_vector
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:29
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:384
const GLubyte * c
Definition: glext.h:6313
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 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
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, mrpt::aligned_std_vector< VECTORLIKE3 > *out_samples_y=nullptr)
Simple Montecarlo-base estimation of the Gaussian distribution of a variable Y=f(X) for an arbitrary ...
GLenum GLenum GLvoid * row
Definition: glext.h:3576
#define MRPT_END
Definition: exceptions.h:266
GLenum GLint GLint y
Definition: glext.h:3538
GLenum GLint x
Definition: glext.h:3538
CRandomGenerator & getRandomGenerator()
A static instance of a CRandomGenerator class, for use in single-thread applications.

 Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: 7d5e6d718 Fri Aug 24 01:51:28 2018 +0200 at lun nov 2 08:35:50 CET 2020