Main MRPT website > C++ reference
MRPT logo

transform_gaussian.h

Go to the documentation of this file.
00001 /* +---------------------------------------------------------------------------+
00002    |          The Mobile Robot Programming Toolkit (MRPT) C++ library          |
00003    |                                                                           |
00004    |                   http://mrpt.sourceforge.net/                            |
00005    |                                                                           |
00006    |   Copyright (C) 2005-2011  University of Malaga                           |
00007    |                                                                           |
00008    |    This software was written by the Machine Perception and Intelligent    |
00009    |      Robotics Lab, University of Malaga (Spain).                          |
00010    |    Contact: Jose-Luis Blanco  <jlblanco@ctima.uma.es>                     |
00011    |                                                                           |
00012    |  This file is part of the MRPT project.                                   |
00013    |                                                                           |
00014    |     MRPT is free software: you can redistribute it and/or modify          |
00015    |     it under the terms of the GNU General Public License as published by  |
00016    |     the Free Software Foundation, either version 3 of the License, or     |
00017    |     (at your option) any later version.                                   |
00018    |                                                                           |
00019    |   MRPT is distributed in the hope that it will be useful,                 |
00020    |     but WITHOUT ANY WARRANTY; without even the implied warranty of        |
00021    |     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         |
00022    |     GNU General Public License for more details.                          |
00023    |                                                                           |
00024    |     You should have received a copy of the GNU General Public License     |
00025    |     along with MRPT.  If not, see <http://www.gnu.org/licenses/>.         |
00026    |                                                                           |
00027    +---------------------------------------------------------------------------+ */
00028 #ifndef  transform_gaussian_H
00029 #define  transform_gaussian_H
00030 
00031 #include <mrpt/utils/utils_defs.h>
00032 #include <mrpt/math/math_frwds.h>
00033 #include <mrpt/math/CMatrixTemplateNumeric.h>
00034 #include <mrpt/math/CMatrixFixedNumeric.h>
00035 #include <mrpt/math/ops_matrices.h>
00036 #include <mrpt/math/jacobians.h>
00037 #include <mrpt/random.h>
00038 
00039 namespace mrpt
00040 {
00041         namespace math
00042         {
00043                 /** @name Gaussian PDF transformation functions
00044                     @{ */
00045 
00046                 /** Scaled unscented transformation (SUT) for estimating the Gaussian distribution of a variable Y=f(X) for an arbitrary function f() provided by the user.
00047                   *  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.
00048                   *
00049                   *  The parameters alpha, K and beta are the common names of the SUT method, and the default values are those recommended in most papers.
00050                   *
00051                   * \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.
00052                   * \sa The example in MRPT/samples/unscented_transform_test
00053                   * \sa transform_gaussian_montecarlo, transform_gaussian_linear
00054                   */
00055                 template <class VECTORLIKE1,class MATLIKE1, class USERPARAM,class VECTORLIKE2,class VECTORLIKE3,class MATLIKE2>
00056                 void transform_gaussian_unscented(
00057                         const VECTORLIKE1 &x_mean,
00058                         const MATLIKE1    &x_cov,
00059                         void  (*functor)(const VECTORLIKE1 &x,const USERPARAM &fixed_param, VECTORLIKE3 &y),
00060                         const USERPARAM &fixed_param,
00061                         VECTORLIKE2 &y_mean,
00062                         MATLIKE2    &y_cov,
00063                         const bool *elem_do_wrap2pi = NULL,
00064                         const double alpha = 1e-3,
00065                         const double K = 0,
00066                         const double beta = 2.0
00067                         )
00068                 {
00069                         MRPT_START
00070                         const size_t Nx = x_mean.size(); // Dimensionality of inputs X
00071                         const double lambda = alpha*alpha*(Nx+K)-Nx;
00072                         const double c = Nx+lambda;
00073 
00074                         // Generate weights:
00075                         const double Wi = 0.5/c;
00076                         vector_double  W_mean(1+2*Nx,Wi),W_cov(1+2*Nx,Wi);
00077                         W_mean[0] = lambda/c;
00078                         W_cov[0]  = W_mean[0]+(1-alpha*alpha+beta);
00079 
00080                         // Generate X_i samples:
00081                         MATLIKE1 L;
00082                         const bool valid = x_cov.chol(L);
00083                         if (!valid) throw std::runtime_error("transform_gaussian_unscented: Singular covariance matrix in Cholesky.");
00084                         L*= sqrt(c);
00085 
00086                         // Propagate the samples X_i -> Y_i:
00087                         // We don't need to store the X sigma points: just use one vector to compute all the Y sigma points:
00088                         typename mrpt::aligned_containers<VECTORLIKE3>::vector_t Y(1+2*Nx); // 2Nx+1 sigma points
00089                         VECTORLIKE1 X = x_mean;
00090                         functor(X,fixed_param,Y[0]);
00091                         VECTORLIKE1 delta; // i'th row of L:
00092                         delta.resize(Nx);
00093                         size_t row=1;
00094                         for (size_t i=0;i<Nx;i++)
00095                         {
00096                                 L.extractRowAsCol(i,delta);
00097                                 X=x_mean;X-=delta;
00098                                 functor(X,fixed_param,Y[row++]);
00099                                 X=x_mean;X+=delta;
00100                                 functor(X,fixed_param,Y[row++]);
00101                         }
00102 
00103                         // Estimate weighted cov & mean:
00104                         mrpt::math::covariancesAndMeanWeighted(Y, y_cov,y_mean,&W_mean,&W_cov,elem_do_wrap2pi);
00105                         MRPT_END
00106                 }
00107 
00108                 /** Simple Montecarlo-base estimation of the Gaussian distribution of a variable Y=f(X) for an arbitrary function f() provided by the user.
00109                   *  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.
00110                   * \param out_samples_y If !=NULL, this vector will contain, upon return, the sequence of random samples generated and propagated through the functor().
00111                   * \sa The example in MRPT/samples/unscented_transform_test
00112                   * \sa transform_gaussian_unscented, transform_gaussian_linear
00113                   */
00114                 template <class VECTORLIKE1,class MATLIKE1, class USERPARAM,class VECTORLIKE2,class VECTORLIKE3,class MATLIKE2>
00115                 void transform_gaussian_montecarlo(
00116                         const VECTORLIKE1 &x_mean,
00117                         const MATLIKE1    &x_cov,
00118                         void  (*functor)(const VECTORLIKE1 &x,const USERPARAM &fixed_param,VECTORLIKE3 &y),
00119                         const USERPARAM &fixed_param,
00120                         VECTORLIKE2 &y_mean,
00121                         MATLIKE2    &y_cov,
00122                         const size_t  num_samples = 1000,
00123                         typename mrpt::aligned_containers<VECTORLIKE3>::vector_t   *out_samples_y = NULL
00124                         )
00125                 {
00126                         MRPT_START
00127                         typename mrpt::aligned_containers<VECTORLIKE1>::vector_t samples_x;
00128                         mrpt::random::randomGenerator.drawGaussianMultivariateMany(samples_x,num_samples,x_cov,&x_mean);
00129                         typename mrpt::aligned_containers<VECTORLIKE3>::vector_t samples_y(num_samples);
00130                         for (size_t i=0;i<num_samples;i++)
00131                                 functor(samples_x[i],fixed_param,samples_y[i]);
00132                         mrpt::math::covariancesAndMean(samples_y,y_cov,y_mean);
00133                         if (out_samples_y) { out_samples_y->clear(); samples_y.swap(*out_samples_y); }
00134                         MRPT_END
00135                 }
00136 
00137                 /** First order uncertainty propagation estimator of the Gaussian distribution of a variable Y=f(X) for an arbitrary function f() provided by the user.
00138                   *  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.
00139                   *  The Jacobians are estimated numerically using the vector of small increments "x_increments".
00140                   * \sa The example in MRPT/samples/unscented_transform_test
00141                   * \sa transform_gaussian_unscented, transform_gaussian_montecarlo
00142                   */
00143                 template <class VECTORLIKE1,class MATLIKE1, class USERPARAM,class VECTORLIKE2,class VECTORLIKE3,class MATLIKE2>
00144                 void transform_gaussian_linear(
00145                         const VECTORLIKE1 &x_mean,
00146                         const MATLIKE1    &x_cov,
00147                         void  (*functor)(const VECTORLIKE1 &x,const USERPARAM &fixed_param,VECTORLIKE3 &y),
00148                         const USERPARAM &fixed_param,
00149                         VECTORLIKE2 &y_mean,
00150                         MATLIKE2    &y_cov,
00151                         const VECTORLIKE1 &x_increments
00152                         )
00153                 {
00154                         MRPT_START
00155                         // Mean: simple propagation:
00156                         functor(x_mean,fixed_param,y_mean);
00157                         // Cov: COV = H C Ht
00158                         Eigen::Matrix<double,VECTORLIKE3::RowsAtCompileTime,VECTORLIKE1::RowsAtCompileTime> H;
00159                         mrpt::math::jacobians::jacob_numeric_estimate(x_mean,functor,x_increments,fixed_param,H);
00160                         H.multiply_HCHt(x_cov, y_cov);
00161                         MRPT_END
00162                 }
00163 
00164                 /** @} */
00165 
00166         } // End of MATH namespace
00167 
00168 } // End of namespace
00169 
00170 
00171 #endif



Page generated by Doxygen 1.7.3 for MRPT 0.9.4 SVN: at Sat Mar 26 06:16:28 UTC 2011