MRPT  1.9.9
List of all members | Public Member Functions | Public Attributes | Protected Member Functions | Protected Attributes
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:

For further information have a look at the apps:

JUN/2013: First design.

Definition at line 59 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 (mrpt::math::CMatrixFloat &x, mrpt::math::CMatrixFloat &y, mrpt::math::CMatrixFloat &z)
 Get the coordinates of the points considered by the visual odometry method. More...
 
void getDepthDerivatives (mrpt::math::CMatrixFloat &cur_du, mrpt::math::CMatrixFloat &cur_dv, mrpt::math::CMatrixFloat &cur_dt)
 Get the depth derivatives (last level) respect to u,v and t respectively. More...
 
mrpt::math::TTwist3D 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::TTwist3D 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 (mrpt::math::CMatrixFloat &we)
 Get the matrix of weights. More...
 
virtual void loadFrame ()=0
 Virtual method to be implemented in derived classes. More...
 
 CDifodo ()
 

Public Attributes

double 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

mrpt::math::CMatrixFloat depth_wf
 Matrix that stores the original depth frames with the image resolution. More...
 
std::vector< mrpt::math::CMatrixFloatdepth
 Matrices that store the point coordinates after downsampling. More...
 
std::vector< mrpt::math::CMatrixFloatdepth_old
 
std::vector< mrpt::math::CMatrixFloatdepth_inter
 
std::vector< mrpt::math::CMatrixFloatdepth_warped
 
std::vector< mrpt::math::CMatrixFloatxx
 
std::vector< mrpt::math::CMatrixFloatxx_inter
 
std::vector< mrpt::math::CMatrixFloatxx_old
 
std::vector< mrpt::math::CMatrixFloatxx_warped
 
std::vector< mrpt::math::CMatrixFloatyy
 
std::vector< mrpt::math::CMatrixFloatyy_inter
 
std::vector< mrpt::math::CMatrixFloatyy_old
 
std::vector< mrpt::math::CMatrixFloatyy_warped
 
mrpt::math::CMatrixFloat du
 Matrices that store the depth derivatives. More...
 
mrpt::math::CMatrixFloat dv
 
mrpt::math::CMatrixFloat dt
 
mrpt::math::CMatrixFloat weights
 Weights for the range flow constraint equations in the least square solution. More...
 
mrpt::math::CMatrixBool null
 Matrix which indicates whether the depth of a pixel is zero (null = 1) or not (null = 00). More...
 
mrpt::math::CMatrixFloat66 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...
 
mrpt::math::CMatrixFloat44 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 m_width
 Aux variables: size from which the pyramid starts to be built. More...
 
unsigned int m_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< mrpt::math::CMatrixFloattransformations
 Transformations of the coarse-to-fine levels. More...
 
mrpt::math::TTwist3D kai_loc_level
 Solution from the solver at a given level. More...
 
mrpt::math::TTwist3D kai_abs
 Last filtered velocity in absolute coordinates. More...
 
mrpt::math::TTwist3D kai_loc
 Filtered velocity in local coordinates. More...
 
mrpt::math::TTwist3D kai_loc_old
 

Constructor & Destructor Documentation

◆ CDifodo()

CDifodo::CDifodo ( )

Definition at line 27 of file CDifodo.cpp.

References M_PIf, and mrpt::round().

Here is the call graph for this function:

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

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

Here is the call graph for this function:

◆ buildCoordinatesPyramidFast()

void CDifodo::buildCoordinatesPyramidFast ( )
protected

Definition at line 272 of file CDifodo.cpp.

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

Here is the call graph for this function:

◆ 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 506 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 541 of file CDifodo.cpp.

References mrpt::square().

Here is the call graph for this function:

◆ computeWeights()

void CDifodo::computeWeights ( )
protected

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

Definition at line 621 of file CDifodo.cpp.

References mrpt::math::CMatrixFixed< T, ROWS, COLS >::cast_float(), mrpt::math::CMatrixFixed< T, ROWS, COLS >::setFromMatrixLike(), and mrpt::square().

Here is the call graph for this function:

◆ filterLevelSolution()

void CDifodo::filterLevelSolution ( )
protected

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

Definition at line 879 of file CDifodo.cpp.

References mrpt::math::CMatrixFixed< T, ROWS, COLS >::asEigen(), mrpt::math::CMatrixFixed< T, ROWS, COLS >::cast_float(), and mrpt::poses::CPose3D::getHomogeneousMatrix().

Here is the call graph for this function:

◆ getCovariance()

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

Get the least-square covariance matrix.

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

References ctf_levels.

◆ getDepthDerivatives()

void CDifodo::getDepthDerivatives ( mrpt::math::CMatrixFloat cur_du,
mrpt::math::CMatrixFloat cur_dv,
mrpt::math::CMatrixFloat cur_dt 
)
inline

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

Definition at line 1046 of file CDifodo.cpp.

References mrpt::math::CMatrixDynamic< T >::resize().

Here is the call graph for this function:

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

References fovh, and fovv.

◆ getLastSpeedAbs()

mrpt::math::TTwist3D 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 ( mrpt::math::CMatrixFloat x,
mrpt::math::CMatrixFloat y,
mrpt::math::CMatrixFloat z 
)
inline

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

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

References cols, and rows.

◆ getSolverSolution()

mrpt::math::TTwist3D 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 248 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 254 of file CDifodo.h.

References previous_speed_eig_weight.

◆ getWeights()

void CDifodo::getWeights ( mrpt::math::CMatrixFloat we)
inline

Get the matrix of weights.

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

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

Here is the call graph for this function:

◆ 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 389 of file CDifodo.cpp.

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

Here is the call graph for this function:

◆ poseUpdate()

void CDifodo::poseUpdate ( )
protected

Update camera pose and the velocities for the filter.

Definition at line 952 of file CDifodo.cpp.

References mrpt::math::CMatrixFixed< T, ROWS, COLS >::asEigen(), and mrpt::math::CMatrixFixed< T, ROWS, COLS >::cast_float().

Here is the call graph for this function:

◆ setFOV()

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

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

Definition at line 1029 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 260 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 266 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 763 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 195 of file CDifodo.h.

◆ cam_oldpose

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

Previous camera pose.

Definition at line 214 of file CDifodo.h.

◆ cam_pose

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

Camera poses.

Last camera pose

Definition at line 212 of file CDifodo.h.

◆ cols

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

Definition at line 115 of file CDifodo.h.

Referenced by getRowsAndCols().

◆ cols_i

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

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

Referenced by getCTFLevels().

◆ depth

std::vector<mrpt::math::CMatrixFloat> mrpt::vision::CDifodo::depth
protected

Matrices that store the point coordinates after downsampling.

Definition at line 67 of file CDifodo.h.

◆ depth_inter

std::vector<mrpt::math::CMatrixFloat> mrpt::vision::CDifodo::depth_inter
protected

Definition at line 69 of file CDifodo.h.

◆ depth_old

std::vector<mrpt::math::CMatrixFloat> mrpt::vision::CDifodo::depth_old
protected

Definition at line 68 of file CDifodo.h.

◆ depth_warped

std::vector<mrpt::math::CMatrixFloat> mrpt::vision::CDifodo::depth_warped
protected

Definition at line 70 of file CDifodo.h.

◆ depth_wf

mrpt::math::CMatrixFloat mrpt::vision::CDifodo::depth_wf
protected

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

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

◆ dt

mrpt::math::CMatrixFloat mrpt::vision::CDifodo::dt
protected

Definition at line 83 of file CDifodo.h.

◆ du

mrpt::math::CMatrixFloat mrpt::vision::CDifodo::du
protected

Matrices that store the depth derivatives.

Definition at line 81 of file CDifodo.h.

◆ dv

mrpt::math::CMatrixFloat mrpt::vision::CDifodo::dv
protected

Definition at line 82 of file CDifodo.h.

◆ est_cov

mrpt::math::CMatrixFloat66 mrpt::vision::CDifodo::est_cov
protected

Least squares covariance matrix.

Definition at line 94 of file CDifodo.h.

Referenced by getCovariance().

◆ execution_time

float mrpt::vision::CDifodo::execution_time

Execution time (ms)

Definition at line 208 of file CDifodo.h.

◆ f_mask

mrpt::math::CMatrixFloat44 mrpt::vision::CDifodo::f_mask
protected

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

◆ fovh

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

Camera properties:

Horizontal field of view (rad)

Definition at line 104 of file CDifodo.h.

Referenced by getFOV().

◆ fovv

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

Vertical field of view (rad)

Definition at line 106 of file CDifodo.h.

Referenced by getFOV().

◆ fps

double mrpt::vision::CDifodo::fps

Frames per second (Hz)

Definition at line 192 of file CDifodo.h.

◆ g_mask

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

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

◆ kai_abs

mrpt::math::TTwist3D mrpt::vision::CDifodo::kai_abs
protected

Last filtered velocity in absolute coordinates.

Definition at line 152 of file CDifodo.h.

Referenced by getLastSpeedAbs().

◆ kai_loc

mrpt::math::TTwist3D mrpt::vision::CDifodo::kai_loc
protected

Filtered velocity in local coordinates.

Definition at line 155 of file CDifodo.h.

◆ kai_loc_level

mrpt::math::TTwist3D mrpt::vision::CDifodo::kai_loc_level
protected

Solution from the solver at a given level.

Definition at line 149 of file CDifodo.h.

Referenced by getSolverSolution().

◆ kai_loc_old

mrpt::math::TTwist3D mrpt::vision::CDifodo::kai_loc_old
protected

Definition at line 156 of file CDifodo.h.

◆ level

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

Definition at line 133 of file CDifodo.h.

◆ m_height

unsigned int mrpt::vision::CDifodo::m_height
protected

Definition at line 119 of file CDifodo.h.

◆ m_width

unsigned int mrpt::vision::CDifodo::m_width
protected

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

Definition at line 118 of file CDifodo.h.

◆ null

mrpt::math::CMatrixBool 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 91 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 205 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 141 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 143 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 114 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 122 of file CDifodo.h.

◆ transformations

std::vector<mrpt::math::CMatrixFloat> mrpt::vision::CDifodo::transformations
protected

Transformations of the coarse-to-fine levels.

Definition at line 146 of file CDifodo.h.

◆ weights

mrpt::math::CMatrixFloat mrpt::vision::CDifodo::weights
protected

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

Definition at line 87 of file CDifodo.h.

◆ xx

std::vector<mrpt::math::CMatrixFloat> mrpt::vision::CDifodo::xx
protected

Definition at line 71 of file CDifodo.h.

◆ xx_inter

std::vector<mrpt::math::CMatrixFloat> mrpt::vision::CDifodo::xx_inter
protected

Definition at line 72 of file CDifodo.h.

◆ xx_old

std::vector<mrpt::math::CMatrixFloat> mrpt::vision::CDifodo::xx_old
protected

Definition at line 73 of file CDifodo.h.

◆ xx_warped

std::vector<mrpt::math::CMatrixFloat> mrpt::vision::CDifodo::xx_warped
protected

Definition at line 74 of file CDifodo.h.

◆ yy

std::vector<mrpt::math::CMatrixFloat> mrpt::vision::CDifodo::yy
protected

Definition at line 75 of file CDifodo.h.

◆ yy_inter

std::vector<mrpt::math::CMatrixFloat> mrpt::vision::CDifodo::yy_inter
protected

Definition at line 76 of file CDifodo.h.

◆ yy_old

std::vector<mrpt::math::CMatrixFloat> mrpt::vision::CDifodo::yy_old
protected

Definition at line 77 of file CDifodo.h.

◆ yy_warped

std::vector<mrpt::math::CMatrixFloat> mrpt::vision::CDifodo::yy_warped
protected

Definition at line 78 of file CDifodo.h.




Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: 479715d5b Tue Nov 12 07:26:21 2019 +0100 at mar nov 12 07:30:12 CET 2019