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".
Call loadFrame();
Call odometryCalculation();

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 55 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 25 of file CDifodo.cpp.

References M_PIf, and mrpt::utils::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 106 of file CDifodo.cpp.

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

◆ buildCoordinatesPyramidFast()

void CDifodo::buildCoordinatesPyramidFast ( )

protected

Definition at line 252 of file CDifodo.cpp.

References mrpt::utils::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 461 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 489 of file CDifodo.cpp.

References mrpt::math::square().

◆ computeWeights()

void CDifodo::computeWeights ( )

protected

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

Definition at line 545 of file CDifodo.cpp.

References mrpt::math::square().

◆ filterLevelSolution()

void CDifodo::filterLevelSolution ( )

protected

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

Definition at line 748 of file CDifodo.cpp.

◆ getCovariance()

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

inline

Get the least-square covariance matrix.

Definition at line 230 of file CDifodo.h.

◆ 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 197 of file CDifodo.h.

◆ 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 877 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 203 of file CDifodo.h.

◆ 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 227 of file CDifodo.h.

◆ 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 866 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 194 of file CDifodo.h.

References num_cols, and num_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 224 of file CDifodo.h.

◆ getSpeedFilterConstWeight()

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

inline

Get the filter constant-weight of the velocity filter.

Definition at line 206 of file CDifodo.h.

◆ getSpeedFilterEigWeight()

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

inline

Get the filter eigen-weight of the velocity filter.

Definition at line 209 of file CDifodo.h.

◆ getWeights()

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

inline

Get the matrix of weights.

Definition at line 888 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 693 of file CDifodo.cpp.

References mrpt::utils::round(), mrpt::utils::CTicTac::Tac(), and mrpt::utils::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 363 of file CDifodo.cpp.

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

◆ poseUpdate()

void CDifodo::poseUpdate ( )

protected

Update camera pose and the velocities for the filter.

Definition at line 808 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 860 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 212 of file CDifodo.h.

◆ setSpeedFilterEigWeight()

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

inline

Set the filter eigen-weight of the velocity filter.

Definition at line 215 of file CDifodo.h.

◆ 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 639 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 172 of file CDifodo.h.

◆ cam_oldpose

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

Previous camera pose.

Definition at line 187 of file CDifodo.h.

◆ cam_pose

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

Camera poses.

Last camera pose

Definition at line 186 of file CDifodo.h.

◆ cols

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

protected

Definition at line 102 of file CDifodo.h.

◆ cols_i

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

protected

Definition at line 110 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 114 of file CDifodo.h.

◆ depth

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

protected

Matrices that store the point coordinates after downsampling.

Definition at line 62 of file CDifodo.h.

◆ depth_inter

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

protected

Definition at line 64 of file CDifodo.h.

◆ depth_old

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

protected

Definition at line 63 of file CDifodo.h.

◆ depth_warped

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

protected

Definition at line 65 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 59 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 177 of file CDifodo.h.

◆ dt

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

protected

Definition at line 78 of file CDifodo.h.

◆ du

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

protected

Matrices that store the depth derivatives.

Definition at line 76 of file CDifodo.h.

◆ dv

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

protected

Definition at line 77 of file CDifodo.h.

◆ est_cov

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

protected

Least squares covariance matrix.

Definition at line 87 of file CDifodo.h.

◆ execution_time

float mrpt::vision::CDifodo::execution_time

Execution time (ms)

Definition at line 183 of file CDifodo.h.

◆ f_mask

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

protected

Definition at line 91 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 90 of file CDifodo.h.

◆ fovh

float mrpt::vision::CDifodo::fovh

protected

Camera properties:

Horizontal field of view (rad)

Definition at line 95 of file CDifodo.h.

◆ fovv

float mrpt::vision::CDifodo::fovv

protected

Vertical field of view (rad)

Definition at line 96 of file CDifodo.h.

◆ fps

float mrpt::vision::CDifodo::fps

Frames per second (Hz)

Definition at line 169 of file CDifodo.h.

◆ g_mask

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

protected

Definition at line 92 of file CDifodo.h.

◆ height

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

protected

Definition at line 106 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 117 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 133 of file CDifodo.h.

◆ kai_loc

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

protected

Filtered velocity in local coordinates.

Definition at line 136 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 130 of file CDifodo.h.

◆ kai_loc_old

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

protected

Definition at line 137 of file CDifodo.h.

◆ level

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

protected

Definition at line 118 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 84 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 180 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 123 of file CDifodo.h.

◆ previous_speed_eig_weight

float mrpt::vision::CDifodo::previous_speed_eig_weight

protected

Default 0.5.

Definition at line 124 of file CDifodo.h.

◆ 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 101 of file CDifodo.h.

◆ rows_i

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

protected

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

Definition at line 109 of file CDifodo.h.

◆ transformations

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

protected

Transformations of the coarse-to-fine levels.

Definition at line 127 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 81 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 105 of file CDifodo.h.

◆ xx

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

protected

Definition at line 66 of file CDifodo.h.

◆ xx_inter

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

protected

Definition at line 67 of file CDifodo.h.

◆ xx_old

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

protected

Definition at line 68 of file CDifodo.h.

◆ xx_warped

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

protected

Definition at line 69 of file CDifodo.h.

◆ yy

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

protected

Definition at line 70 of file CDifodo.h.

◆ yy_inter

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

protected

Definition at line 71 of file CDifodo.h.

◆ yy_old

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

protected

Definition at line 72 of file CDifodo.h.

◆ yy_warped

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

protected

Definition at line 73 of file CDifodo.h.

Detailed Description

Public Member Functions

Public Attributes

Protected Member Functions

Protected Attributes

Constructor & Destructor Documentation

◆ CDifodo()

Member Function Documentation

◆ buildCoordinatesPyramid()

◆ buildCoordinatesPyramidFast()

◆ calculateCoord()

◆ calculateDepthDerivatives()

◆ computeWeights()

◆ filterLevelSolution()

◆ getCovariance()

◆ getCTFLevels()

◆ getDepthDerivatives()

◆ getFOV()

◆ getLastSpeedAbs()

◆ getPointsCoord()

◆ getRowsAndCols()

◆ getSolverSolution()

◆ getSpeedFilterConstWeight()

◆ getSpeedFilterEigWeight()

◆ getWeights()

◆ loadFrame()

◆ odometryCalculation()

◆ performWarping()

◆ poseUpdate()

◆ setFOV()

◆ setSpeedFilterConstWeight()

◆ setSpeedFilterEigWeight()

◆ solveOneLevel()

Member Data Documentation

◆ cam_mode

◆ cam_oldpose

◆ cam_pose

◆ cols

◆ cols_i

◆ ctf_levels

◆ depth

◆ depth_inter

◆ depth_old

◆ depth_warped

◆ depth_wf

◆ downsample

◆ dt

◆ du

◆ dv

◆ est_cov

◆ execution_time

◆ f_mask

◆ fast_pyramid

◆ fovh

◆ fovv

◆ fps

◆ g_mask

◆ height

◆ image_level

◆ kai_abs

◆ kai_loc

◆ kai_loc_level

◆ kai_loc_old

◆ level

◆ null

◆ num_valid_points

◆ previous_speed_const_weight

◆ previous_speed_eig_weight

◆ rows

◆ rows_i

◆ transformations

◆ weights

◆ width

◆ xx

◆ xx_inter

◆ xx_old

◆ xx_warped

◆ yy

◆ yy_inter

◆ yy_old