/*---------------------------------------------------------------
	FILE: ScanMatching.h
	USE: See DOXYGEN description below.

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

#include <MRPT/MRML/CMetricMap.h>

namespace MRML
{
	class	CPosePDFParticles;
	class	CPosePDFGaussian;
	class	CPosePDFSOG;

	/** A set of scan matching-related static functions.
	 *
	 * \sa MRML::CICP
	 */
	namespace ScanMatching
	{
		/** This method provides the closed-form solution of absolute orientation using unit quaternions to a set of over-constrained correspondences for finding the 6D rigid transformation between two cloud of 3D points.
		  * \param in_correspondences The set of correspondences.
		  * \param out_transformation The pose that minimizes the mean-square-error between all the correspondences.
		  * \param out_estimateCovariance If provided (!=NULL) this will contain on return a 3x3 covariance matrix with the NORMALIZED optimal estimate uncertainty. This matrix must be multiplied by \f$\sigma^2_p\f$, the variance of matched points in \f$x\f$ and \f$y\f$ (see paper....)
		  * \exception Raises a std::exception if the list "in_correspondences" has not a minimum of two correspondences.
		  * \return True if there are at least two correspondences, or false if one or none, thus we cannot establish any correspondence.
		  * \sa robustRigidTransformation
		  * by FAMD, 2007
		  */
		bool  ICP6D(
			CMetricMap::TMatchingPairList	&in_correspondences,
			CPose3D							&out_transformation,
			CMatrix							*in_rotationMatrix = NULL,
			CMatrix							*out_estimateCovariance = NULL );

		/** This method provides the basic least-square-error solution to a set of over-constrained correspondences for finding the (x,y,phi) rigid transformation between two planes.
		  * \param in_correspondences The set of correspondences.
		  * \param out_transformation The pose that minimizes the mean-square-error between all the correspondences.
		  * \param out_estimateCovariance If provided (!=NULL) this will contain on return a 3x3 covariance matrix with the NORMALIZED optimal estimate uncertainty. This matrix must be multiplied by \f$\sigma^2_p\f$, the variance of matched points in \f$x\f$ and \f$y\f$ (see paper....)
		  * \exception Raises a std::exception if the list "in_correspondences" has not a minimum of two correspondences.
		  * \return True if there are at least two correspondences, or false if one or none, thus we cannot establish any correspondence.
		  * \sa robustRigidTransformation
		  */
		bool  leastSquareErrorRigidTransformation(
			CMetricMap::TMatchingPairList	&in_correspondences,
			CPose2D							&out_transformation,
			CMatrix							*out_estimateCovariance = NULL );

		/** This method implements a RANSAC-based robust estimation of the rigid transformation between two planes, returning a probability distribution over all the posibilities.
		  * This works are follows:
				- Repeat "ransac_nSimulations" times:
					- Randomly pick TWO correspondences from the set "in_correspondences".
					- Compute the associated rigid transformation.
					- For "ransac_maxSetSize" randomly selected correspondences, test for "consensus" with the current group:
						- If if is compatible (ransac_mahalanobisDistanceThreshold), grow the "consensus set"
						- If not, do not add it.
		  *
		  *  NOTE:
		  *        - If a pointer is supplied to "out_largestSubSet", the largest consensus sub-set
		  *          of correspondences will be returned there.
		  *        - The parameter "normalizationStd" is the <b>standard deviation</b> (not variance) of landmarks
		  *          being matched in X,Y. Used to normalize covariances returned as the SoG.
		  *
		  * \exception Raises a std::exception if the list "in_correspondences" has not a minimum of two correspondences.
		  * \sa leastSquareErrorRigidTransformation
		  */
		void  robustRigidTransformation(
			MRML::CMetricMap::TMatchingPairList	&in_correspondences,
			MRML::CPosePDFSOG				*out_transformation,
			float							normalizationStd,
			unsigned int					ransac_minSetSize = 3,
			unsigned int					ransac_maxSetSize = 20,
			float							ransac_mahalanobisDistanceThreshold = 3.0f,
			unsigned int					ransac_nSimulations = 100,
			MRML::CMetricMap::TMatchingPairList	*out_largestSubSet = NULL
			);

	}


} // End of namespace

#endif
