CDifodo.h

Go to the documentation of this file.

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

#ifndef CDifodo_H
#define CDifodo_H

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

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
     */
    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::Matrix<bool, Eigen::Dynamic, Eigen::Dynamic> null;

    /** Least squares covariance matrix */
    Eigen::Matrix<float, 6, 6> est_cov;

    /** Gaussian masks used to build the pyramid and flag to select accurate or
     * fast pyramid*/
    bool fast_pyramid;
    Eigen::Matrix4f 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 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*/
    /**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<Eigen::MatrixXf> transformations;

    /** Solution from the solver at a given level */
    Eigen::Matrix<float, 6, 1> kai_loc_level;

    /** Last filtered velocity in absolute coordinates */
    Eigen::Matrix<float, 6, 1> kai_abs;

    /** Filtered velocity in local coordinates */
    Eigen::Matrix<float, 6, 1> kai_loc;
    Eigen::Matrix<float, 6, 1> 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) */
    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 */
    /** 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.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