/* +---------------------------------------------------------------------------+
   |          The Mobile Robot Programming Toolkit (MRPT) C++ library          |
   |                                                                           |
   |                   http://mrpt.sourceforge.net/                            |
   |                                                                           |
   |   Copyright (C) 2005-2008  University of Malaga                           |
   |                                                                           |
   |    This software was written by the Perception and Robotics               |
   |      research group, University of Malaga (Spain).                        |
   |    Contact: Jose-Luis Blanco  <jlblanco@ctima.uma.es>                     |
   |                                                                           |
   |  This file is part of the MRPT project.                                   |
   |                                                                           |
   |     MRPT is free software: you can redistribute it and/or modify          |
   |     it under the terms of the GNU General Public License as published by  |
   |     the Free Software Foundation, either version 3 of the License, or     |
   |     (at your option) any later version.                                   |
   |                                                                           |
   |   MRPT is distributed in the hope that it will be useful,                 |
   |     but WITHOUT ANY WARRANTY; without even the implied warranty of        |
   |     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         |
   |     GNU General Public License for more details.                          |
   |                                                                           |
   |     You should have received a copy of the GNU General Public License     |
   |     along with MRPT.  If not, see <http://www.gnu.org/licenses/>.         |
   |                                                                           |
   +---------------------------------------------------------------------------+ */
#ifndef CGridMapAligner_H
#define CGridMapAligner_H

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

namespace MRML
{
	class	CPosePDFSOG;
	class	CLandmark;
	/** A class for aligning two occupancy grid maps, based on features extraction and matching.
     * The matching pose is returned as a Sum of Gaussians (MRML::CPosePDFSOG).
	 *
	 *  This method was reported in the paper: <br>
	 *
	 * See CGridMapAligner::Align for more instructions.
	 *
	 * \sa CMetricMapsAlignmentAlgorithm
	 */
	class CGridMapAligner : public CMetricMapsAlignmentAlgorithm
	{
	private:
		/** Computes the matching between a pair of "panoramic images" landmarks, by the correlation method.
		  */
		void  landmarksMatchingCorrelation(
			MRML::CLandmark		*lm1,
			MRML::CLandmark		*lm2,
			float				&minDistance,
			float				&minDistAngle);

		/** Private member, implements one the algorithms.
		  */
		CPosePDF* AlignPDF_correlation(
				const CMetricMap		*m1,
				const CMetricMap		*m2,
				const CPosePDFGaussian	&initialEstimationPDF,
				float					*runningTime = NULL,
				void					*info = NULL );

		/** Private member, implements one the algorithms.
		  */
		CPosePDF* AlignPDF_robustMatch(
				const CMetricMap		*m1,
				const CMetricMap		*m2,
				const CPosePDFGaussian	&initialEstimationPDF,
				float					*runningTime = NULL,
				void					*info = NULL );

	public:

		CGridMapAligner() :
			options()
		{}

		/** The type for selecting the grid-map alignment algorithm.
		  */
		enum TAlignerMethod
		{
			amRobustMatch = 0,
			amCorrelation
		};

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

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

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


			/** The aligner method:
			  */
			TAlignerMethod		methodSelection;

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

			/** The square root of the uncertainty normalization variance var_m (see papers...)
			  */
			float			ransac_SOG_sigma_m;

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

			/** Features extraction from grid map: How many features to extract
			  */
			float			featsPerSquareMeter;

			/** Features extraction from grid map: Number of directions sectors
			  */
			unsigned int feats_DirectionSectors;

			/** Features extraction from grid map: Number of range sectors
			  */
			unsigned int feats_RangeSectors;

			/** Features extraction from grid map: Closer distance
			  */
			float feats_startDist;

			/** Features extraction from grid map: Farthest distance
			  */
			float feats_endDist;

			/** Correspondences are considered if their distances are below the "mean - thresholdStdTimes · std" of the overall distances.
			  */
			float	thresholdStdTimes;

		} options;

		/** The ICP algorithm return information.
		 */
		struct TReturnInfo
		{
			/** Initialization
			  */
			TReturnInfo() :
				goodness(0),
				noRobustEstimation()
			{
			}

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

			/** The "brute" estimation from using all the available correspondences, provided just for comparison purposes (it is not the robust estimation, available as the result of the Align method).
			  */
			CPose2D			noRobustEstimation;
		};

		/** The method for aligning a pair of 2D points map.
		 *   The meaning of some parameters are implementation dependant,
		 *    so look for derived classes for instructions.
		 *  The target is to find a PDF for the pose displacement between
		 *   maps, thus <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 "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 initialEstimationPDF	[IN] (IGNORED IN THIS ALGORITHM!)
		 * \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 CAlignerFromMotionDraws::TReturnInfo struct, or NULL if result information are not required.
		 *
		 * \note The returned PDF depends on the selected alignment method:
		 *		- "amRobustMatch" --> A "MRML::CPosePDFParticles" object.
		 *		- "amCorrelation" --> A "MRML::CPosePDFGrid" object.
		 *
		 * \return The output estimated pose PDF. <b>REMEMBER to free the memory of the object with "delete" when not required any more</b>.
		 * \sa CPointsMapAlignmentAlgorithm, options
		 */
		CPosePDF* AlignPDF(
				const CMetricMap		*m1,
				const CMetricMap		*m2,
				const CPosePDFGaussian	&initialEstimationPDF,
				float					*runningTime = NULL,
				void					*info = NULL );

	};


} // End of namespace

#endif
