/* +---------------------------------------------------------------------------+
   |                     Mobile Robot Programming Toolkit (MRPT)               |
   |                          http://www.mrpt.org/                             |
   |                                                                           |
   | Copyright (c) 2005-2015, 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 CICP_H
#define CICP_H

#include <mrpt/slam/CMetricMapsAlignmentAlgorithm.h>
#include <mrpt/utils/CLoadableOptions.h>
#include <mrpt/utils/TEnumType.h>

namespace mrpt
{
	namespace slam
	{
		/** The ICP algorithm selection, used in mrpt::slam::CICP::options.
		  *  For details on the algorithms refer to http://www.mrpt.org/Scan_Matching_Algorithms
		  * \ingroup mrpt_slam_grp
		  */
		enum TICPAlgorithm
		{
			icpClassic = 0,
			icpLevenbergMarquardt,
			icpIKF
		};

		/** Several implementations of ICP (Iterative closest point) algorithms for aligning two point maps or a point map wrt a grid map.
		 *
		 *  CICP::AlignPDF() or CICP::Align() are the two main entry points of the algorithm.
		 *
		 *  To choose among existing ICP algorithms or customizing their parameters, see CICP::TConfigParams and the member \a options.
		 *
		 *  There exists an extension of the original ICP algorithm that provides multihypotheses-support for the correspondences, and which generates a Sum-of-Gaussians (SOG)
		 *    PDF as output. See scanmatching::robustRigidTransformation.
		 *
		 * For further details on the implemented methods, check the web:
		 *   http://www.mrpt.org/Iterative_Closest_Point_(ICP)_and_other_matching_algorithms
		 *
		 *  For a paper explaining some of the basic equations, see for example:
		 *   J. Martinez, J. Gonzalez, J. Morales, A. Mandow, A. Garcia-Cerezo,
		 *   "Mobile robot motion estimation by 2D scan matching with genetic and iterative closest point algorithms",
		 *    Journal of Field Robotics, vol. 23, no. 1, pp. 21-34, 2006. ( http://babel.isa.uma.es/~jlblanco/papers/martinez2006gil.pdf )
		 *
		 * \sa CMetricMapsAlignmentAlgorithm
		 * \ingroup mrpt_slam_grp
		 */
		class SLAM_IMPEXP  CICP : public mrpt::slam::CMetricMapsAlignmentAlgorithm
		{
		public:
			/** The ICP algorithm configuration data
			 */
			class SLAM_IMPEXP TConfigParams : public utils::CLoadableOptions
			{
			public:
				TConfigParams();	//!< Initializer for default values:

				void  loadFromConfigFile(
					const mrpt::utils::CConfigFileBase  &source,
					const std::string &section); //!< See utils::CLoadableOptions

				void  dumpToTextStream(mrpt::utils::CStream	&out) const; //!<See utils::CLoadableOptions


				/** The algorithm to use (default: icpClassic)
				  *  See http://www.mrpt.org/Scan_Matching_Algorithms for details.
				  */
				TICPAlgorithm	ICP_algorithm;

				bool	onlyClosestCorrespondences;  //!< The usual approach: to consider only the closest correspondence for each local point (Default to true)
				bool 	onlyUniqueRobust; //! Apart of "onlyClosestCorrespondences=true", if this option is enabled only the closest correspondence for each reference point will be kept (default=false).

				/** @name Termination criteria
				    @{ */
				unsigned int	maxIterations;  //!< Maximum number of iterations to run.
				float           minAbsStep_trans; //!< If the correction in all translation coordinates (X,Y,Z) is below this threshold (in meters), iterations are terminated (Default:1e-6)
				float           minAbsStep_rot;   //!< If the correction in all rotation coordinates (yaw,pitch,roll) is below this threshold (in radians), iterations are terminated (Default:1e-6)
				/** @} */

				float	thresholdDist,thresholdAng; //!< Initial threshold distance for two points to become a correspondence.
				float	ALFA;  //!< The scale factor for threshold everytime convergence is achieved.
				float	smallestThresholdDist;  //!< The size for threshold such that iterations will stop, since it is considered precise enough.

				/** 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;

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

				/** 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;

				/** RANSAC-step options:
				  * \sa CICP::robustRigidTransformation
				  */
				bool			ransac_fuseByCorrsMatch;

				/** RANSAC-step options:
				  * \sa CICP::robustRigidTransformation
				  */
				float			ransac_fuseMaxDiffXY, ransac_fuseMaxDiffPhi;

				/** Cauchy kernel rho, for estimating the optimal transformation covariance, in meters (default = 0.07m). */
				float			kernel_rho;

				/** Whether to use kernel_rho to smooth distances, or use distances directly (default=true) */
				bool			use_kernel;

				/** The size of the perturbance in x & y used to estimate the Jacobians of the square error (in LM & IKF methods, default=0.05).*/
				float			Axy_aprox_derivatives;

				/** The initial value of the lambda parameter in the LM method (default=1e-4). */
				float			LM_initial_lambda;

				/** Skip the computation of the covariance (saves some time) (default=false) */
				bool			skip_cov_calculation;

				/** Skip the (sometimes) expensive evaluation of the term 'quality' at ICP output (Default=true) */
				bool            skip_quality_calculation; 

				/** Decimation of the point cloud being registered against the reference one (default=5) - set to 1 to have the older (MRPT <0.9.5) behavior
				  *  of not approximating ICP by ignoring the correspondence of some points. The speed-up comes from a decimation of the number of KD-tree queries,
				  *  the most expensive step in ICP.
				  */
				uint32_t        corresponding_points_decimation;

			};

			TConfigParams  options; //!< The options employed by the ICP align.


			/** Constructor with the default options */
			CICP() : options() { }
			/** Constructor that directly set the ICP params from a given struct \sa options */
			CICP(const TConfigParams &icpParams) : options(icpParams) { }

			virtual ~CICP() { }	//!< Destructor


			/** The ICP algorithm return information.
			 */
			struct SLAM_IMPEXP TReturnInfo
			{
				TReturnInfo() :
					cbSize(sizeof(TReturnInfo)),
					nIterations(0),
					goodness(0),
					quality(0)
				{
				}

				/** 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;

				/** A measure of the 'quality' of the local minimum of the sqr. error found by the method.
				  * Higher values are better. Low values will be found in ill-conditioned situations (e.g. a corridor).
				  */
				float			quality;
			};

			/** 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"
			 *  \note The output PDF is a CPosePDFGaussian if "doRANSAC=false", or a CPosePDFSOG otherwise.
			 *
			 * \param m1			[IN] The first map (CAN BE A mrpt::poses::CPointsMap derived class or a mrpt::slam::COccupancyGrid2D class)
			 * \param m2			[IN] The second map. (MUST BE A mrpt::poses::CPointsMap derived class)The pose of this map respect to m1 is to be estimated.
			 * \param initialEstimationPDF	[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 A smart pointer to the output estimated pose PDF.
			 *
			 * \sa CMetricMapsAlignmentAlgorithm, CICP::options, CICP::TReturnInfo
			 */
			mrpt::poses::CPosePDFPtr AlignPDF(
					const mrpt::maps::CMetricMap		*m1,
					const mrpt::maps::CMetricMap		*m2,
					const mrpt::poses::CPosePDFGaussian	&initialEstimationPDF,
					float					*runningTime = NULL,
					void					*info = NULL );

			/** Align a pair of metric maps, aligning the full 6D pose.
			 *   The meaning of some parameters are implementation dependant,
			 *    so look at the derived classes for more details.
			 *  The goal is to find a PDF for the pose displacement between
			 *   maps, that is, <b>the pose of m2 relative to m1</b>. This pose
			 *   is returned as a PDF rather than a single value.
			 *
			 *  \note This method can be configurated with a "options" structure in the implementation classes.
			 *
			 * \param m1			[IN] The first map (MUST BE A COccupancyGridMap2D  derived class)
			 * \param m2			[IN] The second map. (MUST BE A CPointsMap derived class) The pose of this map respect to m1 is to be estimated.
			 * \param initialEstimationPDF	[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] See derived classes for details, or NULL if it isn't needed.
			 *
			 * \return A smart pointer to the output estimated pose PDF.
			 * \sa CICP
			 */
			mrpt::poses::CPose3DPDFPtr Align3DPDF(
					const mrpt::maps::CMetricMap		*m1,
					const mrpt::maps::CMetricMap		*m2,
					const mrpt::poses::CPose3DPDFGaussian	&initialEstimationPDF,
					float					*runningTime = NULL,
					void					*info = NULL );


		protected:
			/** Computes:
			  *  \f[ K(x^2) = \frac{x^2}{x^2+\rho^2}  \f]
			  *  or just return the input if options.useKernel = false.
			  */
			float kernel(const float &x2, const float &rho2);

			/** The internal method implementing CICP::AlignPDF when options.ICP_algorithm is icpClassic.
			  */
			mrpt::poses::CPosePDFPtr ICP_Method_Classic(
					const mrpt::maps::CMetricMap		*m1,
					const mrpt::maps::CMetricMap		*m2,
					const mrpt::poses::CPosePDFGaussian	&initialEstimationPDF,
					TReturnInfo				&outInfo );

			/** The internal method implementing CICP::AlignPDF when options.ICP_algorithm is icpLevenbergMarquardt.
			  */
			mrpt::poses::CPosePDFPtr ICP_Method_LM(
					const mrpt::maps::CMetricMap		*m1,
					const mrpt::maps::CMetricMap		*m2,
					const mrpt::poses::CPosePDFGaussian	&initialEstimationPDF,
					TReturnInfo				&outInfo );

			/** The internal method implementing CICP::AlignPDF when options.ICP_algorithm is icpIKF.
			  */
			mrpt::poses::CPosePDFPtr ICP_Method_IKF(
					const mrpt::maps::CMetricMap		*m1,
					const mrpt::maps::CMetricMap		*m2,
					const mrpt::poses::CPosePDFGaussian	&initialEstimationPDF,
					TReturnInfo				&outInfo );

			/** The internal method implementing CICP::Align3DPDF when options.ICP_algorithm is icpClassic.
			  */
			mrpt::poses::CPose3DPDFPtr ICP3D_Method_Classic(
					const mrpt::maps::CMetricMap		*m1,
					const mrpt::maps::CMetricMap		*m2,
					const mrpt::poses::CPose3DPDFGaussian &initialEstimationPDF,
					TReturnInfo				&outInfo );


		};

	} // End of namespace

	// Specializations MUST occur at the same namespace:
	namespace utils
	{
		template <>
		struct TEnumTypeFiller<slam::TICPAlgorithm>
		{
			typedef slam::TICPAlgorithm enum_t;
			static void fill(bimap<enum_t,std::string>  &m_map)
			{
				m_map.insert(slam::icpClassic, "icpClassic");
				m_map.insert(slam::icpLevenbergMarquardt, "icpLevenbergMarquardt");
				m_map.insert(slam::icpIKF, "icpIKF");
			}
		};
	} // End of namespace

} // End of namespace

#endif
