text: /* +------------------------------------------------------------------------+
     |                     Mobile Robot Programming Toolkit (MRPT)            |
     |                          http://www.mrpt.org/                          |
     |                                                                        |
     | Copyright (c) 2005-2018, 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 |
     +------------------------------------------------------------------------+ */
  #pragma once
  
  #include <mrpt/img/TCamera.h>
  #include <mrpt/system/os.h>
  #include <mrpt/vision/utils.h>
  #include <mrpt/math/lightweight_geom_data.h>
  #include <mrpt/math/CArrayNumeric.h>
  
  namespace mrpt::vision
  {
  /** This class represent a pinhole camera model for Monocular SLAM and
   * implements some associated Jacobians
   *
   *  The camera parameters are accessible in the public member CCamModel::cam
   *
   * - Versions:
   *    - First version: By Antonio J. Ortiz de Galistea.
   *    - 2009-2010: Rewritten by various authors.
   *
   * \sa mrpt::img::TCamera, CMonoSlam, the application <a
   * href="http://www.mrpt.org/Application:camera-calib-gui" >camera-calib-gui</a>
   * for calibrating a camera
   * \ingroup mrpt_vision_grp
   */
  class CCamModel : public mrpt::config::CLoadableOptions
  {
     public:
  	/** The parameters of a camera */
  	mrpt::img::TCamera cam;
  
  	/** Default Constructor */
  	CCamModel();
  
  	void loadFromConfigFile(
  		const mrpt::config::CConfigFileBase& source,
  		const std::string& section) override;  // See base docs
  	void dumpToTextStream(std::ostream& out) const override;  // See base docs
  
  	/** Constructor from a ini file
  	 */
  	CCamModel(const mrpt::config::CConfigFileBase& cfgIni);
  
  	/** Jacobian for undistortion the image coordinates */
  	void jacob_undistor_fm(
  		const mrpt::img::TPixelCoordf& uvd, math::CMatrixDouble& J_undist);
  
  	/** Calculate the image coordinates undistorted
  	 */
  	void jacob_undistor(
  		const mrpt::img::TPixelCoordf& p, mrpt::math::CMatrixDouble& J_undist);
  
  	/**	Return the pixel position distorted by the camera
  	 */
  	void distort_a_point(
  		const mrpt::img::TPixelCoordf& p, mrpt::img::TPixelCoordf& distorted_p);
  
  	/**	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(
  		const mrpt::img::TPixelCoordf& p,
  		mrpt::img::TPixelCoordf& undistorted_p);
  
  	/**	Return the (distorted) pixel position of a 3D point given in
  	 * coordinates relative to the camera (+Z pointing forward, +X to the right)
  	 * \sa unproject_3D_point
  	 */
  	void project_3D_point(
  		const mrpt::math::TPoint3D& p3D,
  		mrpt::img::TPixelCoordf& distorted_p) const;
  
  	/**	Return the 3D location of a point (at a fixed distance z=1), for the
  	 * given (distorted) pixel position
  	 * \sa project_3D_point
  	 * \note Of course, there is a depth ambiguity, so the returned 3D point
  	 * must be considered a direction from the camera focus, or a vector, rather
  	 * than a meaninful physical point.
  	 */
  	void unproject_3D_point(
  		const mrpt::img::TPixelCoordf& distorted_p,
  		mrpt::math::TPoint3D& p3D) const;
  
  	/** Jacobian of the projection of 3D points (with distortion), as done in
  	project_3D_point \f$ \frac{\partial h}{\partial y} \f$, evaluated at the
  	point p3D (read below the full explanation)
  
  	We define \f$ h = (h_x ~ h_y) \f$ as the projected point in pixels (origin
  	at the top-left corner),
  	and \f$ y=( y_x ~ y_y ~ y_z ) \f$ as the 3D point in space, in coordinates
  	relative to the camera (+Z pointing forwards).
  
  	Then this method computes the 2x3 Jacobian:
  
  	\f[
  	\frac{\partial h}{\partial y} =  \frac{\partial h}{\partial u}
  	\frac{\partial u}{\partial y}
  	\f]
  
  	With:
  
  	\f[
  	\frac{\partial u}{\partial 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)
  	\f]
  
  	where \f$ f_x, f_y \f$ is the focal length in units of pixel sizes in x and
  	y, respectively.
  	And, if we define:
  
  	\f[
  	 f = 1+ 2  k_1  (u_x^2+u_y^2)
  	\f]
  
  	then:
  
  	\f[
  	\frac{\partial h}{\partial 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)
  	\f]
  
  	\note JLBC: Added in March, 2009. Should be equivalent to Davison's
  	WideCamera::ProjectionJacobian
  	\sa project_3D_point
  	*/
  	void jacobian_project_with_distortion(
  		const mrpt::math::TPoint3D& p3D, math::CMatrixDouble& dh_dy) const;
  
  	/** Jacobian of the unprojection of a pixel (with distortion) back into a 3D
  	point, as done in unproject_3D_point \f$ \frac{\partial y}{\partial h} \f$,
  	evaluated at the pixel p
  	\note JLBC: Added in March, 2009. Should be equivalent to Davison's
  	WideCamera::UnprojectionJacobian
  	\sa unproject_3D_point
  	*/
  	void jacobian_unproject_with_distortion(
  		const mrpt::img::TPixelCoordf& p, math::CMatrixDouble& dy_dh) const;
  
  	template <typename T>
  	struct CameraTempVariables
  	{
  		T x_, y_;
  		T x_2, y_2;
  		T R;
  		T K;
  		T x__, y__;
  	};
  	template <typename T, typename POINT>
  	void getTemporaryVariablesForTransform(
  		const POINT& p, CameraTempVariables<T>& v) const
  	{
  		v.x_ = p[1] / p[0];
  		v.y_ = p[2] / p[0];
  		v.x_2 = square(v.x_);
  		v.y_2 = square(v.y_);
  		v.R = v.x_2 + v.y_2;
  		v.K = 1 + v.R * (cam.k1() + v.R * (cam.k2() + v.R * cam.k3()));
  		T xy = v.x_ * v.y_, p1 = cam.p1(), p2 = cam.p2();
  		v.x__ = v.x_ * v.K + 2 * p1 * xy + p2 * (3 * v.x_2 + v.y_2);
  		v.y__ = v.y_ * v.K + p1 * (v.x_2 + 3 * v.y_2) + 2 * p2 * xy;
  	}
  
  	template <typename T, typename POINT, typename PIXEL>
  	inline void getFullProjection(const POINT& pIn, PIXEL& pOut) const
  	{
  		CameraTempVariables<T> tmp;
  		getTemporaryVariablesForTransform(pIn, tmp);
  		getFullProjectionT(tmp, pOut);
  	}
  
  	template <typename T, typename PIXEL>
  	inline void getFullProjectionT(
  		const CameraTempVariables<T>& tmp, PIXEL& pOut) const
  	{
  		pOut[0] = cam.fx() * tmp.x__ + cam.cx();
  		pOut[1] = cam.fy() * tmp.y__ + cam.cy();
  	}
  
  	template <typename T, typename POINT, typename MATRIX>
  	inline void getFullJacobian(const POINT& pIn, MATRIX& mOut) const
  	{
  		CameraTempVariables<T> tmp;
  		getTemporaryVariablesForTransform(pIn, tmp);
  		getFullJacobianT(pIn, tmp, mOut);
  	}
  
  	template <typename T, typename POINT, typename MATRIX>
  	void getFullJacobianT(
  		const POINT& pIn, const CameraTempVariables<T>& tmp, MATRIX& mOut) const
  	{
  		T x_ = 1 / pIn[0];
  		T x_2 = square(x_);
  		// First two jacobians...
  		mrpt::math::CMatrixFixedNumeric<T, 3, 3> J21;
  		T tmpK = 2 * (cam.k1() + tmp.R * (2 * cam.k2() + 3 * tmp.R * cam.k3()));
  		T tmpKx = tmpK * tmp.x_;
  		T tmpKy = tmpK * tmp.y_;
  		T yx2 = -pIn[1] * x_2;
  		T zx2 = -pIn[2] * x_2;
  		J21.set_unsafe(0, 0, yx2);
  		J21.set_unsafe(0, 1, x_);
  		J21.set_unsafe(0, 2, 0);
  		J21.set_unsafe(1, 0, zx2);
  		J21.set_unsafe(1, 1, 0);
  		J21.set_unsafe(1, 2, x_);
  		J21.set_unsafe(2, 0, tmpKx * yx2 + tmpKy * zx2);
  		J21.set_unsafe(2, 1, tmpKx * x_);
  		J21.set_unsafe(2, 2, tmpKy * x_);
  		// Last two jacobians...
  		T pxpy = 2 * (cam.p1() * tmp.x_ + cam.p2() * tmp.y_);
  		T p1y = cam.p1() * tmp.y_;
  		T p2x = cam.p2() * tmp.x_;
  		mrpt::math::CMatrixFixedNumeric<T, 2, 3> J43;
  		T fx = cam.fx(), fy = cam.fy();
  		J43.set_unsafe(0, 0, fx * (tmp.K + 2 * p1y + 6 * p2x));
  		J43.set_unsafe(0, 1, fx * pxpy);
  		J43.set_unsafe(0, 2, fx * tmp.x_);
  		J43.set_unsafe(1, 0, fy * pxpy);
  		J43.set_unsafe(1, 1, fy * (tmp.K + 6 * p1y + 2 * p2x));
  		J43.set_unsafe(1, 2, fy * tmp.y_);
  		mOut.multiply(J43, J21);
  		// cout<<"J21:\n"<<J21<<"\nJ43:\n"<<J43<<"\nmOut:\n"<<mOut;
  	}
  
     private:
  	// These functions are little tricks to avoid multiple initialization.
  	// They are intended to initialize the common parts of the jacobians just
  	// once,
  	// and not in each iteration.
  	// They are mostly useless outside the scope of this function.
  	mrpt::math::CMatrixFixedNumeric<double, 2, 2> firstInverseJacobian() const
  	{
  		mrpt::math::CMatrixFixedNumeric<double, 2, 2> res;
  		res.set_unsafe(0, 1, 0);
  		res.set_unsafe(1, 0, 0);
  		return res;
  	}
  	mrpt::math::CMatrixFixedNumeric<double, 4, 2> secondInverseJacobian() const
  	{
  		mrpt::math::CMatrixFixedNumeric<double, 4, 2> res;
  		res.set_unsafe(0, 0, 1);
  		res.set_unsafe(0, 1, 0);
  		res.set_unsafe(1, 0, 0);
  		res.set_unsafe(1, 1, 1);
  		return res;
  	}
  	mrpt::math::CMatrixFixedNumeric<double, 3, 4> thirdInverseJacobian() const
  	{
  		mrpt::math::CMatrixFixedNumeric<double, 3, 4> res;
  		res.set_unsafe(0, 1, 0);
  		res.set_unsafe(0, 2, 0);
  		res.set_unsafe(1, 0, 0);
  		res.set_unsafe(1, 2, 0);
  		res.set_unsafe(2, 0, 0);
  		res.set_unsafe(2, 1, 0);
  		res.set_unsafe(2, 2, 1);
  		res.set_unsafe(2, 3, 0);
  		return res;
  	}
  
     public:
  	template <typename POINTIN, typename POINTOUT, typename MAT22>
  	void getFullInverseModelWithJacobian(
  		const POINTIN& pIn, POINTOUT& pOut, MAT22& jOut) const
  	{
  		// Temporary variables (well, there are some more, but these are the
  		// basics)
  		// WARNING!: this shortcut to avoid repeated initialization makes the
  		// method somewhat
  		// faster, but makes it incapable of being used in more than one thread
  		// simultaneously!
  		using mrpt::square;
  		static mrpt::math::CMatrixFixedNumeric<double, 2, 2> J1(
  			firstInverseJacobian());
  		static mrpt::math::CMatrixFixedNumeric<double, 4, 2> J2(
  			secondInverseJacobian());
  		static mrpt::math::CMatrixFixedNumeric<double, 3, 4> J3(
  			thirdInverseJacobian());
  		static mrpt::math::CMatrixFixedNumeric<double, 2, 3> J4;  // This is not
  		// initialized
  		// in a
  		// special
  		// way,
  		// although
  		// declaring
  		// it
  		mrpt::math::CArrayNumeric<double, 4> tmp1;
  		mrpt::math::CArrayNumeric<double, 2> tmp2;  // This would be a
  		// array<double,3>, but to
  		// avoid copying, we let
  		// "R2" lie in tmp1.
  		// Camera Parameters
  		double cx = cam.cx(), cy = cam.cy(), ifx = 1 / cam.fx(),
  			   ify = 1 / cam.fy();
  		double K1 = cam.k1(), K2 = cam.k2(), p1 = cam.p1(), p2 = cam.p2(),
  			   K3 = cam.k3();
  		// First step: intrinsic matrix.
  		tmp1[0] = (pIn[0] - cx) * ifx;
  		tmp1[1] = (pIn[1] - cy) * ify;
  		J1.set_unsafe(0, 0, ifx);
  		J1.set_unsafe(1, 1, ify);
  		// Second step: adding temporary variables, related to the distortion.
  		tmp1[2] = square(tmp1[0]) + square(tmp1[1]);
  		double sK1 = square(K1);
  		double K12 = sK1 - K2;
  		double K123 = -K1 * sK1 + 2 * K1 * K2 - K3;  //-K1^3+2K1K2-K3
  		// tmp1[3]=1-K1*tmp1[2]+K12*square(tmp1[2]);
  		tmp1[3] = 1 + tmp1[2] * (-K1 + tmp1[2] * (K12 + tmp1[2] * K123));
  		J2.set_unsafe(2, 0, 2 * tmp1[0]);
  		J2.set_unsafe(2, 1, 2 * tmp1[1]);
  		double jTemp = -2 * K1 + 4 * tmp1[2] * K12 + 6 * square(tmp1[2]) * K123;
  		J2.set_unsafe(3, 0, tmp1[0] * jTemp);
  		J2.set_unsafe(3, 1, tmp1[1] * jTemp);
  		// Third step: radial distortion. Really simple, since most work has
  		// been done in the previous step.
  		tmp2[0] = tmp1[0] * tmp1[3];
  		tmp2[1] = tmp1[1] * tmp1[3];
  		J3.set_unsafe(0, 0, tmp1[3]);
  		J3.set_unsafe(0, 3, tmp1[0]);
  		J3.set_unsafe(1, 1, tmp1[3]);
  		J3.set_unsafe(1, 3, tmp1[1]);
  		// Fourth step: tangential distorion. A little more complicated, but not
  		// much more.
  		double prod = tmp2[0] * tmp2[1];
  		// References to tmp1[2] are not errors! That element is "R2".
  		pOut[0] = tmp2[0] - p1 * prod - p2 * (tmp1[2] + 2 * square(tmp2[0]));
  		pOut[1] = tmp2[1] - p1 * (tmp1[2] + 2 * square(tmp2[1])) - p2 * prod;
  		J4.set_unsafe(0, 0, 1 - p1 * tmp2[1] - 4 * p2 * tmp2[0]);
  		J4.set_unsafe(0, 1, -p1 * tmp2[0]);
  		J4.set_unsafe(0, 2, -p2);
  		J4.set_unsafe(1, 0, -p2 * tmp2[1]);
  		J4.set_unsafe(1, 1, 1 - 4 * p1 * tmp2[1] - p2 * tmp2[0]);
  		J4.set_unsafe(1, 2, -p1);
  		// As fast as possible, and without more temporaries, let the jacobian
  		// be J4*J3*J2*J1;
  		jOut.multiply_ABC(J4, J3, J2);  // Note that using the other order is
  		// not possible due to matrix sizes
  		// (jOut may, and most probably will, be
  		// fixed).
  		jOut.multiply(jOut, J1);
  	}
  
  };  // end class
  
  }  // namespace mrpt::vision
  