CProbabilityDensityFunction.h

Go to the documentation of this file.

/* +---------------------------------------------------------------------------+
   |                     Mobile Robot Programming Toolkit (MRPT)               |
   |                          http://www.mrpt.org/                             |
   |                                                                           |
   | Copyright (c) 2005-2017, Individual contributors, see AUTHORS file        |
   | See: http://www.mrpt.org/Authors - All rights reserved.                   |
   | Released under BSD License. See details in http://www.mrpt.org/License    |
   +---------------------------------------------------------------------------+ */
#ifndef CProbabilityDensityFunction_H
#define CProbabilityDensityFunction_H

#include <mrpt/math/CMatrixTemplateNumeric.h>
#include <mrpt/math/CMatrixFixedNumeric.h>
#include <mrpt/math/math_frwds.h>

namespace mrpt
{
        namespace utils
        {
                /** A generic template for probability density distributions (PDFs).
                 * This template is used as base for many classes in mrpt::poses
                 *  Any derived class must implement \a getMean() and a getCovarianceAndMean().
                 *  Other methods such as \a getMean() or \a getCovariance() are implemented here for convenience.
                 * \sa mprt::poses::CPosePDF, mprt::poses::CPose3DPDF, mprt::poses::CPointPDF
                 * \ingroup mrpt_base_grp
                 */
                template <class TDATA, size_t STATE_LEN>
                class CProbabilityDensityFunction
                {
                public:
                        static const size_t state_length = STATE_LEN;   //!< The length of the variable, for example, 3 for a 3D point, 6 for a 3D pose (x y z yaw pitch roll).
                        typedef TDATA type_value;  //!< The type of the state the PDF represents
                        typedef CProbabilityDensityFunction<TDATA, STATE_LEN> self_t;

                         /** Returns the mean, or mathematical expectation of the probability density distribution (PDF).
                           * \sa getCovarianceAndMean, getInformationMatrix
                           */
                        virtual void getMean(TDATA &mean_point) const = 0;

                        /** Returns an estimate of the pose covariance matrix (STATE_LENxSTATE_LEN cov matrix) and the mean, both at once.
                          * \sa getMean, getInformationMatrix
                          */
                        virtual void getCovarianceAndMean(mrpt::math::CMatrixFixedNumeric<double,STATE_LEN,STATE_LEN> &cov,TDATA  &mean_point) const = 0;

                        /** Returns an estimate of the pose covariance matrix (STATE_LENxSTATE_LEN cov matrix) and the mean, both at once.
                          * \sa getMean, getInformationMatrix
                          */
                        inline void getCovarianceDynAndMean(mrpt::math::CMatrixDouble &cov,TDATA  &mean_point) const
                        {
                                mrpt::math::CMatrixFixedNumeric<double,STATE_LEN,STATE_LEN> C(mrpt::math::UNINITIALIZED_MATRIX);
                                this->getCovarianceAndMean(C,mean_point);
                                cov = C; // Convert to dynamic size matrix
                        }

                        /** Returns the mean, or mathematical expectation of the probability density distribution (PDF).
                           * \sa getCovariance, getInformationMatrix
                           */
                        inline TDATA getMeanVal() const
                        {
                                TDATA p;
                                getMean(p);
                                return p;
                        }

                        /** Returns the estimate of the covariance matrix (STATE_LEN x STATE_LEN covariance matrix)
                          * \sa getMean, getCovarianceAndMean, getInformationMatrix
                          */
                        inline void getCovariance(mrpt::math::CMatrixDouble &cov) const
                        {
                                TDATA p;
                                this->getCovarianceDynAndMean(cov,p);
                        }

                        /** Returns the estimate of the covariance matrix (STATE_LEN x STATE_LEN covariance matrix)
                          * \sa getMean, getCovarianceAndMean, getInformationMatrix
                          */
                        inline void getCovariance(mrpt::math::CMatrixFixedNumeric<double,STATE_LEN,STATE_LEN> &cov) const
                        {
                                TDATA p;
                                this->getCovarianceAndMean(cov,p);
                        }

                        /** Returns the estimate of the covariance matrix (STATE_LEN x STATE_LEN covariance matrix)
                          * \sa getMean, getInformationMatrix
                          */
                        inline mrpt::math::CMatrixFixedNumeric<double,STATE_LEN,STATE_LEN> getCovariance() const
                        {
                                mrpt::math::CMatrixFixedNumeric<double,STATE_LEN,STATE_LEN> cov(mrpt::math::UNINITIALIZED_MATRIX);
                                TDATA p;
                                this->getCovarianceAndMean(cov,p);
                                return cov;
                        }
                        
                        /** Returns whether the class instance holds the uncertainty in covariance or information form.
                         * \note By default this is going to be covariance form. *Inf classes
                         * (e.g. CPosePDFGaussianInf) store it in information form.
                         *
                         * \sa mrpt::traits::is_inf_type
                         */
                        virtual bool isInfType() const { return false; }

                        /** Returns the information (inverse covariance) matrix (a STATE_LEN x STATE_LEN matrix)
                          *  Unless reimplemented in derived classes, this method first reads the covariance, then invert it.
                          * \sa getMean, getCovarianceAndMean
                          */
                        virtual void getInformationMatrix(mrpt::math::CMatrixFixedNumeric<double,STATE_LEN,STATE_LEN> &inf) const
                        {
                                mrpt::math::CMatrixFixedNumeric<double,STATE_LEN,STATE_LEN> cov(mrpt::math::UNINITIALIZED_MATRIX);
                                TDATA p;
                                this->getCovarianceAndMean(cov,p);
                                cov.inv_fast(inf); // Destroy source cov matrix, since we don't need it anymore.
                        }

                        /** Save PDF's particles to a text file. See derived classes for more information about the format of generated files.
                        */
                        virtual void  saveToTextFile(const std::string &file) const = 0;

                        /** Draws a single sample from the distribution
                          */
                        virtual void  drawSingleSample( TDATA &outPart ) const = 0;

                        /** Draws a number of samples from the distribution, and saves as a list of 1xSTATE_LEN vectors, where each row contains a (x,y,z,yaw,pitch,roll) datum.
                          * This base method just call N times to drawSingleSample, but derived classes should implemented optimized method for each particular PDF.
                          */
                        virtual void  drawManySamples( size_t N, std::vector<mrpt::math::CVectorDouble> & outSamples ) const
                        {
                                outSamples.resize(N);
                                TDATA   pnt;
                                for (size_t i=0;i<N;i++)
                                {
                                        this->drawSingleSample(pnt);
                                        pnt.getAsVector(outSamples[i]);
                                }
                        }

                        /** this = p (+) this. This can be used to convert a PDF from local coordinates to global, providing the point (newReferenceBase) from which
                          *   "to project" the current pdf. Result PDF substituted the currently stored one in the object.
                          */
                        virtual void  changeCoordinatesReference( const mrpt::poses::CPose3D &newReferenceBase ) = 0;

                        /** Compute the entropy of the estimated covariance matrix.
                          * \sa http://en.wikipedia.org/wiki/Multivariate_normal_distribution#Entropy
                          */
                        inline double  getCovarianceEntropy() const
                        {
                                static const double ln_2PI= 1.8378770664093454835606594728112;
                                return 0.5*( STATE_LEN + STATE_LEN * ln_2PI + log( std::max(getCovariance().det(), std::numeric_limits<double>::epsilon() ) ) );
                        }

                }; // End of class def.

        } // End of namespace
} // End of namespace

#endif