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 | buildImagePyramid () |
Create the gaussian image pyramid according to the number of coarse-to-fine levels. More... | |
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. More... | |
void | calculateDepthDerivatives () |
Calculates the depth derivatives respect to u,v (rows and cols) and t (time) More... | |
void | findNullPoints () |
This method finds pixels whose depth is zero to subsequently discard them. 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::MatrixXi | null |
Matrix which indicates whether the depth of a pixel is zero (null = 1) or not (null = 00). More... | |
math::CMatrixFloat66 | est_cov |
Least squares covariance matrix. More... | |
float | g_mask [5][5] |
Gaussian mask used to build the pyramid. More... | |
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... | |
math::CMatrixFloat61 | kai_loc_level |
Solution from the solver at a given level. More... | |
math::CMatrixFloat61 | kai_abs |
Last filtered velocity in absolute coordinates. More... | |
math::CMatrixFloat61 | kai_loc |
Filtered velocity in local coordinates. More... | |
math::CMatrixFloat61 | kai_loc_old |
mrpt::vision::CDifodo::CDifodo | ( | ) |
|
protected |
Create the gaussian image pyramid according to the number of coarse-to-fine levels.
|
protected |
Calculate the "average" coordinates of the points observed by the camera between two consecutive frames.
|
protected |
Calculates the depth derivatives respect to u,v (rows and cols) and t (time)
|
protected |
This method computes the weighting fuction associated to measurement and linearization errors.
|
protected |
Method to filter the velocity at each level of the pyramid.
|
protected |
This method finds pixels whose depth is zero to subsequently discard them.
|
inline |
|
inline |
|
inline |
Get the depth derivatives (last level) respect to u,v and t respectively.
|
inline |
|
inline |
|
inline |
Get the coordinates of the points considered by the visual odometry method.
|
inline |
|
inline |
|
inline |
|
inline |
|
inline |
Get the matrix of weights.
|
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 mrpt::vision::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.
|
protected |
Warp the second depth image against the first one according to the 3D transformations accumulated up to a given level.
|
protected |
Update camera pose and the velocities for the filter.
|
inline |
Set the horizontal and vertical field of vision (in degrees)
|
inline |
|
inline |
|
protected |
The Solver.
It buils the overdetermined system and gets the least-square solution. It also calculates the least-square covariance matrix
unsigned int mrpt::vision::CDifodo::cam_mode |
mrpt::poses::CPose3D mrpt::vision::CDifodo::cam_oldpose |
mrpt::poses::CPose3D mrpt::vision::CDifodo::cam_pose |
|
protected |
|
protected |
|
protected |
|
protected |
|
protected |
|
protected |
unsigned int mrpt::vision::CDifodo::downsample |
|
protected |
|
protected |
float mrpt::vision::CDifodo::execution_time |
|
protected |
|
protected |
|
protected |
|
protected |
|
protected |
|
protected |
|
protected |
|
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
|
protected |
|
protected |
|
protected |
|
protected |
|
protected |
|
protected |
|
protected |
|
protected |
|
protected |
|
protected |
|
protected |
|
protected |
|
protected |
Page generated by Doxygen 1.8.12 for MRPT 1.3.2 SVN: at Thu Nov 10 13:46:27 UTC 2016 |