transform_gaussian.h

Go to the documentation of this file.

/* +---------------------------------------------------------------------------+
   |          The Mobile Robot Programming Toolkit (MRPT) C++ library          |
   |                                                                           |
   |                       http://www.mrpt.org/                                |
   |                                                                           |
   |   Copyright (C) 2005-2011  University of Malaga                           |
   |                                                                           |
   |    This software was written by the Machine Perception and Intelligent    |
   |      Robotics Lab, University of Malaga (Spain).                          |
   |    Contact: Jose-Luis Blanco  <jlblanco@ctima.uma.es>                     |
   |                                                                           |
   |  This file is part of the MRPT project.                                   |
   |                                                                           |
   |     MRPT is free software: you can redistribute it and/or modify          |
   |     it under the terms of the GNU General Public License as published by  |
   |     the Free Software Foundation, either version 3 of the License, or     |
   |     (at your option) any later version.                                   |
   |                                                                           |
   |   MRPT is distributed in the hope that it will be useful,                 |
   |     but WITHOUT ANY WARRANTY; without even the implied warranty of        |
   |     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         |
   |     GNU General Public License for more details.                          |
   |                                                                           |
   |     You should have received a copy of the GNU General Public License     |
   |     along with MRPT.  If not, see <http://www.gnu.org/licenses/>.         |
   |                                                                           |
   +---------------------------------------------------------------------------+ */
#ifndef  transform_gaussian_H
#define  transform_gaussian_H

#include <mrpt/utils/utils_defs.h>
#include <mrpt/math/math_frwds.h>
#include <mrpt/math/CMatrixTemplateNumeric.h>
#include <mrpt/math/CMatrixFixedNumeric.h>
#include <mrpt/math/ops_matrices.h>
#include <mrpt/math/jacobians.h>
#include <mrpt/random.h>

namespace mrpt
{
        namespace math
        {
                /** @addtogroup  gausspdf_transform_grp Gaussian PDF transformation functions
                  *  \ingroup mrpt_base_grp
                  * @{ */

                /** Scaled unscented transformation (SUT) for estimating the Gaussian distribution of a variable Y=f(X) for an arbitrary function f() provided by the user.
                  *  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.
                  *
                  *  The parameters alpha, K and beta are the common names of the SUT method, and the default values are those recommended in most papers.
                  *
                  * \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.
                  * \sa The example in MRPT/samples/unscented_transform_test
                  * \sa transform_gaussian_montecarlo, transform_gaussian_linear
                  */
                template <class VECTORLIKE1,class MATLIKE1, class USERPARAM,class VECTORLIKE2,class VECTORLIKE3,class MATLIKE2>
                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
                        )
                {
                        MRPT_START
                        const size_t Nx = x_mean.size(); // Dimensionality of inputs X
                        const double lambda = alpha*alpha*(Nx+K)-Nx;
                        const double c = Nx+lambda;

                        // Generate weights:
                        const double Wi = 0.5/c;
                        vector_double  W_mean(1+2*Nx,Wi),W_cov(1+2*Nx,Wi);
                        W_mean[0] = lambda/c;
                        W_cov[0]  = W_mean[0]+(1-alpha*alpha+beta);

                        // Generate X_i samples:
                        MATLIKE1 L;
                        const bool valid = x_cov.chol(L);
                        if (!valid) throw std::runtime_error("transform_gaussian_unscented: Singular covariance matrix in Cholesky.");
                        L*= sqrt(c);

                        // Propagate the samples X_i -> Y_i:
                        // We don't need to store the X sigma points: just use one vector to compute all the Y sigma points:
                        typename mrpt::aligned_containers<VECTORLIKE3>::vector_t Y(1+2*Nx); // 2Nx+1 sigma points
                        VECTORLIKE1 X = x_mean;
                        functor(X,fixed_param,Y[0]);
                        VECTORLIKE1 delta; // i'th row of L:
                        delta.resize(Nx);
                        size_t row=1;
                        for (size_t i=0;i<Nx;i++)
                        {
                                L.extractRowAsCol(i,delta);
                                X=x_mean;X-=delta;
                                functor(X,fixed_param,Y[row++]);
                                X=x_mean;X+=delta;
                                functor(X,fixed_param,Y[row++]);
                        }

                        // Estimate weighted cov & mean:
                        mrpt::math::covariancesAndMeanWeighted(Y, y_cov,y_mean,&W_mean,&W_cov,elem_do_wrap2pi);
                        MRPT_END
                }

                /** Simple Montecarlo-base estimation of the Gaussian distribution of a variable Y=f(X) for an arbitrary function f() provided by the user.
                  *  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.
                  * \param out_samples_y If !=NULL, this vector will contain, upon return, the sequence of random samples generated and propagated through the functor().
                  * \sa The example in MRPT/samples/unscented_transform_test
                  * \sa transform_gaussian_unscented, transform_gaussian_linear
                  */
                template <class VECTORLIKE1,class MATLIKE1, class USERPARAM,class VECTORLIKE2,class VECTORLIKE3,class MATLIKE2>
                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
                        )
                {
                        MRPT_START
                        typename mrpt::aligned_containers<VECTORLIKE1>::vector_t samples_x;
                        mrpt::random::randomGenerator.drawGaussianMultivariateMany(samples_x,num_samples,x_cov,&x_mean);
                        typename mrpt::aligned_containers<VECTORLIKE3>::vector_t samples_y(num_samples);
                        for (size_t i=0;i<num_samples;i++)
                                functor(samples_x[i],fixed_param,samples_y[i]);
                        mrpt::math::covariancesAndMean(samples_y,y_cov,y_mean);
                        if (out_samples_y) { out_samples_y->clear(); samples_y.swap(*out_samples_y); }
                        MRPT_END
                }

                /** First order uncertainty propagation estimator of the Gaussian distribution of a variable Y=f(X) for an arbitrary function f() provided by the user.
                  *  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.
                  *  The Jacobians are estimated numerically using the vector of small increments "x_increments".
                  * \sa The example in MRPT/samples/unscented_transform_test
                  * \sa transform_gaussian_unscented, transform_gaussian_montecarlo
                  */
                template <class VECTORLIKE1,class MATLIKE1, class USERPARAM,class VECTORLIKE2,class VECTORLIKE3,class MATLIKE2>
                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
                        )
                {
                        MRPT_START
                        // Mean: simple propagation:
                        functor(x_mean,fixed_param,y_mean);
                        // Cov: COV = H C Ht
                        Eigen::Matrix<double,VECTORLIKE3::RowsAtCompileTime,VECTORLIKE1::RowsAtCompileTime> H;
                        mrpt::math::jacobians::jacob_numeric_estimate(x_mean,functor,x_increments,fixed_param,H);
                        H.multiply_HCHt(x_cov, y_cov);
                        MRPT_END
                }

                /** @} */

        } // End of MATH namespace

} // End of namespace


#endif