Main MRPT website > C++ reference for 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 |
6  | See: http://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See details in http://www.mrpt.org/License |
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
22 {
23 namespace math
24 {
25 /** @addtogroup gausspdf_transform_grp Gaussian PDF transformation functions
26  * \ingroup mrpt_math_grp
27  * @{ */
28 
29 /** Scaled unscented transformation (SUT) for estimating the Gaussian
30  * distribution of a variable Y=f(X) for an arbitrary function f() provided by
31  * the user.
32  * The user must supply the function in "functor" which takes points in the X
33  * space and returns the mapped point in Y, optionally using an extra, constant
34  * parameter ("fixed_param") which remains constant.
35  *
36  * The parameters alpha, K and beta are the common names of the SUT method,
37  * and the default values are those recommended in most papers.
38  *
39  * \param elem_do_wrap2pi If !=nullptr; it must point to an array of "bool" of
40  * size()==dimension of each element, stating if it's needed to do a wrap to
41  * [-pi,pi] to each dimension.
42  * \sa The example in MRPT/samples/unscented_transform_test
43  * \sa transform_gaussian_montecarlo, transform_gaussian_linear
44  */
45 template <class VECTORLIKE1, class MATLIKE1, class USERPARAM, class VECTORLIKE2,
46  class VECTORLIKE3, class MATLIKE2>
48  const VECTORLIKE1& x_mean, const MATLIKE1& x_cov,
49  void (*functor)(
50  const VECTORLIKE1& x, const USERPARAM& fixed_param, VECTORLIKE3& y),
51  const USERPARAM& fixed_param, VECTORLIKE2& y_mean, MATLIKE2& y_cov,
52  const bool* elem_do_wrap2pi = nullptr, const double alpha = 1e-3,
53  const double K = 0, const double beta = 2.0)
54 {
56  const size_t Nx = x_mean.size(); // Dimensionality of inputs X
57  const double lambda = alpha * alpha * (Nx + K) - Nx;
58  const double c = Nx + lambda;
59 
60  // Generate weights:
61  const double Wi = 0.5 / c;
62  std::vector<double> W_mean(1 + 2 * Nx, Wi), W_cov(1 + 2 * Nx, Wi);
63  W_mean[0] = lambda / c;
64  W_cov[0] = W_mean[0] + (1 - alpha * alpha + beta);
65 
66  // Generate X_i samples:
67  MATLIKE1 L;
68  const bool valid = x_cov.chol(L);
69  if (!valid)
70  throw std::runtime_error(
71  "transform_gaussian_unscented: Singular covariance matrix in "
72  "Cholesky.");
73  L *= sqrt(c);
74 
75  // Propagate the samples X_i -> Y_i:
76  // We don't need to store the X sigma points: just use one vector to compute
77  // all the Y sigma points:
78  mrpt::aligned_std_vector<VECTORLIKE3> Y(1 + 2 * Nx); // 2Nx+1 sigma points
79  VECTORLIKE1 X = x_mean;
80  functor(X, fixed_param, Y[0]);
81  VECTORLIKE1 delta; // i'th row of L:
82  delta.resize(Nx);
83  size_t row = 1;
84  for (size_t i = 0; i < Nx; i++)
85  {
86  L.extractRowAsCol(i, delta);
87  X = x_mean;
88  X -= delta;
89  functor(X, fixed_param, Y[row++]);
90  X = x_mean;
91  X += delta;
92  functor(X, fixed_param, Y[row++]);
93  }
94 
95  // Estimate weighted cov & mean:
97  Y, y_cov, y_mean, &W_mean, &W_cov, elem_do_wrap2pi);
98  MRPT_END
99 }
100 
101 /** Simple Montecarlo-base estimation of the Gaussian distribution of a variable
102  * Y=f(X) for an arbitrary function f() provided by the user.
103  * The user must supply the function in "functor" which takes points in the X
104  * space and returns the mapped point in Y, optionally using an extra, constant
105  * parameter ("fixed_param") which remains constant.
106  * \param out_samples_y If !=nullptr, this vector will contain, upon return,
107  * the sequence of random samples generated and propagated through the
108  * functor().
109  * \sa The example in MRPT/samples/unscented_transform_test
110  * \sa transform_gaussian_unscented, transform_gaussian_linear
111  */
112 template <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  mrpt::aligned_std_vector<VECTORLIKE3>* out_samples_y = nullptr)
121 {
122  MRPT_START
125  samples_x, num_samples, x_cov, &x_mean);
126  mrpt::aligned_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  */
148 template <class VECTORLIKE1, class MATLIKE1, class USERPARAM, class VECTORLIKE2,
149  class VECTORLIKE3, class MATLIKE2>
151  const VECTORLIKE1& x_mean, const MATLIKE1& x_cov,
152  void (*functor)(
153  const VECTORLIKE1& x, const USERPARAM& fixed_param, VECTORLIKE3& y),
154  const USERPARAM& fixed_param, VECTORLIKE2& y_mean, MATLIKE2& y_cov,
155  const VECTORLIKE1& x_increments)
156 {
157  MRPT_START
158  // Mean: simple propagation:
159  functor(x_mean, fixed_param, y_mean);
160  // Cov: COV = H C Ht
161  Eigen::Matrix<double, VECTORLIKE3::RowsAtCompileTime,
162  VECTORLIKE1::RowsAtCompileTime>
163  H;
165  x_mean, std::function<void(
166  const VECTORLIKE1& x, const USERPARAM& fixed_param,
167  VECTORLIKE3& y)>(functor),
168  x_increments, fixed_param, H);
169  H.multiply_HCHt(x_cov, y_cov);
170  MRPT_END
171 }
172 
173 /** @} */
174 
175 } // End of MATH namespace
176 } // End of namespace
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:31
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...
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
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: ad3a9d8ae Tue May 1 23:10:22 2018 -0700 at lun oct 28 00:14:14 CET 2019