CDifodo.h

Go to the documentation of this file.

/* +------------------------------------------------------------------------+
   |                     Mobile Robot Programming Toolkit (MRPT)            |
   |                          https://www.mrpt.org/                         |
   |                                                                        |
   | Copyright (c) 2005-2020, 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