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

#include <MRPT/UTILS/CMatrixTemplateNumeric.h>
#include <MRPT/UTILS/CVectorTemplate.h>
#include <MRPT/UTILS/CConfigFile.h>
#include <MRPT/UTILS/MRPT_OS.h>

namespace MRVL
{
	/** AJOGD Estructura para almacenar los datos de la camara para MONOSLAM
	 */
	struct TCamera{
		float	d;
		unsigned int	nrows;
		unsigned int	ncols;
		float	cx;
		float	cy;
		float	k1;
		float	k2;
		float	f;
	};

	/** This class represent the camera model for Monocular SLAM.
	 * For the moment only pinhole model is employed and implemented.
	*/
	class CCamModel
	{
    protected:
		MRVL::TCamera cam;

	public:
	/** Defalult Constructor
	 */
		CCamModel();
	/** Constructor from a init file
	 */
        CCamModel(const std::string &file);

	/** Return the pixel dimension (it is supossed that pixel heigth and weigth is equal).
	 */
		float  cam_d()const {return cam.d;}
	/** Return the rows number for a image captured by the camera.
	 */
		unsigned int  cam_nrows()const {return cam.nrows;}

	/** Return the columns number for a image captured by the camera.
	 */
		unsigned int  cam_ncols()const {return cam.ncols;}

	/** Return the 'x' position of the optic center over the image
	 */
		float  cam_cx()const {return cam.cx;}

	/** Return the 'y' position of the optic center over the image
	 */
		float  cam_cy()const {return cam.cy;}

	/** Return the first parameter of radial distortion
	 */
		float  cam_k1()const {return cam.k1;}

	/** Return the first parameter of radial distortion
	 */
		float  cam_k2()const {return cam.k2;}

	/** Return the focal distance of the camera
	 */
		float  cam_f()const {return cam.f;}

	/** Jacobian for undistortion the image coordinates
	 */
		void  jacob_undistor_fm(const UTILS::CVectorFloat &uvd, UTILS::CMatrixFloat &J_undist);

	/** Calculate the image coordinates undistorted
	 */
		UTILS::CMatrixFloat  jacob_undistor(float &col , float &row );

	/**	Return the pixel position distorted by the camera
	 */
		void  distort_a_point(const UTILS::CVectorFloat &uvu, UTILS::CVectorFloat &uvd);

	/**	Return the pixel position undistorted by the camera
	 *	The input values 'col' and 'row' will be replace for the new values (undistorted)
	 */
		void  undistort_point(float &col,float &row);

	}; // end class

} // end namespace

#endif //__CCamModel_H
