Main MRPT website > C++ reference
MRPT logo
transform_gaussian.h
Go to the documentation of this file.
1 /* +---------------------------------------------------------------------------+
2  | The Mobile Robot Programming Toolkit (MRPT) C++ library |
3  | |
4  | http://www.mrpt.org/ |
5  | |
6  | Copyright (C) 2005-2012 University of Malaga |
7  | |
8  | This software was written by the Machine Perception and Intelligent |
9  | Robotics Lab, University of Malaga (Spain). |
10  | Contact: Jose-Luis Blanco <jlblanco@ctima.uma.es> |
11  | |
12  | This file is part of the MRPT project. |
13  | |
14  | MRPT is free software: you can redistribute it and/or modify |
15  | it under the terms of the GNU General Public License as published by |
16  | the Free Software Foundation, either version 3 of the License, or |
17  | (at your option) any later version. |
18  | |
19  | MRPT is distributed in the hope that it will be useful, |
20  | but WITHOUT ANY WARRANTY; without even the implied warranty of |
21  | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the |
22  | GNU General Public License for more details. |
23  | |
24  | You should have received a copy of the GNU General Public License |
25  | along with MRPT. If not, see <http://www.gnu.org/licenses/>. |
26  | |
27  +---------------------------------------------------------------------------+ */
28 #ifndef transform_gaussian_H
29 #define transform_gaussian_H
30 
31 #include <mrpt/utils/utils_defs.h>
32 #include <mrpt/math/math_frwds.h>
33 #include <mrpt/math/CMatrixTemplateNumeric.h>
34 #include <mrpt/math/CMatrixFixedNumeric.h>
35 #include <mrpt/math/ops_matrices.h>
36 #include <mrpt/math/jacobians.h>
37 #include <mrpt/random.h>
38 
39 namespace mrpt
40 {
41  namespace math
42  {
43  /** @addtogroup gausspdf_transform_grp Gaussian PDF transformation functions
44  * \ingroup mrpt_base_grp
45  * @{ */
46 
47  /** Scaled unscented transformation (SUT) for estimating the Gaussian distribution of a variable Y=f(X) for an arbitrary function f() provided by the user.
48  * 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.
49  *
50  * The parameters alpha, K and beta are the common names of the SUT method, and the default values are those recommended in most papers.
51  *
52  * \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.
53  * \sa The example in MRPT/samples/unscented_transform_test
54  * \sa transform_gaussian_montecarlo, transform_gaussian_linear
55  */
56  template <class VECTORLIKE1,class MATLIKE1, class USERPARAM,class VECTORLIKE2,class VECTORLIKE3,class MATLIKE2>
57  void transform_gaussian_unscented(
58  const VECTORLIKE1 &x_mean,
59  const MATLIKE1 &x_cov,
60  void (*functor)(const VECTORLIKE1 &x,const USERPARAM &fixed_param, VECTORLIKE3 &y),
61  const USERPARAM &fixed_param,
62  VECTORLIKE2 &y_mean,
63  MATLIKE2 &y_cov,
64  const bool *elem_do_wrap2pi = NULL,
65  const double alpha = 1e-3,
66  const double K = 0,
67  const double beta = 2.0
68  )
69  {
70  MRPT_START
71  const size_t Nx = x_mean.size(); // Dimensionality of inputs X
72  const double lambda = alpha*alpha*(Nx+K)-Nx;
73  const double c = Nx+lambda;
74 
75  // Generate weights:
76  const double Wi = 0.5/c;
77  vector_double W_mean(1+2*Nx,Wi),W_cov(1+2*Nx,Wi);
78  W_mean[0] = lambda/c;
79  W_cov[0] = W_mean[0]+(1-alpha*alpha+beta);
80 
81  // Generate X_i samples:
82  MATLIKE1 L;
83  const bool valid = x_cov.chol(L);
84  if (!valid) throw std::runtime_error("transform_gaussian_unscented: Singular covariance matrix in Cholesky.");
85  L*= sqrt(c);
86 
87  // Propagate the samples X_i -> Y_i:
88  // We don't need to store the X sigma points: just use one vector to compute all the Y sigma points:
89  typename mrpt::aligned_containers<VECTORLIKE3>::vector_t Y(1+2*Nx); // 2Nx+1 sigma points
90  VECTORLIKE1 X = x_mean;
91  functor(X,fixed_param,Y[0]);
92  VECTORLIKE1 delta; // i'th row of L:
93  delta.resize(Nx);
94  size_t row=1;
95  for (size_t i=0;i<Nx;i++)
96  {
97  L.extractRowAsCol(i,delta);
98  X=x_mean;X-=delta;
99  functor(X,fixed_param,Y[row++]);
100  X=x_mean;X+=delta;
101  functor(X,fixed_param,Y[row++]);
102  }
103 
104  // Estimate weighted cov & mean:
105  mrpt::math::covariancesAndMeanWeighted(Y, y_cov,y_mean,&W_mean,&W_cov,elem_do_wrap2pi);
106  MRPT_END
107  }
108 
109  /** Simple Montecarlo-base estimation of the Gaussian distribution of a variable Y=f(X) for an arbitrary function f() provided by the user.
110  * 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.
111  * \param out_samples_y If !=NULL, this vector will contain, upon return, the sequence of random samples generated and propagated through the functor().
112  * \sa The example in MRPT/samples/unscented_transform_test
113  * \sa transform_gaussian_unscented, transform_gaussian_linear
114  */
115  template <class VECTORLIKE1,class MATLIKE1, class USERPARAM,class VECTORLIKE2,class VECTORLIKE3,class MATLIKE2>
116  void transform_gaussian_montecarlo(
117  const VECTORLIKE1 &x_mean,
118  const MATLIKE1 &x_cov,
119  void (*functor)(const VECTORLIKE1 &x,const USERPARAM &fixed_param,VECTORLIKE3 &y),
120  const USERPARAM &fixed_param,
121  VECTORLIKE2 &y_mean,
122  MATLIKE2 &y_cov,
123  const size_t num_samples = 1000,
124  typename mrpt::aligned_containers<VECTORLIKE3>::vector_t *out_samples_y = NULL
125  )
126  {
127  MRPT_START
128  typename mrpt::aligned_containers<VECTORLIKE1>::vector_t samples_x;
129  mrpt::random::randomGenerator.drawGaussianMultivariateMany(samples_x,num_samples,x_cov,&x_mean);
130  typename mrpt::aligned_containers<VECTORLIKE3>::vector_t samples_y(num_samples);
131  for (size_t i=0;i<num_samples;i++)
132  functor(samples_x[i],fixed_param,samples_y[i]);
133  mrpt::math::covariancesAndMean(samples_y,y_cov,y_mean);
134  if (out_samples_y) { out_samples_y->clear(); samples_y.swap(*out_samples_y); }
135  MRPT_END
136  }
137 
138  /** First order uncertainty propagation estimator of the Gaussian distribution of a variable Y=f(X) for an arbitrary function f() provided by the user.
139  * 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.
140  * The Jacobians are estimated numerically using the vector of small increments "x_increments".
141  * \sa The example in MRPT/samples/unscented_transform_test
142  * \sa transform_gaussian_unscented, transform_gaussian_montecarlo
143  */
144  template <class VECTORLIKE1,class MATLIKE1, class USERPARAM,class VECTORLIKE2,class VECTORLIKE3,class MATLIKE2>
145  void transform_gaussian_linear(
146  const VECTORLIKE1 &x_mean,
147  const MATLIKE1 &x_cov,
148  void (*functor)(const VECTORLIKE1 &x,const USERPARAM &fixed_param,VECTORLIKE3 &y),
149  const USERPARAM &fixed_param,
150  VECTORLIKE2 &y_mean,
151  MATLIKE2 &y_cov,
152  const VECTORLIKE1 &x_increments
153  )
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,VECTORLIKE1::RowsAtCompileTime> H;
160  mrpt::math::jacobians::jacob_numeric_estimate(x_mean,functor,x_increments,fixed_param,H);
161  H.multiply_HCHt(x_cov, y_cov);
162  MRPT_END
163  }
164 
165  /** @} */
166 
167  } // End of MATH namespace
168 
169 } // End of namespace
170 
171 
172 #endif


Page generated by Doxygen 1.8.3 for MRPT 0.9.6 SVN: at Fri Feb 15 22:05:02 EST 2013