Main MRPT website > C++ reference for MRPT 1.4.0
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-2016, 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#ifndef transform_gaussian_H
10#define transform_gaussian_H
11
17#include <mrpt/math/jacobians.h>
19#include <mrpt/random.h>
20
21namespace mrpt
22{
23 namespace math
24 {
25 /** @addtogroup gausspdf_transform_grp Gaussian PDF transformation functions
26 * \ingroup mrpt_base_grp
27 * @{ */
28
29 /** Scaled unscented transformation (SUT) for estimating the Gaussian distribution of a variable Y=f(X) for an arbitrary function f() provided by the user.
30 * The user must supply the function in "functor" which takes points in the X space and returns the mapped point in Y, optionally using an extra, constant parameter ("fixed_param") which remains constant.
31 *
32 * The parameters alpha, K and beta are the common names of the SUT method, and the default values are those recommended in most papers.
33 *
34 * \param elem_do_wrap2pi If !=NULL; it must point to an array of "bool" of size()==dimension of each element, stating if it's needed to do a wrap to [-pi,pi] to each dimension.
35 * \sa The example in MRPT/samples/unscented_transform_test
36 * \sa transform_gaussian_montecarlo, transform_gaussian_linear
37 */
38 template <class VECTORLIKE1,class MATLIKE1, class USERPARAM,class VECTORLIKE2,class VECTORLIKE3,class MATLIKE2>
40 const VECTORLIKE1 &x_mean,
41 const MATLIKE1 &x_cov,
42 void (*functor)(const VECTORLIKE1 &x,const USERPARAM &fixed_param, VECTORLIKE3 &y),
43 const USERPARAM &fixed_param,
44 VECTORLIKE2 &y_mean,
45 MATLIKE2 &y_cov,
46 const bool *elem_do_wrap2pi = NULL,
47 const double alpha = 1e-3,
48 const double K = 0,
49 const double beta = 2.0
50 )
51 {
53 const size_t Nx = x_mean.size(); // Dimensionality of inputs X
54 const double lambda = alpha*alpha*(Nx+K)-Nx;
55 const double c = Nx+lambda;
56
57 // Generate weights:
58 const double Wi = 0.5/c;
59 std::vector<double> W_mean(1+2*Nx,Wi),W_cov(1+2*Nx,Wi);
60 W_mean[0] = lambda/c;
61 W_cov[0] = W_mean[0]+(1-alpha*alpha+beta);
62
63 // Generate X_i samples:
64 MATLIKE1 L;
65 const bool valid = x_cov.chol(L);
66 if (!valid) throw std::runtime_error("transform_gaussian_unscented: Singular covariance matrix in Cholesky.");
67 L*= sqrt(c);
68
69 // Propagate the samples X_i -> Y_i:
70 // We don't need to store the X sigma points: just use one vector to compute all the Y sigma points:
71 typename mrpt::aligned_containers<VECTORLIKE3>::vector_t Y(1+2*Nx); // 2Nx+1 sigma points
72 VECTORLIKE1 X = x_mean;
73 functor(X,fixed_param,Y[0]);
74 VECTORLIKE1 delta; // i'th row of L:
75 delta.resize(Nx);
76 size_t row=1;
77 for (size_t i=0;i<Nx;i++)
78 {
79 L.extractRowAsCol(i,delta);
80 X=x_mean;X-=delta;
81 functor(X,fixed_param,Y[row++]);
82 X=x_mean;X+=delta;
83 functor(X,fixed_param,Y[row++]);
84 }
85
86 // Estimate weighted cov & mean:
87 mrpt::math::covariancesAndMeanWeighted(Y, y_cov,y_mean,&W_mean,&W_cov,elem_do_wrap2pi);
89 }
90
91 /** Simple Montecarlo-base estimation of the Gaussian distribution of a variable Y=f(X) for an arbitrary function f() provided by the user.
92 * The user must supply the function in "functor" which takes points in the X space and returns the mapped point in Y, optionally using an extra, constant parameter ("fixed_param") which remains constant.
93 * \param out_samples_y If !=NULL, this vector will contain, upon return, the sequence of random samples generated and propagated through the functor().
94 * \sa The example in MRPT/samples/unscented_transform_test
95 * \sa transform_gaussian_unscented, transform_gaussian_linear
96 */
97 template <class VECTORLIKE1,class MATLIKE1, class USERPARAM,class VECTORLIKE2,class VECTORLIKE3,class MATLIKE2>
99 const VECTORLIKE1 &x_mean,
100 const MATLIKE1 &x_cov,
101 void (*functor)(const VECTORLIKE1 &x,const USERPARAM &fixed_param,VECTORLIKE3 &y),
102 const USERPARAM &fixed_param,
103 VECTORLIKE2 &y_mean,
104 MATLIKE2 &y_cov,
105 const size_t num_samples = 1000,
106 typename mrpt::aligned_containers<VECTORLIKE3>::vector_t *out_samples_y = NULL
107 )
108 {
111 mrpt::random::randomGenerator.drawGaussianMultivariateMany(samples_x,num_samples,x_cov,&x_mean);
112 typename mrpt::aligned_containers<VECTORLIKE3>::vector_t samples_y(num_samples);
113 for (size_t i=0;i<num_samples;i++)
114 functor(samples_x[i],fixed_param,samples_y[i]);
115 mrpt::math::covariancesAndMean(samples_y,y_cov,y_mean);
116 if (out_samples_y) { out_samples_y->clear(); samples_y.swap(*out_samples_y); }
118 }
119
120 /** First order uncertainty propagation estimator of the Gaussian distribution of a variable Y=f(X) for an arbitrary function f() provided by the user.
121 * The user must supply the function in "functor" which takes points in the X space and returns the mapped point in Y, optionally using an extra, constant parameter ("fixed_param") which remains constant.
122 * The Jacobians are estimated numerically using the vector of small increments "x_increments".
123 * \sa The example in MRPT/samples/unscented_transform_test
124 * \sa transform_gaussian_unscented, transform_gaussian_montecarlo
125 */
126 template <class VECTORLIKE1,class MATLIKE1, class USERPARAM,class VECTORLIKE2,class VECTORLIKE3,class MATLIKE2>
128 const VECTORLIKE1 &x_mean,
129 const MATLIKE1 &x_cov,
130 void (*functor)(const VECTORLIKE1 &x,const USERPARAM &fixed_param,VECTORLIKE3 &y),
131 const USERPARAM &fixed_param,
132 VECTORLIKE2 &y_mean,
133 MATLIKE2 &y_cov,
134 const VECTORLIKE1 &x_increments
135 )
136 {
138 // Mean: simple propagation:
139 functor(x_mean,fixed_param,y_mean);
140 // Cov: COV = H C Ht
141 Eigen::Matrix<double,VECTORLIKE3::RowsAtCompileTime,VECTORLIKE1::RowsAtCompileTime> H;
142 mrpt::math::jacobians::jacob_numeric_estimate(x_mean,functor,x_increments,fixed_param,H);
143 H.multiply_HCHt(x_cov, y_cov);
145 }
146
147 /** @} */
148
149 } // End of MATH namespace
150
151} // End of namespace
152
153
154#endif
void drawGaussianMultivariateMany(VECTOR_OF_VECTORS &ret, size_t desiredSamples, const COVMATRIX &cov, const typename VECTOR_OF_VECTORS::value_type *mean=NULL)
Generate a given number of multidimensional random samples according to a given covariance matrix.
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, typename mrpt::aligned_containers< VECTORLIKE3 >::vector_t *out_samples_y=NULL)
Simple Montecarlo-base estimation of the Gaussian distribution of a variable Y=f(X) for an arbitrary ...
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=NULL, 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 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 covariancesAndMean(const VECTOR_OF_VECTORS &elements, MATRIXLIKE &covariances, VECTORLIKE &means, const bool *elem_do_wrap2pi=NULL)
Computes covariances and mean of any vector of containers.
Definition: data_utils.h:355
void covariancesAndMeanWeighted(const VECTOR_OF_VECTORS &elements, MATRIXLIKE &covariances, VECTORLIKE &means, const VECTORLIKE2 *weights_mean, const VECTORLIKE3 *weights_cov, const bool *elem_do_wrap2pi=NULL)
Computes covariances and mean of any vector of containers, given optional weights for the different s...
Definition: data_utils.h:252
#define MRPT_START
Definition: mrpt_macros.h:349
#define MRPT_END
Definition: mrpt_macros.h:353
void jacob_numeric_estimate(const VECTORLIKE &x, void(*functor)(const VECTORLIKE &x, const USERPARAM &y, VECTORLIKE3 &out), const VECTORLIKE2 &increments, const USERPARAM &userParam, MATRIXLIKE &out_Jacobian)
Numerical estimation of the Jacobian of a user-supplied function - this template redirects to mrpt::m...
Definition: jacobians.h:119
BASE_IMPEXP CRandomGenerator randomGenerator
A static instance of a CRandomGenerator class, for use in single-thread applications.
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
This file implements miscelaneous matrix and matrix/vector operations, and internal functions in mrpt...
std::vector< TYPE1, Eigen::aligned_allocator< TYPE1 > > vector_t



Page generated by Doxygen 1.9.5 for MRPT 1.4.0 SVN: at Tue Dec 27 00:53:09 UTC 2022