/*---------------------------------------------------------------
	FILE: CPointPDFGaussian.h
	USE: See above.

   Part of the MRPT Library
   ISA - Universidad de Malaga - http://www.isa.uma.es
  ---------------------------------------------------------------*/
#ifndef CPointPDFGaussian_H
#define CPointPDFGaussian_H

#include <MRPT/MRML/CPointPDF.h>
#include <MRPT/UTILS/CMatrix.h>

namespace MRML
{
	/** A gaussian distribution for 3D points. Also a method for bayesian fusion is provided.
     *
	 * \sa CPointPDF
	 */
	class CPointPDFGaussian : public CPointPDF
	{
		// This must be added to any CSerializable derived class:
		DEFINE_SERIALIZABLE( CPointPDFGaussian )

	 public:
		/** Default constructor
		  */
		CPointPDFGaussian();

		/** Constructor
		  */
		CPointPDFGaussian( const CPoint3D &init_Mean );

		/** Constructor
		  */
		CPointPDFGaussian( const CPoint3D &init_Mean, const CMatrix &init_Cov );

		/** The mean value
		 */
		CPoint3D	mean;

		/** The 3x3 covariance matrix
		 */
		CMatrix		cov;

		 /** Returns an estimate of the pose, (the mean, or mathematical expectation of the PDF)
		  */
		CPoint3D  getEstimatedPoint() const;

		 /** Returns an estimate of the pose covariance matrix (3x3 cov.matrix  for x,y,phi variables)
		  */
		CMatrix  getEstimatedCovariance() const;

		/** Copy operator, translating if necesary (for example, between particles and gaussian representations)
		  */
		void  copyFrom(const CPointPDF &o);

		/** Save PDF's particles to a text file, containing the 2D pose in the first line, then the covariance matrix in next 3 lines.
		 */
		void  saveToTextFile(char *file) const;

		/** 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. Both the mean value and the covariance matrix are updated correctly.
		  */
		void  changeCoordinatesReference( const CPose3D &newReferenceBase );

		/** Bayesian fusion of two points gauss. distributions, then save the result in this object.
		  *  The process is as follows:<br>
		  *		- (x1,S1): Mean and variance of the p1 distribution.
		  *		- (x2,S2): Mean and variance of the p2 distribution.
		  *		- (x,S): Mean and variance of the resulting distribution.
		  *
		  *    S = (S1<sup>-1</sup> + S2<sup>-1</sup>)<sup>-1</sup>;
		  *    x = S * ( S1<sup>-1</sup>*x1 + S2<sup>-1</sup>*x2 );
		  */
		void  bayesianFusion( CPointPDFGaussian &p1, CPointPDFGaussian &p2 );

		/** Computes the "correspondence likelihood" of this PDF with another one: This is implemented as the integral from -inf to +inf of the product of both PDF.
		  * The resulting number is >=0.
		  * \sa productIntegralNormalizedWith
		  * \exception std::exception On errors like covariance matrix with null determinant, etc...
		  */
		double  productIntegralWith( CPointPDFGaussian &p);

		/** Computes the "correspondence likelihood" of this PDF with another one: This is implemented as the integral from -inf to +inf of the product of both PDF.
		  * The resulting number is in the range [0,1]
		  * \sa productIntegralWith
		  * \exception std::exception On errors like covariance matrix with null determinant, etc...
		  */
		double  productIntegralNormalizedWith( CPointPDFGaussian *p);

		/** Computes the "correspondence likelihood" of this PDF with another one: This is implemented as the integral from -inf to +inf of the product of both PDF.
		  * The resulting number is in the range [0,1].
		  * NOTE: This version ignores the "z" coordinates!!
		  * \sa productIntegralWith
		  * \exception std::exception On errors like covariance matrix with null determinant, etc...
		  */
		double  productIntegralNormalizedWith2D( CPointPDFGaussian *p);


	}; // End of class def.

} // End of namespace

#endif
