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

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

#include <MRPT/MRML/CMetricMapsAlignmentAlgorithm.h>
#include <MRPT/UTILS/CLoadableOptions.h>

namespace MRML
{
	class	CPosePDFParticles;
	/** An implementation of ICP (Iterative closest point) algorithm for aligning two point maps.
     *
	 *   See CICP::AlignPDF for the invokation method and CICP::TConfigParams for all the parameters to the method.
	 *
	 * \sa CMetricMapsAlignmentAlgorithm
	 */
	class CICP : public CMetricMapsAlignmentAlgorithm
	{
	public:
        /** Destructor
          */
        virtual ~CICP() { }

		/** The ICP algorithm configuration data
		 */
		class TConfigParams : public UTILS::CLoadableOptions
		{
		public:
			/** Initializer for default values:
			  */
			TConfigParams();

			/** See UTILS::CLoadableOptions
			  */
			void  loadFromConfigFile(
				const CConfigFileBase  &source,
				const std::string &section);

			/** See UTILS::CLoadableOptions
			  */
			void  dumpToTextStream(CStream		&out);

			/** Faster but less robust to wrong associations (Default to true)
			  */
			bool	onlyClosestCorrespondences;

			/** Maximum number of iterations to run.
			  */
			unsigned short	maxIterations;

			/** Initial threshold distance for two points to become a correspondence.
			  */
			float	thresholdDist,thresholdAng;

			/**	The scale factor for threshold everytime convergence is achieved.
			 */
			float	ALFA;

			/** The size for threshold such that iterations will stop, since it is considered precise enough.
			 */
			float	smallestThresholdDist;

			/** This is the normalization constant \f$ \sigma^2_p \f$ that is used to scale the whole 3x3 covariance.
			  *  This has a default value of \f$ (0.02)^2 \f$, that is, a 2cm sigma.
			  *  See the paper: ....
			  */
			float	covariance_varPoints;

			/** Perform a RANSAC step after the ICP convergence, to obtain a better estimation of the pose PDF.
			  */
			bool	doRANSAC;

			/** RANSAC-step options:
			  * \sa CICP::robustRigidTransformation
			  */
			unsigned int	ransac_minSetSize,ransac_maxSetSize,ransac_nSimulations;

			/** RANSAC-step options:
			  * \sa CICP::robustRigidTransformation
			  */
			float			ransac_mahalanobisDistanceThreshold;

			/** RANSAC-step option: The standard deviation in X,Y of landmarks/points which are being matched (used to compute covariances in the SoG)
			  * \sa CICP::robustRigidTransformation
			  */
			float			normalizationStd;

		} options;

		/** The ICP algorithm return information.
		 */
		struct TReturnInfo
		{
			TReturnInfo() : cbSize(sizeof(TReturnInfo))
			{
			}

			/** Size in bytes of this struct: Must be set correctly before using it */
			unsigned int	cbSize;

			/** The number of executed iterations until convergence.
			 */
			unsigned short	nIterations;

			/** A goodness measure for the alignment, it is a [0,1] range indicator of percentage of correspondences.
			 */
			float			goodness;
		};

		/** An implementation of CMetricMapsAlignmentAlgorithm for the case of a point maps and a occupancy grid/point map.
         *
		 *  This method computes the PDF of the displacement (relative pose) between
		 *   two maps: <b>the relative pose of m2 with respect to m1</b>. This pose
		 *   is returned as a PDF rather than a single value.
		 *
		 *  \note This method can be configurated with "CICP::options"
		 *
		 * \param m1			[IN] The first map (CAN BE A MRML::CPointsMap derived class or a MRML::COccupancyGrid2D class)
		 * \param m2			[IN] The second map. (MUST BE A MRML::CPointsMap derived class)The pose of this map respect to m1 is to be estimated.
		 * \param initalEstimationPDF	[IN] An initial gross estimation for the displacement.
		 * \param runningTime	[OUT] A pointer to a container for obtaining the algorithm running time in seconds, or NULL if you don't need it.
		 * \param info			[OUT] A pointer to a CICP::TReturnInfo, or NULL if it isn't needed.
		 *
		 * \return The output estimated pose PDF. <b>REMEMBER to free the memory of the object with "delete" when not required any more</b>.
		 *
		 * \sa CMetricMapsAlignmentAlgorithm, CICP::options, CICP::TReturnInfo
		 */
		CPosePDF* AlignPDF(
				const CMetricMap		*m1,
				const CMetricMap		*m2,
				const CPosePDFGaussian	&initalEstimationPDF,
				float					*runningTime = NULL,
				void					*info = NULL );
	};


} // End of namespace

#endif
