/* +------------------------------------------------------------------------+
   |                     Mobile Robot Programming Toolkit (MRPT)            |
   |                          https://www.mrpt.org/                         |
   |                                                                        |
   | Copyright (c) 2005-2022, Individual contributors, see AUTHORS file     |
   | See: https://www.mrpt.org/Authors - All rights reserved.               |
   | Released under BSD License. See: https://www.mrpt.org/License          |
   +------------------------------------------------------------------------+ */
#pragma once

#include <mrpt/config.h>
#include <mrpt/math/math_frwds.h>  // must be included before Eigen

#include <Eigen/Core>
#include <Eigen/Dense>

namespace mrpt
{
namespace vision
{
/** Perspective n Point (PnP) Algorithms toolkit for MRPT \ingroup
 * mrpt_vision_grp */
namespace pnp
{
/** \addtogroup pnp Perspective-n-Point pose estimation
 *  \ingroup mrpt_vision_grp
 *  @{
 */

/**
 * @class CPnP
 * @author Chandra Mangipudi
 * @date 17/08/16
 * @file pnp_algos.h
 * @brief PnP Algorithms toolkit for MRPT
 */

/** This class is used for Pose estimation from a known landmark using a
 * monocular camera.
 *  The toolkit comprises of state of the art  PnP (Perspective n Point)
 * algorithms
 *
 *  The Python Bindings pnp_perf_comp.py can be used to generate performance
 * comparison using
 *  standard tests between the different algorithms.
 *
 *  <h2> <a href="pnp_perf_comp.html">Performance comparison Results </a> </h2>
 *
 */
class CPnP
{
   public:
	/**
	 * @brief \cite hesch Direct Least Squares (DLS) - PnP : Algorithm
	 * formulates position as a function of rotation.
	 *        Use Cayley's rotation theorem to represent the rotation using
	 * parameters ($s_1, s_2, s_3$).
	 *        Solve the rotation using multi-variate polynomial expressions
	 *
	 * @param[in] obj_pts Object points in Camera Co-ordinate system {C} nX3
	 * (only 4 points used) array [p_x, p_y, p_z]
	 * @param[in] img_pts Image points in pixels nX3 (only 4 points used) array
	 * containing pixel data from camera [u, v, 1]
	 * @param[in] n number of 2D-3D correspondences
	 * @param[in] cam_intrinsic Camera Intrinsic matrix
	 * @param[out] pose_mat Output pose vector 6X1, pose_mat[0:2]-> Translation,
	 * pose_mat[3:5] -> Quaternion vector component
	 * @return success flag
	 */
	bool dls(
		const Eigen::Ref<Eigen::MatrixXd> obj_pts,
		const Eigen::Ref<Eigen::MatrixXd> img_pts, int n,
		const Eigen::Ref<Eigen::MatrixXd> cam_intrinsic,
		Eigen::Ref<Eigen::MatrixXd> pose_mat);

	/**
	 * @brief \cite lepetit Efficient-PnP (EPnP) - Algorithm takes 4 control
	 * points based on n points and uses the control points to compute the pose
	 *
	 * @param[in] obj_pts Object points in Camera Co-ordinate system {C} nX3
	 * (only 4 points used) array [p_x, p_y, p_z]
	 * @param[in] img_pts Image points in pixels nX3 (only 4 points used) array
	 * containing pixel data from camera [u, v, 1]
	 * @param[in] n number of 2D-3D correspondences
	 * @param[in] cam_intrinsic Camera Intrinsic matrix
	 * @param[out] pose_mat Output pose vector 6X1, pose_mat[0:2]-> Translation,
	 * pose_mat[3:5] -> Quaternion vector component
	 * @return success flag
	 */
	bool epnp(
		const Eigen::Ref<Eigen::MatrixXd> obj_pts,
		const Eigen::Ref<Eigen::MatrixXd> img_pts, int n,
		const Eigen::Ref<Eigen::MatrixXd> cam_intrinsic,
		Eigen::Ref<Eigen::MatrixXd> pose_mat);

	/**
	 * @brief \cite Kneip2014 Unified-PnP (UPnP) : Algorithm to compute pose
	 * from unknown camera intrinsic matrix
	 *
	 * @param[in] obj_pts Object points in Camera Co-ordinate system {C} nX3
	 * (only 4 points used) array [p_x, p_y, p_z]
	 * @param[in] img_pts Image points in pixels nX3 (only 4 points used) array
	 * containing pixel data from camera [u, v, 1]
	 * @param[in] n number of 2D-3D correspondences
	 * @param[in] cam_intrinsic Camera Intrinsic matrix
	 * @param[out] pose_mat Output pose vector 6X1, pose_mat[0:2]-> Translation,
	 * pose_mat[3:5] -> Quaternion vector component
	 * @return success flag
	 */
	bool upnp(
		const Eigen::Ref<Eigen::MatrixXd> obj_pts,
		const Eigen::Ref<Eigen::MatrixXd> img_pts, int n,
		const Eigen::Ref<Eigen::MatrixXd> cam_intrinsic,
		Eigen::Ref<Eigen::MatrixXd> pose_mat);

	/**
	 * @brief \cite kneip P3P - A closed form solution for n=3, 2D-3D
	 * correspondences
	 *
	 * @param[in] obj_pts Object points in Camera Co-ordinate system {C} nX3
	 * (only 4 points used) array [p_x, p_y, p_z]
	 * @param[in] img_pts Image points in pixels nX3 (only 4 points used) array
	 * containing pixel data from camera [u, v, 1]
	 * @param[in] n number of 2D-3D correspondences
	 * @param[in] cam_intrinsic Camera Intrinsic matrix
	 * @param[out] pose_mat Output pose vector 6X1, pose_mat[0:2]-> Translation,
	 * pose_mat[3:5] -> Quaternion vector component
	 * @return success flag
	 */
	bool p3p(
		const Eigen::Ref<Eigen::MatrixXd> obj_pts,
		const Eigen::Ref<Eigen::MatrixXd> img_pts, int n,
		const Eigen::Ref<Eigen::MatrixXd> cam_intrinsic,
		Eigen::Ref<Eigen::MatrixXd> pose_mat);

	/**
	 * @brief \cite xie Robust (R-PnP)- A closed form solution with intermediate
	 * P3P computations
	 *
	 * @param[in] obj_pts Object points in Camera Co-ordinate system {C} nX3
	 * (only 4 points used) array [p_x, p_y, p_z]
	 * @param[in] img_pts Image points in pixels nX3 (only 4 points used) array
	 * containing pixel data from camera [u, v, 1]
	 * @param[in] n number of 2D-3D correspondences
	 * @param[in] cam_intrinsic Camera Intrinsic matrix
	 * @param[out] pose_mat Output pose vector 6X1, pose_mat[0:2]-> Translation,
	 * pose_mat[3:5] -> Quaternion vector component
	 * @return success flag
	 */
	bool rpnp(
		const Eigen::Ref<Eigen::MatrixXd> obj_pts,
		const Eigen::Ref<Eigen::MatrixXd> img_pts, int n,
		const Eigen::Ref<Eigen::MatrixXd> cam_intrinsic,
		Eigen::Ref<Eigen::MatrixXd> pose_mat);

	/**
	 * @brief \cite garro Procrustes-PnP(PPnP) Algorithm : Iterative SVD based
	 * algorithm
	 * @param[in] obj_pts Object points in Camera Co-ordinate system {C} nX3
	 * array [p_x, p_y, p_z]
	 * @param[in] img_pts Image points in pixels nX3 array containing pixel data
	 * from camera [u, v, 1]
	 * @param[in] n number of 2D-3D correspondences
	 * @param[in] cam_intrinsic Camera Intrinsic matrix
	 * @param[out] pose_mat Output pose vector 6X1, pose_mat[0:2]-> Translation,
	 * pose_mat[3:5] -> Quaternion vector component
	 * @return success flag
	 */
	bool ppnp(
		const Eigen::Ref<Eigen::MatrixXd> obj_pts,
		const Eigen::Ref<Eigen::MatrixXd> img_pts, int n,
		const Eigen::Ref<Eigen::MatrixXd> cam_intrinsic,
		Eigen::Ref<Eigen::MatrixXd> pose_mat);

	/**
	 * @brief \cite dementhon Pose from Orthogoanlity and Scaling :Iterative
	 * (POSIT) -  A Geometric algorithm to compute scale and orthogonality
	 * independently
	 * @param[in] obj_pts Object points in Camera Co-ordinate system {C} nX3
	 * array [p_x, p_y, p_z]
	 * @param[in] img_pts Image points in pixels nX3 array containing pixel data
	 * from camera [u, v, 1]
	 * @param[in] n number of 2D-3D correspondences
	 * @param[in] cam_intrinsic Camera Intrinsic matrix
	 * @param[out] pose_mat Output pose vector 6X1, pose_mat[0:2]-> Translation,
	 * pose_mat[3:5] -> Quaternion vector component
	 * @return success flag
	 */
	bool posit(
		const Eigen::Ref<Eigen::MatrixXd> obj_pts,
		const Eigen::Ref<Eigen::MatrixXd> img_pts, int n,
		const Eigen::Ref<Eigen::MatrixXd> cam_intrinsic,
		Eigen::Ref<Eigen::MatrixXd> pose_mat);

	/**@brief \cite lu Lu-Hager-Mjolsness(LHM)-PnP algorithm : Iterative
	 * algorithm to reduce object space error
	 * @param[in] obj_pts   Object points in Camera Co-ordinate system {C} nX3
	 * array [p_x p_y p_z]
	 * @param[in] img_pts Image points in pixels nX3 array containing pixel data
	 * from camera [u, v, 1]
	 * @param[in] n number of 2D-3D correspondences
	 * @param[in] cam_intrinsic Camera Intrinsic matrix
	 * @param[out] pose_mat Output pose vector 6X1, pose_mat[0:2]-> Translation,
	 * pose_mat[3:5] -> Quaternion vector component
	 * @return success flag
	 */
	bool lhm(
		const Eigen::Ref<Eigen::MatrixXd> obj_pts,
		const Eigen::Ref<Eigen::MatrixXd> img_pts, int n,
		const Eigen::Ref<Eigen::MatrixXd> cam_intrinsic,
		Eigen::Ref<Eigen::MatrixXd> pose_mat);
};

/** @}  */	// end of grouping
}  // namespace pnp
}  // namespace vision
}  // namespace mrpt
