/* +---------------------------------------------------------------------------+
   |                     Mobile Robot Programming Toolkit (MRPT)               |
   |                          http://www.mrpt.org/                             |
   |                                                                           |
   | Copyright (c) 2005-2015, Individual contributors, see AUTHORS file        |
   | See: http://www.mrpt.org/Authors - All rights reserved.                   |
   | Released under BSD License. See details in http://www.mrpt.org/License    |
   +---------------------------------------------------------------------------+ */

#include "vision-precomp.h"   // Precompiled headers
#include <mrpt/vision/CCamModel.h>
#include <mrpt/vision/pinhole.h>
#include <mrpt/utils/CFileOutputStream.h>
#include <mrpt/utils/types_math.h>

using namespace mrpt;
using namespace mrpt::vision;
using namespace mrpt::poses;
using namespace mrpt::math;
using namespace mrpt::utils;


/**	Constructor */
CCamModel::CCamModel() : cam()
{
}

/**********************************************************************************************************************/
void  CCamModel::jacob_undistor_fm(const mrpt::utils::TPixelCoordf &p, math::CMatrixDouble &J_undist)
{
	// JL: CHECK!!!!
	const double Cx = cam.cx();
	const double Cy = cam.cy();
	const double k1 = cam.k1();
	const double k2 = cam.k2();
	const double dx = 1.0 / cam.fx();   // JL: Check if formulas hold with this change!!!
	const double dy = 1.0 / cam.fy();

	double xd = (p.x-Cx)*dx;
	double yd = (p.y-Cy)*dy;
	double rd2=xd*xd+yd*yd;
	double rd4=rd2*rd2;

	J_undist.setSize(2,2);
	J_undist(0,0) = (1+k1*rd2+k2*rd4) + (p.x-Cx) * (k1+2*k2*rd2) * (2*(p.x-Cx)*dx*dx);
	J_undist(1,1) = (1+k1*rd2+k2*rd4) + (p.y-Cy) * (k1+2*k2*rd2) * (2*(p.y-Cy)*dy*dy);
	J_undist(0,1) = (p.x-Cx) * (k1+2*k2*rd2) * (2*(p.y-Cy)*dy*dy);
	J_undist(1,0) = (p.y-Cy) * (k1+2*k2*rd2) * (2*(p.x-Cx)*dx*dx);

}
/******************************************************************************************************************************/

void CCamModel::jacob_undistor(const mrpt::utils::TPixelCoordf &p, mrpt::math::CMatrixDouble &J_undistor)
{

	//J_undistor.setSize(2,2);
	const double dx = p.x-cam.cx();
	const double dy = p.y-cam.cy();
	const double f = 1 - 2*cam.k1()*square(dx)*square(dy);
	const double inv_f_15 = 1.0/pow(f,1.5);

	//// dy/du
	//CMatrixDouble dy_du(2,2);  // Default is all zeroes
	//dy_du(0,0) = 1.0/cam.fx;
	//dy_du(1,1) = 1.0/cam.fy;

	// du/dh
	//CMatrixDouble du_dh(2,2);

	J_undistor(0,0) = ( 1 - 2*cam.k1()*square(dy) ) * inv_f_15;
	J_undistor(0,1) =
	J_undistor(1,0) = ( 2*cam.k1()*dx*dy ) * inv_f_15;
	J_undistor(1,1) = ( 1 - 2*cam.k1()*square(dx) ) * inv_f_15;


	//// JL: TODO: CHECK!
	//const double Cx = cam.cx;
	//const double Cy = cam.cy;
	//const double k1 = cam.k1();
	//const double k2 = cam.k2();
	//const double dx = 1.0 / cam.fx;   // JL: Check if formulas hold with this change!!!
	//const double dy = 1.0 / cam.fy;

	//double xd = (p.x-Cx)*dx;
	//double yd = (p.y-Cy)*dy;

	//double rd2 = xd*xd + yd*yd;
	//double rd4 = rd2 * rd2;

	//J_undistor.setSize(2,2);

	//J_undistor(0,0) = (1+k1*rd2+k2*rd4) + (p.x-Cx) * (k1+2*k2*rd2) * (2*(p.x-Cx)*dx*dx);
	//J_undistor(1,1) = (1+k1*rd2+k2*rd4) + (p.y-Cy) * (k1+2*k2*rd2) * (2*(p.y-Cy)*dy*dy);
	//J_undistor(0,1) = (p.x-Cx) * (k1+2*k2*rd2) * (2*(p.y-Cy)*dy*dy);
	//J_undistor(1,0) = (p.y-Cy) * (k1+2*k2*rd2) * (2*(p.x-Cx)*dx*dx);



}
/**********************************************************************************************************************/

void  CCamModel::distort_a_point(const mrpt::utils::TPixelCoordf &p, mrpt::utils::TPixelCoordf &distorted_p)
{
	// JLBC: Added from Davison's SceneLib:
	//
	// 1 distortion coefficient model
	//
	const double dx = (p.x-cam.cx()); // / cam.fx;   //JL: commented out cam.fxy... (dx,dy) units must be pixels
	const double dy = (p.y-cam.cy()); // / cam.fy;

	const double r2 = square(dx) + square(dy);

	const double fact = 1.0/sqrt(1+2*cam.k1()*r2);

	distorted_p.x = cam.cx() + dx*fact;
	distorted_p.y = cam.cy() + dy*fact;
	return;
}
/*************************************************************************************************************************/
// Removes distortion of a pair of pixel coordinates x,y.
void  CCamModel::undistort_point(const mrpt::utils::TPixelCoordf &p, mrpt::utils::TPixelCoordf &undistorted_p)
{
	std::vector<TPixelCoordf> in_p(1), out_p;
	in_p[0] = p;

	mrpt::vision::pinhole::undistort_points(
		in_p,
		out_p,
		cam.intrinsicParams,
		cam.getDistortionParamsAsVector()
		);

	ASSERT_(out_p.size()==1);
	undistorted_p = out_p[0];

	// It's explained fine in page 3, "A visual compass based on SLAM"
}


/*************************************************************************************************************************/
/*************************************************************************************************************************/
/**************************************************Davison Style**********************************************************/
/*************************************************************************************************************************/


/**	Return the (distorted) pixel position of a 3D point given in coordinates relative to the camera (+Z pointing forward, +X to the right)
 */
void  CCamModel::project_3D_point(const mrpt::math::TPoint3D &p3D, mrpt::utils::TPixelCoordf &distorted_p) const
{
	// JLBC: From Davison's SceneLib:
	//
	// 1 distortion coefficient model (+ projection)
	//

	// Offsets from the image center for the undistorted projection, in units of pixels:

	ASSERT_(p3D.z!=0)
	const double dx = (p3D.x / p3D.z)*cam.fx();
	const double dy = (p3D.y / p3D.z)*cam.fy();

	// 1 distortion coeff. model:
	const double r2 = square(dx) + square(dy);

	const double fact = 1.0/sqrt(1+2*cam.k1()*r2);   // Note the "+2" sign

	distorted_p.x = cam.cx() + dx*fact;
	distorted_p.y = cam.cy() + dy*fact;
}

/**	Return the 3D location of a point (at a fixed distance z=1), for the given (distorted) pixel position
  */
void  CCamModel::unproject_3D_point(const mrpt::utils::TPixelCoordf &distorted_p, mrpt::math::TPoint3D &p3D) const
{
	// JLBC: From Davison's SceneLib:
	//
	// 1 distortion coefficient model (+ projection)
	//
	const double dx = distorted_p.x - cam.cx();
	const double dy = distorted_p.y - cam.cy();
	const double r2 = square(dx)+square(dy);
	double factor = 1.0/sqrt(1 - 2*cam.k1()*r2);   // Note the "-2" sign

	p3D.x = dx * factor / cam.fx();
	p3D.y = dy * factor / cam.fy();
	p3D.z = 1.0;
}


// Jacobian of the projection of 3D points (with distortion), as done in project_3D_point \f$ \frac{\partial \vct{h}}{\partial \vct{y}} \f$
// JL: See .h file for all the formulas
void CCamModel::jacobian_project_with_distortion(const mrpt::math::TPoint3D &p3D, math::CMatrixDouble & dh_dy ) const
{
	/*
	\frac{\partial \vct{u}}{\partial \vct{y}} =
	\left( \begin{array}{ccc}
	 \frac{f_x}{y_z} &  0 & - y \frac f_x}{y_z^2} \\
	 0 & \frac{f_y}{y_z} & - y \frac f_y}{y_z^2} \\
	\end{array} \right)
	*/
	ASSERT_(p3D.z!=0)

	CMatrixDouble  du_dy(2,3); // Default all to zeroes.

	du_dy(0,0) = cam.fx() / p3D.z;  du_dy(0,2) = - p3D.x * cam.fx() / square(p3D.z);
	du_dy(1,1) = cam.fy() / p3D.z;  du_dy(1,2) = - p3D.y * cam.fy() / square(p3D.z);

	/*
	 f = 1+ 2  k_1  (u_x^2+u_y^2),  then:

	\frac{\partial \vct{h}}{\partial \vct{u}} =
	\left( \begin{array}{cc}
	 \frac{ 1+2 k_1 u_y^2 }{f^{3/2}}  &  -\frac{2 u_x u_y k_1 }{f^{3/2}} \\
	 -\frac{2 u_x u_y k_1 }{f^{3/2}}  & \frac{ 1+2 k_1 u_x^2 }{f^{3/2}}
	\end{array} \right)
	*/
	const double ux = (p3D.x / p3D.z)*cam.fx();  // coordinates with (0,0) at the image center
	const double uy = (p3D.y / p3D.z)*cam.fy();

	const double ux_sqr = square(ux);
	const double uy_sqr = square(uy);

	const double r2 = ux_sqr + uy_sqr;

	const double f = 1+2*cam.k1()*r2;
    const double f1_2 = sqrt(f);
    const double f3_2 = f1_2 * f;

  // Now form the proper dh_by_du by manipulating the outer product matrix

	CMatrixDouble  dh_du(2,2);
	dh_du *= -2 * cam.k1() / f3_2;
	dh_du(0,0) += (1/f1_2);
	dh_du(1,1) += (1/f1_2);

	//dh_du(0,0) = 1+2*cam.k1()*uy_sqr;
	//dh_du(1,1) = 1+2*cam.k1()*ux_sqr;

	//dh_du(0,1) =
	//dh_du(1,0) = -2*ux*uy*cam.k1()/pow(f,1.5);

	// Jacobian dh_dy = dh_du * du_dy   (Result is 2x3)
	dh_dy.multiply( dh_du, du_dy );
}

/* Jacobian of the unprojection of a pixel (with distortion) back into a 3D point, as done in unproject_3D_point \f$ \frac{\partial \vct{y}}{\partial \vct{h}} \f$, evaluated at the pixel p
\note JLBC: Added in March, 2009. Should be equivalent to Davison's WideCamera::ProjectionJacobian
\sa unproject_3D_point
*/
void CCamModel::jacobian_unproject_with_distortion(const mrpt::utils::TPixelCoordf &p, math::CMatrixDouble & dy_dh ) const
{
	// dy/du
	CMatrixDouble dy_du(3,2);  // Default is all zeroes
	dy_du(0,0) = 1.0/cam.fx();
	dy_du(1,1) = 1.0/cam.fy();

	//MAAA:
	//// du/dh
	const double dx = p.x-cam.cx();
	const double dy = p.y-cam.cy();
	const double radi2 = square(dx)+square(dy);

	double f = 1 - 2 * cam.k1() * radi2;
	double f1_2 = sqrt(f);
	double f3_2 = f1_2 * f;

	CMatrixDouble du_dh(2,3);

	//const double f = 1 - 2*cam.k1()*radi2;
	//const double inv_f_15 = 1.0f/powf(f,1.5f);
	//du_dh(0,0) = ( 1 - 2*cam.k1()*square(dy) ) * inv_f_15;
	//du_dh(0,1) =
	//du_dh(1,0) = ( 2*cam.k1()*dx*dy ) * inv_f_15;
	//du_dh(1,1) = ( 1 - 2*cam.k1()*square(dx) ) * inv_f_15;

    du_dh *= 2 * cam.k1() / f3_2;
    du_dh(0,0) += (1/f1_2);
    du_dh(1,1) += (1/f1_2);

	// Jacobian dy_dh = dy_du * du_dh   (Result is 3x2)
	dy_dh.multiply( dy_du, du_dh );

}

void  CCamModel::loadFromConfigFile(
	const mrpt::utils::CConfigFileBase	&source,
	const std::string		&section)
{
	MRPT_START

    // Read camera parameters: They are mandatory, we'll raise an exception if not found:
	double	cx = 0.0f, cy = 0.0f, fx = 0.0f, fy = 0.0f;

	//MRPT_LOAD_HERE_CONFIG_VAR_NO_DEFAULT(nrows,int,cam.nrows,    source, section)
	//MRPT_LOAD_HERE_CONFIG_VAR_NO_DEFAULT(ncols,int,cam.ncols,    source, section)

	MRPT_LOAD_HERE_CONFIG_VAR_NO_DEFAULT(cx,double,cx,    source, section)
	MRPT_LOAD_HERE_CONFIG_VAR_NO_DEFAULT(cy,double,cy,    source, section)
	MRPT_LOAD_HERE_CONFIG_VAR_NO_DEFAULT(fx,double,fx,    source, section)
	MRPT_LOAD_HERE_CONFIG_VAR_NO_DEFAULT(fy,double,fy,    source, section)

	cam.setIntrinsicParamsFromValues( fx, fy, cx, cy );

	CVectorDouble DD;
	source.read_vector(section,"dist_params",CVectorDouble(),DD,true);
	ASSERT_( DD.size()==4 || DD.size()==5 )

	this->cam.setDistortionParamsVector(DD);

	MRPT_END
}

/** This method displays clearly all the contents of the structure in textual form, sending it to a CStream. */
void  CCamModel::dumpToTextStream( CStream		&out) const
{
	MRPT_UNUSED_PARAM(out);
}

