/* +------------------------------------------------------------------------+
   |                     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/math/CMatrixDynamic.h>
#include <mrpt/math/CMatrixFixed.h>
#include <mrpt/math/TTwist3D.h>
#include <mrpt/poses/CPose3D.h>

namespace mrpt::vision
{
/** This abstract class implements a method called "Difodo" to perform Visual
 *odometry with range cameras.
 *	It is based on the range flow equation and assumes that the scene is rigid.
 *	It can work with different image resolutions (640 x 480, 320 x 240 or 160 x
 *120) and a different number of
 * coarse-to-fine levels which can be adjusted with the member variables
 *(rows,cols,ctf_levels).
 *
 * How to use:
 * 	- A derived class must be created which defines the method
 *"loadFrame(...)"
 *according to the user application.
 *		  This method has to load the depth image into the variable "depth_wf".
 *		- Call loadFrame();
 *		- Call odometryCalculation();
 *
 *	For further information have a look at the apps:
 *    -
 *[DifOdometry-Camera](http://www.mrpt.org/list-of-mrpt-apps/application-difodometry-camera/)
 *    -
 *[DifOdometry-Datasets](http://www.mrpt.org/list-of-mrpt-apps/application-difodometry-datasets/)
 *
 *	Please refer to the respective publication when using this method:
 *		title = {Fast Visual Odometry for {3-D} Range Sensors},
 *		author = {Jaimez, Mariano and Gonzalez-Jimenez, Javier},
 *		journal = {IEEE Transactions on Robotics},
 *		volume = {31},
 *		number = {4},
 *		pages = {809 - 822},
 *		year = {2015}
 *
 * - JUN/2013: First design.
 * - JAN/2014: Integrated into MRPT library.
 * - DIC/2014: Reformulated and improved. The class now needs Eigen version
 *3.1.0 or above.
 *
 *  \sa CDifodoCamera, CDifodoDatasets
 *  \ingroup mrpt_vision_grp
 */

class CDifodo
{
   protected:
	/** Matrix that stores the original depth frames with the image resolution
	 */
	mrpt::math::CMatrixFloat depth_wf;

	/** Matrices that store the point coordinates after downsampling. */
	std::vector<mrpt::math::CMatrixFloat> depth;
	std::vector<mrpt::math::CMatrixFloat> depth_old;
	std::vector<mrpt::math::CMatrixFloat> depth_inter;
	std::vector<mrpt::math::CMatrixFloat> depth_warped;
	std::vector<mrpt::math::CMatrixFloat> xx;
	std::vector<mrpt::math::CMatrixFloat> xx_inter;
	std::vector<mrpt::math::CMatrixFloat> xx_old;
	std::vector<mrpt::math::CMatrixFloat> xx_warped;
	std::vector<mrpt::math::CMatrixFloat> yy;
	std::vector<mrpt::math::CMatrixFloat> yy_inter;
	std::vector<mrpt::math::CMatrixFloat> yy_old;
	std::vector<mrpt::math::CMatrixFloat> yy_warped;

	/** Matrices that store the depth derivatives */
	mrpt::math::CMatrixFloat du;
	mrpt::math::CMatrixFloat dv;
	mrpt::math::CMatrixFloat dt;

	/** Weights for the range flow constraint equations in the least square
	 * solution */
	mrpt::math::CMatrixFloat weights;

	/** Matrix which indicates whether the depth of a pixel is zero (null = 1)
	 * or not (null = 00).*/
	mrpt::math::CMatrixBool null;

	/** Least squares covariance matrix */
	mrpt::math::CMatrixFloat66 est_cov;

	/** Gaussian masks used to build the pyramid and flag to select accurate or
	 * fast pyramid*/
	bool fast_pyramid;
	mrpt::math::CMatrixFloat44 f_mask;
	float g_mask[5][5];

	/** Camera properties: */
	/**Horizontal field of view (rad) */
	float fovh;
	/**Vertical field of view (rad) */
	float fovv;

	/** The maximum resolution that will be considered by the visual odometry
	 * method.
	 * As a rule, the higher the resolution the slower but more accurate the
	 * method becomes.
	 * They always have to be less or equal to the size of the original depth
	 * image. */
	unsigned int rows;
	unsigned int cols;

	/** Aux variables: size from which the pyramid starts to be built */
	unsigned int m_width;
	unsigned int m_height;

	/** Aux variables: number of rows and cols at a given level */
	unsigned int rows_i;
	unsigned int cols_i;

	/** Number of coarse-to-fine levels. I has to be consistent with the number
	 * of rows and cols, because the
	 * coarsest level cannot be smaller than 15 x 20. */
	unsigned int ctf_levels;

	/** Aux varibles: levels of the image pyramid and the solver, respectively
	 */
	unsigned int image_level;
	unsigned int level;

	/** Speed filter parameters:
	 * Previous_speed_const_weight - Directly weights the previous speed in
	 * order to calculate the filtered velocity. Recommended range - (0, 0.5)
	 * Previous_speed_eig_weight - Weights the product of the corresponding
	 * eigenvalue and the previous velocity to calculate the filtered velocity*/
	/**Default 0.05 */
	float previous_speed_const_weight;
	/**Default 0.5 */
	float previous_speed_eig_weight;

	/** Transformations of the coarse-to-fine levels */
	std::vector<mrpt::math::CMatrixFloat> transformations;

	/** Solution from the solver at a given level */
	mrpt::math::TTwist3D kai_loc_level;

	/** Last filtered velocity in absolute coordinates */
	mrpt::math::TTwist3D kai_abs;

	/** Filtered velocity in local coordinates */
	mrpt::math::TTwist3D kai_loc;
	mrpt::math::TTwist3D kai_loc_old;

	/** Create the gaussian image pyramid according to the number of
	 * coarse-to-fine levels */
	void buildCoordinatesPyramid();
	void buildCoordinatesPyramidFast();

	/** Warp the second depth image against the first one according to the 3D
	 * transformations accumulated up to a given level */
	void performWarping();

	/** Calculate the "average" coordinates of the points observed by the camera
	 * between two consecutive frames and find the Null measurements */
	void calculateCoord();

	/** Calculates the depth derivatives respect to u,v (rows and cols) and t
	 * (time) */
	void calculateDepthDerivatives();

	/** This method computes the weighting fuction associated to measurement and
	 * linearization errors */
	void computeWeights();

	/** The Solver. It buils the overdetermined system and gets the least-square
	 * solution.
	 * It also calculates the least-square covariance matrix */
	void solveOneLevel();

	/** Method to filter the velocity at each level of the pyramid. */
	void filterLevelSolution();

	/** Update camera pose and the velocities for the filter */
	void poseUpdate();

   public:
	/** Frames per second (Hz) */
	double fps;

	/** Resolution of the images taken by the range camera */
	unsigned int cam_mode;	// (1 - 640 x 480, 2 - 320 x 240, 4 - 160 x 120)

	/** Downsample the image taken by the range camera. Useful to reduce the
	  computational burden,
	  * as this downsampling is applied before the gaussian pyramid is built. It
	  must be used when
		the virtual method "loadFrame()" is implemented */
	unsigned int downsample;  // (1 - original size, 2 - res/2, 4 - res/4)

	/** Num of valid points after removing null pixels*/
	unsigned int num_valid_points;

	/** Execution time (ms) */
	float execution_time;

	/** Camera poses */
	/** Last camera pose */
	mrpt::poses::CPose3D cam_pose;
	/** Previous camera pose */
	mrpt::poses::CPose3D cam_oldpose;

	/** This method performs all the necessary steps to estimate the camera
	   velocity once the new image is read,
		and updates the camera pose */
	void odometryCalculation();

	/** Get the rows and cols of the depth image that are considered by the
	 * visual odometry method. */
	inline void getRowsAndCols(
		unsigned int& num_rows, unsigned int& num_cols) const
	{
		num_rows = rows;
		num_cols = cols;
	}

	/** Get the number of coarse-to-fine levels that are considered by the
	 * visual odometry method. */
	inline void getCTFLevels(unsigned int& levels) const
	{
		levels = ctf_levels;
	}

	/** Set the horizontal and vertical field of vision (in degrees) */
	inline void setFOV(float new_fovh, float new_fovv);

	/** Get the horizontal and vertical field of vision (in degrees) */
	inline void getFOV(float& cur_fovh, float& cur_fovv) const
	{
		cur_fovh = 57.296f * fovh;
		cur_fovv = 57.296f * fovv;
	}

	/** Get the filter constant-weight of the velocity filter. */
	inline float getSpeedFilterConstWeight() const
	{
		return previous_speed_const_weight;
	}

	/** Get the filter eigen-weight of the velocity filter. */
	inline float getSpeedFilterEigWeight() const
	{
		return previous_speed_eig_weight;
	}

	/** Set the filter constant-weight of the velocity filter. */
	inline void setSpeedFilterConstWeight(float new_cweight)
	{
		previous_speed_const_weight = new_cweight;
	}

	/** Set the filter eigen-weight of the velocity filter. */
	inline void setSpeedFilterEigWeight(float new_eweight)
	{
		previous_speed_eig_weight = new_eweight;
	}

	/** Get the coordinates of the points considered by the visual odometry
	 * method */
	inline void getPointsCoord(
		mrpt::math::CMatrixFloat& x, mrpt::math::CMatrixFloat& y,
		mrpt::math::CMatrixFloat& z);

	/** Get the depth derivatives (last level) respect to u,v and t respectively
	 */
	inline void getDepthDerivatives(
		mrpt::math::CMatrixFloat& cur_du, mrpt::math::CMatrixFloat& cur_dv,
		mrpt::math::CMatrixFloat& cur_dt);

	/** Get the camera velocity (vx, vy, vz, wx, wy, wz) expressed in local
	 * reference frame estimated by the solver (before filtering) */
	inline mrpt::math::TTwist3D getSolverSolution() const
	{
		return kai_loc_level;
	}

	/** Get the last camera velocity (vx, vy, vz, wx, wy, wz) expressed in the
	 * world reference frame, obtained after filtering */
	inline mrpt::math::TTwist3D getLastSpeedAbs() const { return kai_abs; }

	/** Get the least-square covariance matrix */
	inline mrpt::math::CMatrixFloat66 getCovariance() const { return est_cov; }
	/** Get the matrix of weights */
	inline void getWeights(mrpt::math::CMatrixFloat& we);

	/** Virtual method to be implemented in derived classes.
	 * It should be used to load a new depth image into the variable depth_wf
	 */
	virtual void loadFrame() = 0;

	// Constructor. Initialize variables and matrix sizes
	CDifodo();
};
}  // namespace mrpt::vision
