Go to the documentation of this file.
67 std::vector<mrpt::math::CMatrixFloat>
depth;
71 std::vector<mrpt::math::CMatrixFloat>
xx;
72 std::vector<mrpt::math::CMatrixFloat>
xx_inter;
73 std::vector<mrpt::math::CMatrixFloat>
xx_old;
75 std::vector<mrpt::math::CMatrixFloat>
yy;
76 std::vector<mrpt::math::CMatrixFloat>
yy_inter;
77 std::vector<mrpt::math::CMatrixFloat>
yy_old;
224 unsigned int& num_rows,
unsigned int& num_cols)
const
238 inline void setFOV(
float new_fovh,
float new_fovv);
241 inline void getFOV(
float& cur_fovh,
float& cur_fovv)
const
243 cur_fovh = 57.296f *
fovh;
244 cur_fovv = 57.296f *
fovv;
unsigned int rows_i
Aux variables: number of rows and cols at a given level.
void solveOneLevel()
The Solver.
std::vector< mrpt::math::CMatrixFloat > depth_warped
3D twist: 3D velocity vector (vx,vy,vz) + angular velocity (wx,wy,wz)
unsigned int m_width
Aux variables: size from which the pyramid starts to be built.
void filterLevelSolution()
Method to filter the velocity at each level of the pyramid.
mrpt::math::CMatrixFloat66 est_cov
Least squares covariance matrix.
mrpt::math::CMatrixFloat depth_wf
Matrix that stores the original depth frames with the image resolution.
mrpt::poses::CPose3D cam_oldpose
Previous camera pose.
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.
float fovv
Vertical field of view (rad)
virtual void loadFrame()=0
Virtual method to be implemented in derived classes.
Copyright (C) 2010 Hauke Strasdat Imperial College London Copyright (c) 2005-2020,...
std::vector< mrpt::math::CMatrixFloat > transformations
Transformations of the coarse-to-fine levels.
std::vector< mrpt::math::CMatrixFloat > depth_old
std::vector< mrpt::math::CMatrixFloat > xx_inter
float fovh
Camera properties:
mrpt::math::TTwist3D kai_loc
Filtered velocity in local coordinates.
unsigned int image_level
Aux varibles: levels of the image pyramid and the solver, respectively.
void setSpeedFilterEigWeight(float new_eweight)
Set the filter eigen-weight of the velocity filter.
mrpt::math::TTwist3D kai_loc_old
mrpt::math::CMatrixFloat du
Matrices that store the depth derivatives.
std::vector< mrpt::math::CMatrixFloat > yy_old
std::vector< mrpt::math::CMatrixFloat > depth_inter
void setSpeedFilterConstWeight(float new_cweight)
Set the filter constant-weight of the velocity filter.
mrpt::poses::CPose3D cam_pose
Camera poses.
float getSpeedFilterEigWeight() const
Get the filter eigen-weight of the velocity filter.
void poseUpdate()
Update camera pose and the velocities for the filter.
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
std::vector< mrpt::math::CMatrixFloat > xx_old
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.
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.
mrpt::math::TTwist3D getSolverSolution() const
Get the camera velocity (vx, vy, vz, wx, wy, wz) expressed in local reference frame estimated by the ...
mrpt::math::TTwist3D getLastSpeedAbs() const
Get the last camera velocity (vx, vy, vz, wx, wy, wz) expressed in the world reference frame,...
unsigned int num_valid_points
Num of valid points after removing null pixels.
void getWeights(mrpt::math::CMatrixFloat &we)
Get the matrix of weights.
mrpt::math::CMatrixFloat dt
unsigned int ctf_levels
Number of coarse-to-fine levels.
unsigned int cam_mode
Resolution of the images taken by the range camera.
void calculateDepthDerivatives()
Calculates the depth derivatives respect to u,v (rows and cols) and t (time)
mrpt::math::TTwist3D kai_loc_level
Solution from the solver at a given level.
void odometryCalculation()
This method performs all the necessary steps to estimate the camera velocity once the new image is re...
This abstract class implements a method called "Difodo" to perform Visual odometry with range cameras...
void performWarping()
Warp the second depth image against the first one according to the 3D transformations accumulated up ...
std::vector< mrpt::math::CMatrixFloat > yy_warped
mrpt::math::CMatrixFloat66 getCovariance() const
Get the least-square covariance matrix.
mrpt::math::CMatrixFloat weights
Weights for the range flow constraint equations in the least square solution.
mrpt::math::TTwist3D kai_abs
Last filtered velocity in absolute coordinates.
float previous_speed_const_weight
Speed filter parameters: Previous_speed_const_weight - Directly weights the previous speed in order t...
double fps
Frames per second (Hz)
std::vector< mrpt::math::CMatrixFloat > yy_inter
std::vector< mrpt::math::CMatrixFloat > depth
Matrices that store the point coordinates after downsampling.
unsigned int downsample
Downsample the image taken by the range camera.
bool fast_pyramid
Gaussian masks used to build the pyramid and flag to select accurate or fast pyramid.
void computeWeights()
This method computes the weighting fuction associated to measurement and linearization errors.
float previous_speed_eig_weight
Default 0.5.
float execution_time
Execution time (ms)
mrpt::math::CMatrixFloat44 f_mask
void buildCoordinatesPyramidFast()
void setFOV(float new_fovh, float new_fovv)
Set the horizontal and vertical field of vision (in degrees)
mrpt::math::CMatrixFloat dv
std::vector< mrpt::math::CMatrixFloat > yy
void calculateCoord()
Calculate the "average" coordinates of the points observed by the camera between two consecutive fram...
void buildCoordinatesPyramid()
Create the gaussian image pyramid according to the number of coarse-to-fine levels.
std::vector< mrpt::math::CMatrixFloat > xx
float getSpeedFilterConstWeight() const
Get the filter constant-weight of the velocity filter.
void getFOV(float &cur_fovh, float &cur_fovv) const
Get the horizontal and vertical field of vision (in degrees)
unsigned int rows
The maximum resolution that will be considered by the visual odometry method.
void getCTFLevels(unsigned int &levels) const
Get the number of coarse-to-fine levels that are considered by the visual odometry method.
std::vector< mrpt::math::CMatrixFloat > xx_warped
This template class provides the basic functionality for a general 2D any-size, resizable container o...
Page generated by Doxygen 1.8.17 for MRPT 2.0.4 at Sat Jun 27 14:00:59 UTC 2020 | |