62 std::vector<Eigen::MatrixXf>
depth;
66 std::vector<Eigen::MatrixXf>
xx;
70 std::vector<Eigen::MatrixXf>
yy;
84 Eigen::Matrix<bool, Eigen::Dynamic, Eigen::Dynamic>
null;
194 inline void getRowsAndCols(
unsigned int &num_rows,
unsigned int &num_cols)
const {num_rows = rows; num_cols = cols;}
197 inline void getCTFLevels(
unsigned int &levels)
const {levels = ctf_levels;}
200 inline void setFOV(
float new_fovh,
float new_fovv);
203 inline void getFOV(
float &cur_fovh,
float &cur_fovv)
const {cur_fovh = 57.296*fovh; cur_fovv = 57.296*fovv;}
218 inline void getPointsCoord(Eigen::MatrixXf &x, Eigen::MatrixXf &y, Eigen::MatrixXf &z);
221 inline void getDepthDerivatives(Eigen::MatrixXf &cur_du, Eigen::MatrixXf &cur_dv, Eigen::MatrixXf &cur_dt);
A numeric matrix of compile-time fixed size.
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
This abstract class implements a method called "Difodo" to perform Visual odometry with range cameras...
void calculateDepthDerivatives()
Calculates the depth derivatives respect to u,v (rows and cols) and t (time)
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).
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.
std::vector< Eigen::MatrixXf > depth_warped
void setSpeedFilterConstWeight(float new_cweight)
Set the filter constant-weight of the velocity filter.
Eigen::Matrix< float, 6, 1 > kai_loc
Filtered velocity in local coordinates.
std::vector< Eigen::MatrixXf > depth_inter
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.
void solveOneLevel()
The Solver.
unsigned int image_level
Aux varibles: levels of the image pyramid and the solver, respectively.
std::vector< Eigen::MatrixXf > depth
Matrices that store the point coordinates after downsampling.
std::vector< Eigen::MatrixXf > xx
float execution_time
Execution time (ms)
void getPointsCoord(Eigen::MatrixXf &x, Eigen::MatrixXf &y, Eigen::MatrixXf &z)
Get the coordinates of the points considered by the visual odometry method.
Eigen::Matrix< float, 6, 6 > est_cov
Least squares covariance matrix.
float fps
Frames per second (Hz)
mrpt::math::CMatrixFloat66 getCovariance() const
Get the least-square covariance matrix.
unsigned int width
Aux variables: size from which the pyramid starts to be built.
std::vector< Eigen::MatrixXf > depth_old
Eigen::Matrix< float, 6, 1 > kai_abs
Last filtered velocity in absolute coordinates.
std::vector< Eigen::MatrixXf > transformations
Transformations of the coarse-to-fine levels.
float previous_speed_const_weight
Speed filter parameters: Previous_speed_const_weight - Directly weights the previous speed in order t...
Eigen::Matrix< float, 6, 1 > kai_loc_level
Solution from the solver at a given level.
void poseUpdate()
Update camera pose and the velocities for the filter.
std::vector< Eigen::MatrixXf > yy_warped
Eigen::MatrixXf du
Matrices that store the depth derivatives.
void performWarping()
Warp the second depth image against the first one according to the 3D transformations accumulated up ...
void computeWeights()
This method computes the weighting fuction associated to measurement and linearization errors.
void setSpeedFilterEigWeight(float new_eweight)
Set the filter eigen-weight of the velocity filter.
Eigen::Matrix< float, 6, 1 > kai_loc_old
unsigned int cam_mode
Resolution of the images taken by the range camera.
std::vector< Eigen::MatrixXf > yy_inter
std::vector< Eigen::MatrixXf > xx_old
void getWeights(Eigen::MatrixXf &we)
Get the matrix of weights.
float previous_speed_eig_weight
Default 0.5.
mrpt::math::CMatrixFloat61 getLastSpeedAbs() const
Get the last camera velocity (vx, vy, vz, wx, wy, wz) expressed in the world reference frame,...
float getSpeedFilterEigWeight() const
Get the filter eigen-weight of the velocity filter.
std::vector< Eigen::MatrixXf > xx_warped
mrpt::poses::CPose3D cam_pose
Camera poses.
float fovv
Vertical field of view (rad)
void buildCoordinatesPyramid()
Create the gaussian image pyramid according to the number of coarse-to-fine levels.
void getCTFLevels(unsigned int &levels) const
Get the number of coarse-to-fine levels that are considered by the visual odometry method.
void getFOV(float &cur_fovh, float &cur_fovv) const
Get the horizontal and vertical field of vision (in degrees)
Eigen::MatrixXf depth_wf
Matrix that stores the original depth frames with the image resolution.
bool fast_pyramid
Gaussian masks used to build the pyramid and flag to select accurate or fast pyramid.
void odometryCalculation()
This method performs all the necessary steps to estimate the camera velocity once the new image is re...
mrpt::math::CMatrixFloat61 getSolverSolution() const
Get the camera velocity (vx, vy, vz, wx, wy, wz) expressed in local reference frame estimated by the ...
Eigen::MatrixXf weights
Weights for the range flow constraint equations in the least square solution.
void buildCoordinatesPyramidFast()
void filterLevelSolution()
Method to filter the velocity at each level of the pyramid.
float fovh
Camera properties:
void setFOV(float new_fovh, float new_fovv)
Set the horizontal and vertical field of vision (in degrees)
std::vector< Eigen::MatrixXf > xx_inter
unsigned int ctf_levels
Number of coarse-to-fine levels.
std::vector< Eigen::MatrixXf > yy
unsigned int rows
The maximum resolution that will be considered by the visual odometry method.
unsigned int num_valid_points
Num of valid points after removing null pixels.
virtual void loadFrame()=0
Virtual method to be implemented in derived classes.
unsigned int downsample
Downsample the image taken by the range camera.
void calculateCoord()
Calculate the "average" coordinates of the points observed by the camera between two consecutive fram...
float getSpeedFilterConstWeight() const
Get the filter constant-weight of the velocity filter.
unsigned int rows_i
Aux variables: number of rows and cols at a given level.
std::vector< Eigen::MatrixXf > yy_old
mrpt::poses::CPose3D cam_oldpose
Previous camera pose.
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.