/*---------------------------------------------------------------
	FILE: CCamModel.h

	Clase CCamModel

   ISA - Universidad de Malaga - http://www.isa.uma.es

   Autor: Antonio J. Ortiz de Galisteo (AJOGD)
  ---------------------------------------------------------------*/
#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::CVectorTemplate<float> &uvd, UTILS::CMatrixTemplateNumeric<float> &J_undist);

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

	/**	Return the pixel position distorted by the camera
	 */
		void  distort_a_point(const UTILS::CVectorTemplate<float> &uvu, UTILS::CVectorTemplate<float> &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
