MRPT  1.9.9
mrpt::vision::CDifodo Class Referenceabstract

Detailed Description

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".

For further information have a look at the apps:

  • DifOdometry-Camera
  • 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.

    See also
    CDifodoCamera, CDifodoDatasets

Definition at line 60 of file CDifodo.h.

#include <mrpt/vision/CDifodo.h>

Public Member Functions

void odometryCalculation ()
 This method performs all the necessary steps to estimate the camera velocity once the new image is read, and updates the camera pose. More...
 
void getRowsAndCols (unsigned int &num_rows, unsigned int &num_cols) const
 Get the rows and cols of the depth image that are considered by the visual odometry method. More...
 
void getCTFLevels (unsigned int &levels) const
 Get the number of coarse-to-fine levels that are considered by the visual odometry method. More...
 
void setFOV (float new_fovh, float new_fovv)
 Set the horizontal and vertical field of vision (in degrees) More...
 
void getFOV (float &cur_fovh, float &cur_fovv) const
 Get the horizontal and vertical field of vision (in degrees) More...
 
float getSpeedFilterConstWeight () const
 Get the filter constant-weight of the velocity filter. More...
 
float getSpeedFilterEigWeight () const
 Get the filter eigen-weight of the velocity filter. More...
 
void setSpeedFilterConstWeight (float new_cweight)
 Set the filter constant-weight of the velocity filter. More...
 
void setSpeedFilterEigWeight (float new_eweight)
 Set the filter eigen-weight of the velocity filter. More...
 
void getPointsCoord (Eigen::MatrixXf &x, Eigen::MatrixXf &y, Eigen::MatrixXf &z)
 Get the coordinates of the points considered by the visual odometry method. More...
 
void getDepthDerivatives (Eigen::MatrixXf &cur_du, Eigen::MatrixXf &cur_dv, Eigen::MatrixXf &cur_dt)
 Get the depth derivatives (last level) respect to u,v and t respectively. More...
 
mrpt::math::CMatrixFloat61 getSolverSolution () const
 Get the camera velocity (vx, vy, vz, wx, wy, wz) expressed in local reference frame estimated by the solver (before filtering) More...
 
mrpt::math::CMatrixFloat61 getLastSpeedAbs () const
 Get the last camera velocity (vx, vy, vz, wx, wy, wz) expressed in the world reference frame, obtained after filtering. More...
 
mrpt::math::CMatrixFloat66 getCovariance () const
 Get the least-square covariance matrix. More...
 
void getWeights (Eigen::MatrixXf &we)
 Get the matrix of weights. More...
 
virtual void loadFrame ()=0
 Virtual method to be implemented in derived classes. More...
 
 CDifodo ()
 

Public Attributes

float fps
 Frames per second (Hz) More...
 
unsigned int cam_mode
 Resolution of the images taken by the range camera. More...
 
unsigned int downsample
 Downsample the image taken by the range camera. More...
 
unsigned int num_valid_points
 Num of valid points after removing null pixels. More...
 
float execution_time
 Execution time (ms) More...
 
mrpt::poses::CPose3D cam_pose
 Camera poses. More...
 
mrpt::poses::CPose3D cam_oldpose
 Previous camera pose. More...
 

Protected Member Functions

void buildCoordinatesPyramid ()
 Create the gaussian image pyramid according to the number of coarse-to-fine levels. More...
 
void buildCoordinatesPyramidFast ()
 
void performWarping ()
 Warp the second depth image against the first one according to the 3D transformations accumulated up to a given level. More...
 
void calculateCoord ()
 Calculate the "average" coordinates of the points observed by the camera between two consecutive frames and find the Null measurements. More...
 
void calculateDepthDerivatives ()
 Calculates the depth derivatives respect to u,v (rows and cols) and t (time) More...
 
void computeWeights ()
 This method computes the weighting fuction associated to measurement and linearization errors. More...
 
void solveOneLevel ()
 The Solver. More...
 
void filterLevelSolution ()
 Method to filter the velocity at each level of the pyramid. More...
 
void poseUpdate ()
 Update camera pose and the velocities for the filter. More...
 

Protected Attributes

Eigen::MatrixXf depth_wf
 Matrix that stores the original depth frames with the image resolution. More...
 
std::vector< Eigen::MatrixXf > depth
 Matrices that store the point coordinates after downsampling. More...
 
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
 
Eigen::MatrixXf du
 Matrices that store the depth derivatives. More...
 
Eigen::MatrixXf dv
 
Eigen::MatrixXf dt
 
Eigen::MatrixXf weights
 Weights for the range flow constraint equations in the least square solution. More...
 
Eigen::Matrix< bool, Eigen::Dynamic, Eigen::Dynamic > null
 Matrix which indicates whether the depth of a pixel is zero (null = 1) or not (null = 00). More...
 
Eigen::Matrix< float, 6, 6 > est_cov
 Least squares covariance matrix. More...
 
bool fast_pyramid
 Gaussian masks used to build the pyramid and flag to select accurate or fast pyramid. More...
 
Eigen::Matrix4f f_mask
 
float g_mask [5][5]
 
float fovh
 Camera properties: More...
 
float fovv
 Vertical field of view (rad) More...
 
unsigned int rows
 The maximum resolution that will be considered by the visual odometry method. More...
 
unsigned int cols
 
unsigned int width
 Aux variables: size from which the pyramid starts to be built. More...
 
unsigned int height
 
unsigned int rows_i
 Aux variables: number of rows and cols at a given level. More...
 
unsigned int cols_i
 
unsigned int ctf_levels
 Number of coarse-to-fine levels. More...
 
unsigned int image_level
 Aux varibles: levels of the image pyramid and the solver, respectively. More...
 
unsigned int level
 
float previous_speed_const_weight
 Speed filter parameters: Previous_speed_const_weight - Directly weights the previous speed in order to calculate the filtered velocity. More...
 
float previous_speed_eig_weight
 Default 0.5. More...
 
std::vector< Eigen::MatrixXf > transformations
 Transformations of the coarse-to-fine levels. More...
 
Eigen::Matrix< float, 6, 1 > kai_loc_level
 Solution from the solver at a given level. More...
 
Eigen::Matrix< float, 6, 1 > kai_abs
 Last filtered velocity in absolute coordinates. More...
 
Eigen::Matrix< float, 6, 1 > kai_loc
 Filtered velocity in local coordinates. More...
 
Eigen::Matrix< float, 6, 1 > kai_loc_old
 

Constructor & Destructor Documentation

◆ CDifodo()

CDifodo::CDifodo ( )

Definition at line 24 of file CDifodo.cpp.

References M_PIf, and mrpt::round().

Member Function Documentation

◆ buildCoordinatesPyramid()

void CDifodo::buildCoordinatesPyramid ( )
protected

Create the gaussian image pyramid according to the number of coarse-to-fine levels.

Definition at line 110 of file CDifodo.cpp.

References mrpt::round(), and mrpt::math::sum().

◆ buildCoordinatesPyramidFast()

void CDifodo::buildCoordinatesPyramidFast ( )
protected

Definition at line 269 of file CDifodo.cpp.

References mrpt::round(), and mrpt::math::sum().

◆ calculateCoord()

void CDifodo::calculateCoord ( )
protected

Calculate the "average" coordinates of the points observed by the camera between two consecutive frames and find the Null measurements.

Definition at line 503 of file CDifodo.cpp.

◆ calculateDepthDerivatives()

void CDifodo::calculateDepthDerivatives ( )
protected

Calculates the depth derivatives respect to u,v (rows and cols) and t (time)

Definition at line 538 of file CDifodo.cpp.

References mrpt::square().

◆ computeWeights()

void CDifodo::computeWeights ( )
protected

This method computes the weighting fuction associated to measurement and linearization errors.

Definition at line 618 of file CDifodo.cpp.

References mrpt::square().

◆ filterLevelSolution()

void CDifodo::filterLevelSolution ( )
protected

Method to filter the velocity at each level of the pyramid.

Definition at line 872 of file CDifodo.cpp.

References mrpt::poses::CPose3D::exp(), and mrpt::poses::CPose3D::getHomogeneousMatrix().

◆ getCovariance()

mrpt::math::CMatrixFloat66 mrpt::vision::CDifodo::getCovariance ( ) const
inline

Get the least-square covariance matrix.

Definition at line 298 of file CDifodo.h.

References est_cov.

◆ getCTFLevels()

void mrpt::vision::CDifodo::getCTFLevels ( unsigned int &  levels) const
inline

Get the number of coarse-to-fine levels that are considered by the visual odometry method.

Definition at line 233 of file CDifodo.h.

References ctf_levels.

◆ getDepthDerivatives()

void CDifodo::getDepthDerivatives ( Eigen::MatrixXf &  cur_du,
Eigen::MatrixXf &  cur_dv,
Eigen::MatrixXf &  cur_dt 
)
inline

Get the depth derivatives (last level) respect to u,v and t respectively.

Definition at line 1014 of file CDifodo.cpp.

◆ getFOV()

void mrpt::vision::CDifodo::getFOV ( float &  cur_fovh,
float &  cur_fovv 
) const
inline

Get the horizontal and vertical field of vision (in degrees)

Definition at line 242 of file CDifodo.h.

References fovh, and fovv.

◆ getLastSpeedAbs()

mrpt::math::CMatrixFloat61 mrpt::vision::CDifodo::getLastSpeedAbs ( ) const
inline

Get the last camera velocity (vx, vy, vz, wx, wy, wz) expressed in the world reference frame, obtained after filtering.

Definition at line 292 of file CDifodo.h.

References kai_abs.

◆ getPointsCoord()

void CDifodo::getPointsCoord ( Eigen::MatrixXf &  x,
Eigen::MatrixXf &  y,
Eigen::MatrixXf &  z 
)
inline

Get the coordinates of the points considered by the visual odometry method.

Definition at line 1003 of file CDifodo.cpp.

◆ getRowsAndCols()

void mrpt::vision::CDifodo::getRowsAndCols ( unsigned int &  num_rows,
unsigned int &  num_cols 
) const
inline

Get the rows and cols of the depth image that are considered by the visual odometry method.

Definition at line 224 of file CDifodo.h.

References cols, and rows.

◆ getSolverSolution()

mrpt::math::CMatrixFloat61 mrpt::vision::CDifodo::getSolverSolution ( ) const
inline

Get the camera velocity (vx, vy, vz, wx, wy, wz) expressed in local reference frame estimated by the solver (before filtering)

Definition at line 285 of file CDifodo.h.

References kai_loc_level.

◆ getSpeedFilterConstWeight()

float mrpt::vision::CDifodo::getSpeedFilterConstWeight ( ) const
inline

Get the filter constant-weight of the velocity filter.

Definition at line 249 of file CDifodo.h.

References previous_speed_const_weight.

◆ getSpeedFilterEigWeight()

float mrpt::vision::CDifodo::getSpeedFilterEigWeight ( ) const
inline

Get the filter eigen-weight of the velocity filter.

Definition at line 255 of file CDifodo.h.

References previous_speed_eig_weight.

◆ getWeights()

void CDifodo::getWeights ( Eigen::MatrixXf &  we)
inline

Get the matrix of weights.

Definition at line 1026 of file CDifodo.cpp.

◆ loadFrame()

virtual void mrpt::vision::CDifodo::loadFrame ( )
pure virtual

Virtual method to be implemented in derived classes.

It should be used to load a new depth image into the variable depth_wf

◆ odometryCalculation()

void CDifodo::odometryCalculation ( )

This method performs all the necessary steps to estimate the camera velocity once the new image is read, and updates the camera pose.

Definition at line 814 of file CDifodo.cpp.

References mrpt::round(), mrpt::system::CTicTac::Tac(), and mrpt::system::CTicTac::Tic().

◆ performWarping()

void CDifodo::performWarping ( )
protected

Warp the second depth image against the first one according to the 3D transformations accumulated up to a given level.

Definition at line 386 of file CDifodo.cpp.

References mrpt::round(), and mrpt::square().

◆ poseUpdate()

void CDifodo::poseUpdate ( )
protected

Update camera pose and the velocities for the filter.

Definition at line 942 of file CDifodo.cpp.

References mrpt::poses::CPose3D::getRotationMatrix().

◆ setFOV()

void CDifodo::setFOV ( float  new_fovh,
float  new_fovv 
)
inline

Set the horizontal and vertical field of vision (in degrees)

Definition at line 997 of file CDifodo.cpp.

References M_PI.

◆ setSpeedFilterConstWeight()

void mrpt::vision::CDifodo::setSpeedFilterConstWeight ( float  new_cweight)
inline

Set the filter constant-weight of the velocity filter.

Definition at line 261 of file CDifodo.h.

References previous_speed_const_weight.

◆ setSpeedFilterEigWeight()

void mrpt::vision::CDifodo::setSpeedFilterEigWeight ( float  new_eweight)
inline

Set the filter eigen-weight of the velocity filter.

Definition at line 267 of file CDifodo.h.

References previous_speed_eig_weight.

◆ solveOneLevel()

void CDifodo::solveOneLevel ( )
protected

The Solver.

It buils the overdetermined system and gets the least-square solution. It also calculates the least-square covariance matrix

Definition at line 756 of file CDifodo.cpp.

Member Data Documentation

◆ cam_mode

unsigned int mrpt::vision::CDifodo::cam_mode

Resolution of the images taken by the range camera.

Definition at line 196 of file CDifodo.h.

◆ cam_oldpose

mrpt::poses::CPose3D mrpt::vision::CDifodo::cam_oldpose

Previous camera pose.

Definition at line 215 of file CDifodo.h.

◆ cam_pose

mrpt::poses::CPose3D mrpt::vision::CDifodo::cam_pose

Camera poses.

Last camera pose

Definition at line 213 of file CDifodo.h.

◆ cols

unsigned int mrpt::vision::CDifodo::cols
protected

Definition at line 116 of file CDifodo.h.

Referenced by getRowsAndCols().

◆ cols_i

unsigned int mrpt::vision::CDifodo::cols_i
protected

Definition at line 124 of file CDifodo.h.

◆ ctf_levels

unsigned int mrpt::vision::CDifodo::ctf_levels
protected

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.

Definition at line 129 of file CDifodo.h.

Referenced by getCTFLevels().

◆ depth

std::vector<Eigen::MatrixXf> mrpt::vision::CDifodo::depth
protected

Matrices that store the point coordinates after downsampling.

Definition at line 68 of file CDifodo.h.

◆ depth_inter

std::vector<Eigen::MatrixXf> mrpt::vision::CDifodo::depth_inter
protected

Definition at line 70 of file CDifodo.h.

◆ depth_old

std::vector<Eigen::MatrixXf> mrpt::vision::CDifodo::depth_old
protected

Definition at line 69 of file CDifodo.h.

◆ depth_warped

std::vector<Eigen::MatrixXf> mrpt::vision::CDifodo::depth_warped
protected

Definition at line 71 of file CDifodo.h.

◆ depth_wf

Eigen::MatrixXf mrpt::vision::CDifodo::depth_wf
protected

Matrix that stores the original depth frames with the image resolution.

Definition at line 65 of file CDifodo.h.

◆ downsample

unsigned int mrpt::vision::CDifodo::downsample

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

Definition at line 203 of file CDifodo.h.

◆ dt

Eigen::MatrixXf mrpt::vision::CDifodo::dt
protected

Definition at line 84 of file CDifodo.h.

◆ du

Eigen::MatrixXf mrpt::vision::CDifodo::du
protected

Matrices that store the depth derivatives.

Definition at line 82 of file CDifodo.h.

◆ dv

Eigen::MatrixXf mrpt::vision::CDifodo::dv
protected

Definition at line 83 of file CDifodo.h.

◆ est_cov

Eigen::Matrix<float, 6, 6> mrpt::vision::CDifodo::est_cov
protected

Least squares covariance matrix.

Definition at line 95 of file CDifodo.h.

Referenced by getCovariance().

◆ execution_time

float mrpt::vision::CDifodo::execution_time

Execution time (ms)

Definition at line 209 of file CDifodo.h.

◆ f_mask

Eigen::Matrix4f mrpt::vision::CDifodo::f_mask
protected

Definition at line 100 of file CDifodo.h.

◆ fast_pyramid

bool mrpt::vision::CDifodo::fast_pyramid
protected

Gaussian masks used to build the pyramid and flag to select accurate or fast pyramid.

Definition at line 99 of file CDifodo.h.

◆ fovh

float mrpt::vision::CDifodo::fovh
protected

Camera properties:

Horizontal field of view (rad)

Definition at line 105 of file CDifodo.h.

Referenced by getFOV().

◆ fovv

float mrpt::vision::CDifodo::fovv
protected

Vertical field of view (rad)

Definition at line 107 of file CDifodo.h.

Referenced by getFOV().

◆ fps

float mrpt::vision::CDifodo::fps

Frames per second (Hz)

Definition at line 193 of file CDifodo.h.

◆ g_mask

float mrpt::vision::CDifodo::g_mask[5][5]
protected

Definition at line 101 of file CDifodo.h.

◆ height

unsigned int mrpt::vision::CDifodo::height
protected

Definition at line 120 of file CDifodo.h.

◆ image_level

unsigned int mrpt::vision::CDifodo::image_level
protected

Aux varibles: levels of the image pyramid and the solver, respectively.

Definition at line 133 of file CDifodo.h.

◆ kai_abs

Eigen::Matrix<float, 6, 1> mrpt::vision::CDifodo::kai_abs
protected

Last filtered velocity in absolute coordinates.

Definition at line 153 of file CDifodo.h.

Referenced by getLastSpeedAbs().

◆ kai_loc

Eigen::Matrix<float, 6, 1> mrpt::vision::CDifodo::kai_loc
protected

Filtered velocity in local coordinates.

Definition at line 156 of file CDifodo.h.

◆ kai_loc_level

Eigen::Matrix<float, 6, 1> mrpt::vision::CDifodo::kai_loc_level
protected

Solution from the solver at a given level.

Definition at line 150 of file CDifodo.h.

Referenced by getSolverSolution().

◆ kai_loc_old

Eigen::Matrix<float, 6, 1> mrpt::vision::CDifodo::kai_loc_old
protected

Definition at line 157 of file CDifodo.h.

◆ level

unsigned int mrpt::vision::CDifodo::level
protected

Definition at line 134 of file CDifodo.h.

◆ null

Eigen::Matrix<bool, Eigen::Dynamic, Eigen::Dynamic> mrpt::vision::CDifodo::null
protected

Matrix which indicates whether the depth of a pixel is zero (null = 1) or not (null = 00).

Definition at line 92 of file CDifodo.h.

◆ num_valid_points

unsigned int mrpt::vision::CDifodo::num_valid_points

Num of valid points after removing null pixels.

Definition at line 206 of file CDifodo.h.

◆ previous_speed_const_weight

float mrpt::vision::CDifodo::previous_speed_const_weight
protected

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

Definition at line 142 of file CDifodo.h.

Referenced by getSpeedFilterConstWeight(), and setSpeedFilterConstWeight().

◆ previous_speed_eig_weight

float mrpt::vision::CDifodo::previous_speed_eig_weight
protected

Default 0.5.

Definition at line 144 of file CDifodo.h.

Referenced by getSpeedFilterEigWeight(), and setSpeedFilterEigWeight().

◆ rows

unsigned int mrpt::vision::CDifodo::rows
protected

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.

Definition at line 115 of file CDifodo.h.

Referenced by getRowsAndCols().

◆ rows_i

unsigned int mrpt::vision::CDifodo::rows_i
protected

Aux variables: number of rows and cols at a given level.

Definition at line 123 of file CDifodo.h.

◆ transformations

std::vector<Eigen::MatrixXf> mrpt::vision::CDifodo::transformations
protected

Transformations of the coarse-to-fine levels.

Definition at line 147 of file CDifodo.h.

◆ weights

Eigen::MatrixXf mrpt::vision::CDifodo::weights
protected

Weights for the range flow constraint equations in the least square solution.

Definition at line 88 of file CDifodo.h.

◆ width

unsigned int mrpt::vision::CDifodo::width
protected

Aux variables: size from which the pyramid starts to be built.

Definition at line 119 of file CDifodo.h.

◆ xx

std::vector<Eigen::MatrixXf> mrpt::vision::CDifodo::xx
protected

Definition at line 72 of file CDifodo.h.

◆ xx_inter

std::vector<Eigen::MatrixXf> mrpt::vision::CDifodo::xx_inter
protected

Definition at line 73 of file CDifodo.h.

◆ xx_old

std::vector<Eigen::MatrixXf> mrpt::vision::CDifodo::xx_old
protected

Definition at line 74 of file CDifodo.h.

◆ xx_warped

std::vector<Eigen::MatrixXf> mrpt::vision::CDifodo::xx_warped
protected

Definition at line 75 of file CDifodo.h.

◆ yy

std::vector<Eigen::MatrixXf> mrpt::vision::CDifodo::yy
protected

Definition at line 76 of file CDifodo.h.

◆ yy_inter

std::vector<Eigen::MatrixXf> mrpt::vision::CDifodo::yy_inter
protected

Definition at line 77 of file CDifodo.h.

◆ yy_old

std::vector<Eigen::MatrixXf> mrpt::vision::CDifodo::yy_old
protected

Definition at line 78 of file CDifodo.h.

◆ yy_warped

std::vector<Eigen::MatrixXf> mrpt::vision::CDifodo::yy_warped
protected

Definition at line 79 of file CDifodo.h.




Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: 7d5e6d718 Fri Aug 24 01:51:28 2018 +0200 at lun nov 2 08:35:50 CET 2020