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

#ifndef CDifodo_H
#define CDifodo_H

#include <mrpt/utils/types_math.h> // Eigen
#include <mrpt/math/CMatrixFixedNumeric.h>
#include <mrpt/poses/CPose3D.h>
#include <mrpt/vision/link_pragmas.h>
#include <unsupported/Eigen/MatrixFunctions>

namespace mrpt
{
	namespace 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 VISION_IMPEXP CDifodo {
		protected:

			/** Matrix that stores the original depth frames with the image resolution */
			Eigen::MatrixXf depth_wf;

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

			/** Matrices that store the depth derivatives */
			Eigen::MatrixXf du;
			Eigen::MatrixXf dv;
			Eigen::MatrixXf dt;

			/** Weights for the range flow constraint equations in the least square solution */
			Eigen::MatrixXf weights;

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

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

			/** Gaussian mask used to build the pyramid */
			float g_mask[5][5];

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

			/** 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 width;
			unsigned int 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*/
			float previous_speed_const_weight;	//!<Default 0.05
			float previous_speed_eig_weight;	//!<Default 0.5

			/** Transformations of the coarse-to-fine levels */
			std::vector<Eigen::MatrixXf> transformations;
			
			/** Solution from the solver at a given level */
			math::CMatrixFloat61 kai_loc_level;

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

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

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

			/** 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 */
			void calculateCoord();

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

			/** This method finds pixels whose depth is zero to subsequently discard them */
			void findNullPoints();

			/** 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) */
			float 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 */
			mrpt::poses::CPose3D cam_pose;		//!< Last camera pose
			mrpt::poses::CPose3D cam_oldpose;	//!< Previous camera pose

			/** 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.296*fovh; cur_fovv = 57.296*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(Eigen::MatrixXf &x, Eigen::MatrixXf &y, Eigen::MatrixXf &z);

			/** Get the depth derivatives (last level) respect to u,v and t respectively */
			inline void getDepthDerivatives(Eigen::MatrixXf &cur_du, Eigen::MatrixXf &cur_dv, Eigen::MatrixXf &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::CMatrixFloat61 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::CMatrixFloat61 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(Eigen::MatrixXf &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();

		};
	}
}



#endif
