/* +---------------------------------------------------------------------------+
   |          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 CVisualOdometryStereo_H
#define CVisualOdometryStereo_H

#include <MRPT/MRML/CObservationStereoImages.h>
#include <MRPT/MRML/CPose3DPDFGaussian.h>
#include <MRPT/MRML/CPose3DPDFParticles.h>
#include <MRPT/MRML/CPose3D.h>
#include <MRPT/MRML/CMetricMap.h>
#include <MRPT/MRML/CObservationVisualLandmarks.h>
#include <MRPT/MRML/ScanMatching.h>

//#include <MRPT/MRVL/CKanadeLucasTomasi.h>
#include <MRPT/MRVL/mrvl.h>

#include <MRPT/UTILS/CMatrix.h>
#include <MRPT/UTILS/CMRPTImage.h>
#include <MRPT/UTILS/CDisplayWindow.h>

namespace MRVL
{

	class CVisualOdometryStereo
	{

	public:

		/** Constructor
		  */
		CVisualOdometryStereo();

		/** Resets the state of the Visual Odometry system.
		  */
		void  reset();

		/** Performs one step of the Visual Odometry system. It takes the input stereo observation and estimates the
		  *	camera movement between the previous observation and this one.
		  * If this is the first observation inserted into the system, this functions return FALSE.
		  * Otherwise it returns TRUE and yields a CPose3DPDFGaussian object containing the change in the 6D camera pose.
		  */
		bool  process( const MRML::CObservationStereoImages &inObs, MRML::CPose3DPDFGaussian &outEst );
		//bool  process( const MRML::CObservationStereoImages &inObs, MRML::CPose3DPDFGaussian &outEst, UTILS::CDisplayWindow &wind1, UTILS::CDisplayWindow &wind2 );


		enum TCov_Computation_Method
		{
			TCov_Linear = -1,			// Linear propagation of the uncertainty
			TCov_UT,					// Using the Unscented Transform (UT)
			TCov_Particles				// Using particles
		};

		/** Options for the Visual Odometry System
		  */
		struct TOptions
		{
			/** Initializer for default values
			  */
			TOptions();

			/** Method for computing the covariance
			  */
			TCov_Computation_Method covMethod;
			bool					verbose;
			unsigned int			nIter;

		} m_options;

		MRVL::TStereoSystemParams					param;

	protected:

		bool										firstObs;

		MRVL::CKanadeLucasTomasi					tracker;
		UTILS::CMRPTImage							imgL, imgR;
		MRML::CObservationVisualLandmarks			Prev_cloud;
		MRVL::CKanadeLucasTomasi::TKLTFeatureList	Left_KLTList, Right_KLTList;

		/** Compute the covariance of the change in pose using the method defined in the options
		  * (for internal use only)
		  */
		void  computeRTCovariance( const MRML::CPose3D &meanPose,
											 const MRML::CMetricMap::TMatchingPairList &list,
											 const MRML::CObservationVisualLandmarks &Pre_Cloud,
											 const MRML::CObservationVisualLandmarks &Cur_Cloud,
											 UTILS::CMatrixD &RTCov );

		/** Fills in the KLT lists from the matched SIFTS
		  * (for internal use only)
		  */
		void  storeKLTLists( const TSIFTFeatureList &Left_SIFTmatchedList,
									   const TSIFTFeatureList &Right_SIFTmatchedList );

		/** Adds the new SIFT matched points into the KLT Lists
		  * (for internal use only)
		  */
		void  addPointsToLists( const TSIFTFeatureList &Left_SIFTmatchedList,
										  const TSIFTFeatureList &Right_SIFTmatchedList );


	}; // end of class CVisualOdometryStereo
} // end of namespace

#endif
