Main MRPT website > C++ reference for MRPT 1.5.7
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-2017, 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 
12 #include <mrpt/utils/utils_defs.h>
13 #include <mrpt/math/math_frwds.h>
16 #include <mrpt/math/ops_matrices.h>
17 #include <mrpt/math/jacobians.h>
18 #include <mrpt/math/data_utils.h>
19 #include <mrpt/random.h>
20 
21 namespace 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);
88  MRPT_END
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  {
109  MRPT_START
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); }
117  MRPT_END
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  {
137  MRPT_START
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);
144  MRPT_END
145  }
146 
147  /** @} */
148 
149  } // End of MATH namespace
150 
151 } // End of namespace
152 
153 
154 #endif
GLclampf GLclampf GLclampf alpha
Definition: glext.h:3510
BASE_IMPEXP CRandomGenerator randomGenerator
A static instance of a CRandomGenerator class, for use in single-thread applications.
This file implements miscelaneous matrix and matrix/vector operations, and internal functions in mrpt...
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...
#define MRPT_END
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 ...
const GLubyte * c
Definition: glext.h:5590
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...
#define MRPT_START
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
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...
GLenum GLenum GLvoid * row
Definition: glext.h:3533
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:251
GLenum GLint GLint y
Definition: glext.h:3516
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
GLenum GLint x
Definition: glext.h:3516
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:354
std::vector< TYPE1, Eigen::aligned_allocator< TYPE1 > > vector_t



Page generated by Doxygen 1.8.14 for MRPT 1.5.7 Git: 5902e14cc Wed Apr 24 15:04:01 2019 +0200 at lun oct 28 01:39:17 CET 2019