/* +------------------------------------------------------------------------+
   |                     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          |
   +------------------------------------------------------------------------+ */

#ifndef LHM_

#define LHM_

#include <iostream>

//#include <mrpt/math/types_math.h>  // Eigen must be included first via MRPT to
// enable the plugin system
#include <Eigen/Dense>
#include <Eigen/SVD>
#include <Eigen/StdVector>

#define TOL_LHM 0.00001
#define EPSILON_LHM 0.00000001

namespace mrpt::vision::pnp
{
/** \addtogroup pnp Perspective-n-Point pose estimation
 *  \ingroup mrpt_vision_grp
 *  @{
 */

/**
 * @class lhm
 * @author Chandra Mangipudi
 * @date 10/08/16
 * @file lhm.h
 * @brief Lu Hage Mjolsness - Iterative PnP Algorithm (Eigen Implementation
 */
class lhm
{
	Eigen::MatrixXd obj_pts;  //! Object points in Camera Co-ordinate system
	Eigen::MatrixXd img_pts;  //! Image points in pixel co-ordinates
	Eigen::MatrixXd cam_intrinsic;	//! Camera intrinsic matrix
	Eigen::MatrixXd P;	//! Trnaspose of Object points @obj_pts
	Eigen::MatrixXd Q;	//! Transpose of Image points @img_pts

	Eigen::Matrix3d R;	//! Matrix for internal computations
	Eigen::Matrix3d G;	//! Rotation Matrix
	Eigen::Vector3d t;	//! Translation Vector

	std::vector<Eigen::Matrix3d> F;	 //! Storage matrix for each point
	double err;	 //! Error variable for convergence selection
	double err2;  //! Error variable for convergence selection

	int n;	//! Number of 2d/3d correspondences

   public:
	//! Constructor for the LHM class
	lhm(Eigen::MatrixXd obj_pts_, Eigen::MatrixXd img_pts_,
		Eigen::MatrixXd cam_, int n0);
	//~lhm();

	/**
	 * @brief Function to compute pose using LHM PnP algorithm
	 * @param R_ Rotation matrix
	 * @param t_ Trnaslation Vector
	 * @return Success flag
	 */
	bool compute_pose(
		Eigen::Ref<Eigen::Matrix3d> R_, Eigen::Ref<Eigen::Vector3d> t_);

	/**
	 * @brief Function to compute pose during an iteration
	 */
	void absKernel();

	/**
	 * @brief Function to estimate translation given an estimated rotation
	 * matrix
	 */
	void estimate_t();

	/**
	 * @brief Transform object points in Body frame (Landmark Frame) to
	 * estimated Camera Frame
	 */
	void xform();

	/**
	 * @brief Iternal Function of quaternion
	 * @param q Quaternion
	 * @return
	 */
	Eigen::Matrix4d qMatQ(Eigen::VectorXd q);

	/**
	 * @brief Iternal Function of quaternion
	 * @param q Quaternion
	 * @return
	 */
	Eigen::Matrix4d qMatW(Eigen::VectorXd q);
};

/** @}  */	// end of grouping
}  // namespace mrpt::vision::pnp
#endif
