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:
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.
DIC/2014: Reformulated and improved. The class now needs Eigen version 3.1.0 or above.
#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 |
CDifodo::CDifodo | ( | ) |
Definition at line 25 of file CDifodo.cpp.
References M_PIf, and mrpt::utils::round().
|
protected |
Create the gaussian image pyramid according to the number of coarse-to-fine levels.
Definition at line 111 of file CDifodo.cpp.
References mrpt::utils::round(), and mrpt::math::sum().
|
protected |
Definition at line 270 of file CDifodo.cpp.
References mrpt::utils::round(), and mrpt::math::sum().
|
protected |
Calculate the "average" coordinates of the points observed by the camera between two consecutive frames and find the Null measurements.
Definition at line 504 of file CDifodo.cpp.
|
protected |
Calculates the depth derivatives respect to u,v (rows and cols) and t (time)
Definition at line 539 of file CDifodo.cpp.
References mrpt::math::square().
|
protected |
This method computes the weighting fuction associated to measurement and linearization errors.
Definition at line 619 of file CDifodo.cpp.
References mrpt::math::square(), z2, and z4.
|
protected |
Method to filter the velocity at each level of the pyramid.
Definition at line 873 of file CDifodo.cpp.
|
inline |
|
inline |
Get the number of coarse-to-fine levels that are considered by the visual odometry method.
Definition at line 235 of file CDifodo.h.
References ctf_levels.
|
inline |
Get the depth derivatives (last level) respect to u,v and t respectively.
Definition at line 1015 of file CDifodo.cpp.
|
inline |
|
inline |
|
inline |
Get the coordinates of the points considered by the visual odometry method.
Definition at line 1004 of file CDifodo.cpp.
|
inline |
|
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 287 of file CDifodo.h.
References kai_loc_level.
|
inline |
Get the filter constant-weight of the velocity filter.
Definition at line 251 of file CDifodo.h.
References previous_speed_const_weight.
|
inline |
Get the filter eigen-weight of the velocity filter.
Definition at line 257 of file CDifodo.h.
References previous_speed_eig_weight.
|
inline |
Get the matrix of weights.
Definition at line 1027 of file CDifodo.cpp.
|
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
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 815 of file CDifodo.cpp.
References mrpt::utils::round(), mrpt::utils::CTicTac::Tac(), and mrpt::utils::CTicTac::Tic().
|
protected |
Warp the second depth image against the first one according to the 3D transformations accumulated up to a given level.
Definition at line 387 of file CDifodo.cpp.
References mrpt::utils::round(), and mrpt::math::square().
|
protected |
Update camera pose and the velocities for the filter.
Definition at line 943 of file CDifodo.cpp.
References mrpt::poses::CPose3D::getRotationMatrix().
|
inline |
Set the horizontal and vertical field of vision (in degrees)
Definition at line 998 of file CDifodo.cpp.
References M_PI.
|
inline |
Set the filter constant-weight of the velocity filter.
Definition at line 263 of file CDifodo.h.
References previous_speed_const_weight.
|
inline |
Set the filter eigen-weight of the velocity filter.
Definition at line 269 of file CDifodo.h.
References previous_speed_eig_weight.
|
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 757 of file CDifodo.cpp.
unsigned int mrpt::vision::CDifodo::cam_mode |
mrpt::poses::CPose3D mrpt::vision::CDifodo::cam_oldpose |
mrpt::poses::CPose3D mrpt::vision::CDifodo::cam_pose |
|
protected |
Definition at line 118 of file CDifodo.h.
Referenced by getRowsAndCols().
|
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 131 of file CDifodo.h.
Referenced by getCTFLevels().
|
protected |
|
protected |
|
protected |
|
protected |
|
protected |
unsigned int mrpt::vision::CDifodo::downsample |
|
protected |
|
protected |
Least squares covariance matrix.
Definition at line 97 of file CDifodo.h.
Referenced by getCovariance().
float mrpt::vision::CDifodo::execution_time |
|
protected |
|
protected |
|
protected |
|
protected |
|
protected |
Last filtered velocity in absolute coordinates.
Definition at line 155 of file CDifodo.h.
Referenced by getLastSpeedAbs().
|
protected |
|
protected |
Solution from the solver at a given level.
Definition at line 152 of file CDifodo.h.
Referenced by getSolverSolution().
|
protected |
|
protected |
unsigned int mrpt::vision::CDifodo::num_valid_points |
|
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 144 of file CDifodo.h.
Referenced by getSpeedFilterConstWeight(), and setSpeedFilterConstWeight().
|
protected |
Default 0.5.
Definition at line 146 of file CDifodo.h.
Referenced by getSpeedFilterEigWeight(), and setSpeedFilterEigWeight().
|
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 117 of file CDifodo.h.
Referenced by getRowsAndCols().
|
protected |
|
protected |
|
protected |
|
protected |
|
protected |
|
protected |
|
protected |
|
protected |
|
protected |
|
protected |
|
protected |
Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: ae4571287 Thu Nov 23 00:06:53 2017 +0100 at dom oct 27 23:51:55 CET 2019 |