/* +---------------------------------------------------------------------------+
   |          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 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.
		  *  The output 3D pose is computed using the method described in "Closed-form solution of absolute orientation using unit quaternions", BKP Horn, Journal of the Optical Society of America, 1987.
		  *  If supplied, the output covariance matrix is computed using... TODO
		  * \todo Explain covariance!!
		  *
		  * \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.
		  *  Implemented by FAMD, 2007.
		  * \sa robustRigidTransformation
		  */
		bool  leastSquareErrorRigidTransformation6D(
			CMetricMap::TMatchingPairList	&in_correspondences,
			CPose3D							&out_transformation,
			CMatrixD						*in_rotationMatrix = NULL,
			CMatrixD						*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,
			CMatrixD						*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 as a Sum of Gaussians.
		  * 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.
		  *
		  *  For more details refer to the tutorial on <a href="http://babel.isa.uma.es/mrpt/index.php/Scan_Matching_Algorithms">scan matching methods</a>.
		  *  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.
		  *
		  *  If ransac_fuseByCorrsMatch=true (the default), the weight of Gaussian modes will be increased when an exact match in the 
		  *   subset of correspondences for the modes is found. Otherwise, an approximate method is used as test by just looking at the 
		  *   resulting X,Y,PHI means (Threshold in this case are: ransac_fuseMaxDiffXY, ransac_fuseMaxDiffPhi).
		  *
		  * \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,
			bool						ransac_fuseByCorrsMatch = true,
			float						ransac_fuseMaxDiffXY = 0.01f, 
			float						ransac_fuseMaxDiffPhi = DEG2RAD(0.1f),
			bool						ransac_algorithmForLandmarks = true
			);

	}


} // End of namespace

#endif
