Main MRPT website > C++ reference for MRPT 1.4.0
CDifodo.h
Go to the documentation of this file.
1/* +---------------------------------------------------------------------------+
2 | Mobile Robot Programming Toolkit (MRPT) |
3 | http://www.mrpt.org/ |
4 | |
5 | Copyright (c) 2005-2016, Individual contributors, see AUTHORS file |
6 | See: http://www.mrpt.org/Authors - All rights reserved. |
7 | Released under BSD License. See details in http://www.mrpt.org/License |
8 +---------------------------------------------------------------------------+ */
9
10#ifndef CDifodo_H
11#define CDifodo_H
12
13#include <mrpt/utils/types_math.h> // Eigen
15#include <mrpt/poses/CPose3D.h>
17//#include <unsupported/Eigen/MatrixFunctions>
18
19namespace mrpt
20{
21 namespace vision
22 {
23 /** This abstract class implements a method called "Difodo" to perform Visual odometry with range cameras.
24 * It is based on the range flow equation and assumes that the scene is rigid.
25 * It can work with different image resolutions (640 x 480, 320 x 240 or 160 x 120) and a different number of
26 * coarse-to-fine levels which can be adjusted with the member variables (rows,cols,ctf_levels).
27 *
28 * How to use:
29 * - A derived class must be created which defines the method "loadFrame(...)" according to the user application.
30 * This method has to load the depth image into the variable "depth_wf".
31 * - Call loadFrame();
32 * - Call odometryCalculation();
33 *
34 * For further information have a look at the apps:
35 * - [DifOdometry-Camera](http://www.mrpt.org/list-of-mrpt-apps/application-difodometry-camera/)
36 * - [DifOdometry-Datasets](http://www.mrpt.org/list-of-mrpt-apps/application-difodometry-datasets/)
37 *
38 * Please refer to the respective publication when using this method:
39 * title = {Fast Visual Odometry for {3-D} Range Sensors},
40 * author = {Jaimez, Mariano and Gonzalez-Jimenez, Javier},
41 * journal = {IEEE Transactions on Robotics},
42 * volume = {31},
43 * number = {4},
44 * pages = {809 - 822},
45 * year = {2015}
46 *
47 * - JUN/2013: First design.
48 * - JAN/2014: Integrated into MRPT library.
49 * - DIC/2014: Reformulated and improved. The class now needs Eigen version 3.1.0 or above.
50 *
51 * \sa CDifodoCamera, CDifodoDatasets
52 * \ingroup mrpt_vision_grp
53 */
54
56 protected:
57
58 /** Matrix that stores the original depth frames with the image resolution */
59 Eigen::MatrixXf depth_wf;
60
61 /** Matrices that store the point coordinates after downsampling. */
62 std::vector<Eigen::MatrixXf> depth;
63 std::vector<Eigen::MatrixXf> depth_old;
64 std::vector<Eigen::MatrixXf> depth_inter;
65 std::vector<Eigen::MatrixXf> depth_warped;
66 std::vector<Eigen::MatrixXf> xx;
67 std::vector<Eigen::MatrixXf> xx_inter;
68 std::vector<Eigen::MatrixXf> xx_old;
69 std::vector<Eigen::MatrixXf> xx_warped;
70 std::vector<Eigen::MatrixXf> yy;
71 std::vector<Eigen::MatrixXf> yy_inter;
72 std::vector<Eigen::MatrixXf> yy_old;
73 std::vector<Eigen::MatrixXf> yy_warped;
74
75 /** Matrices that store the depth derivatives */
76 Eigen::MatrixXf du;
77 Eigen::MatrixXf dv;
78 Eigen::MatrixXf dt;
79
80 /** Weights for the range flow constraint equations in the least square solution */
81 Eigen::MatrixXf weights;
82
83 /** Matrix which indicates whether the depth of a pixel is zero (null = 1) or not (null = 00).*/
84 Eigen::Matrix<bool, Eigen::Dynamic, Eigen::Dynamic> null;
85
86 /** Least squares covariance matrix */
87 Eigen::Matrix<float, 6, 6> est_cov;
88
89 /** Gaussian masks used to build the pyramid and flag to select accurate or fast pyramid*/
91 Eigen::Matrix4f f_mask;
92 float g_mask[5][5];
93
94 /** Camera properties: */
95 float fovh; //!<Horizontal field of view (rad)
96 float fovv; //!<Vertical field of view (rad)
97
98 /** The maximum resolution that will be considered by the visual odometry method.
99 * As a rule, the higher the resolution the slower but more accurate the method becomes.
100 * They always have to be less or equal to the size of the original depth image. */
101 unsigned int rows;
102 unsigned int cols;
103
104 /** Aux variables: size from which the pyramid starts to be built */
105 unsigned int width;
106 unsigned int height;
107
108 /** Aux variables: number of rows and cols at a given level */
109 unsigned int rows_i;
110 unsigned int cols_i;
111
112 /** Number of coarse-to-fine levels. I has to be consistent with the number of rows and cols, because the
113 * coarsest level cannot be smaller than 15 x 20. */
114 unsigned int ctf_levels;
115
116 /** Aux varibles: levels of the image pyramid and the solver, respectively */
117 unsigned int image_level;
118 unsigned int level;
119
120 /** Speed filter parameters:
121 * Previous_speed_const_weight - Directly weights the previous speed in order to calculate the filtered velocity. Recommended range - (0, 0.5)
122 * Previous_speed_eig_weight - Weights the product of the corresponding eigenvalue and the previous velocity to calculate the filtered velocity*/
123 float previous_speed_const_weight; //!<Default 0.05
124 float previous_speed_eig_weight; //!<Default 0.5
125
126 /** Transformations of the coarse-to-fine levels */
127 std::vector<Eigen::MatrixXf> transformations;
128
129 /** Solution from the solver at a given level */
130 Eigen::Matrix<float, 6, 1> kai_loc_level;
131
132 /** Last filtered velocity in absolute coordinates */
133 Eigen::Matrix<float,6,1> kai_abs;
134
135 /** Filtered velocity in local coordinates */
136 Eigen::Matrix<float,6,1> kai_loc;
137 Eigen::Matrix<float,6,1> kai_loc_old;
138
139 /** Create the gaussian image pyramid according to the number of coarse-to-fine levels */
142
143 /** Warp the second depth image against the first one according to the 3D transformations accumulated up to a given level */
145
146 /** Calculate the "average" coordinates of the points observed by the camera between two consecutive frames and find the Null measurements */
148
149 /** Calculates the depth derivatives respect to u,v (rows and cols) and t (time) */
151
152 /** This method computes the weighting fuction associated to measurement and linearization errors */
154
155 /** The Solver. It buils the overdetermined system and gets the least-square solution.
156 * It also calculates the least-square covariance matrix */
158
159 /** Method to filter the velocity at each level of the pyramid. */
161
162 /** Update camera pose and the velocities for the filter */
164
165
166 public:
167
168 /** Frames per second (Hz) */
169 float fps;
170
171 /** Resolution of the images taken by the range camera */
172 unsigned int cam_mode; // (1 - 640 x 480, 2 - 320 x 240, 4 - 160 x 120)
173
174 /** Downsample the image taken by the range camera. Useful to reduce the computational burden,
175 * as this downsampling is applied before the gaussian pyramid is built. It must be used when
176 the virtual method "loadFrame()" is implemented */
177 unsigned int downsample; // (1 - original size, 2 - res/2, 4 - res/4)
178
179 /** Num of valid points after removing null pixels*/
180 unsigned int num_valid_points;
181
182 /** Execution time (ms) */
184
185 /** Camera poses */
186 mrpt::poses::CPose3D cam_pose; //!< Last camera pose
187 mrpt::poses::CPose3D cam_oldpose; //!< Previous camera pose
188
189 /** This method performs all the necessary steps to estimate the camera velocity once the new image is read,
190 and updates the camera pose */
192
193 /** Get the rows and cols of the depth image that are considered by the visual odometry method. */
194 inline void getRowsAndCols(unsigned int &num_rows, unsigned int &num_cols) const {num_rows = rows; num_cols = cols;}
195
196 /** Get the number of coarse-to-fine levels that are considered by the visual odometry method. */
197 inline void getCTFLevels(unsigned int &levels) const {levels = ctf_levels;}
198
199 /** Set the horizontal and vertical field of vision (in degrees) */
200 inline void setFOV(float new_fovh, float new_fovv);
201
202 /** Get the horizontal and vertical field of vision (in degrees) */
203 inline void getFOV(float &cur_fovh, float &cur_fovv) const {cur_fovh = 57.296*fovh; cur_fovv = 57.296*fovv;}
204
205 /** Get the filter constant-weight of the velocity filter. */
206 inline float getSpeedFilterConstWeight() const {return previous_speed_const_weight;}
207
208 /** Get the filter eigen-weight of the velocity filter. */
209 inline float getSpeedFilterEigWeight() const {return previous_speed_eig_weight;}
210
211 /** Set the filter constant-weight of the velocity filter. */
212 inline void setSpeedFilterConstWeight(float new_cweight) { previous_speed_const_weight = new_cweight;}
213
214 /** Set the filter eigen-weight of the velocity filter. */
215 inline void setSpeedFilterEigWeight(float new_eweight) { previous_speed_eig_weight = new_eweight;}
216
217 /** Get the coordinates of the points considered by the visual odometry method */
218 inline void getPointsCoord(Eigen::MatrixXf &x, Eigen::MatrixXf &y, Eigen::MatrixXf &z);
219
220 /** Get the depth derivatives (last level) respect to u,v and t respectively */
221 inline void getDepthDerivatives(Eigen::MatrixXf &cur_du, Eigen::MatrixXf &cur_dv, Eigen::MatrixXf &cur_dt);
222
223 /** Get the camera velocity (vx, vy, vz, wx, wy, wz) expressed in local reference frame estimated by the solver (before filtering) */
224 inline mrpt::math::CMatrixFloat61 getSolverSolution() const {return kai_loc_level;}
225
226 /** Get the last camera velocity (vx, vy, vz, wx, wy, wz) expressed in the world reference frame, obtained after filtering */
227 inline mrpt::math::CMatrixFloat61 getLastSpeedAbs() const {return kai_abs;}
228
229 /** Get the least-square covariance matrix */
230 inline mrpt::math::CMatrixFloat66 getCovariance() const {return est_cov;}
231
232 /** Get the matrix of weights */
233 inline void getWeights(Eigen::MatrixXf &we);
234
235 /** Virtual method to be implemented in derived classes.
236 * It should be used to load a new depth image into the variable depth_wf */
237 virtual void loadFrame() = 0;
238
239 //Constructor. Initialize variables and matrix sizes
241
242 };
243 }
244}
245
246
247
248#endif
A numeric matrix of compile-time fixed size.
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition CPose3D.h:73
This abstract class implements a method called "Difodo" to perform Visual odometry with range cameras...
Definition CDifodo.h:55
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).
Definition CDifodo.h:84
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.
Definition CDifodo.h:194
std::vector< Eigen::MatrixXf > depth_warped
Definition CDifodo.h:65
unsigned int height
Definition CDifodo.h:106
void setSpeedFilterConstWeight(float new_cweight)
Set the filter constant-weight of the velocity filter.
Definition CDifodo.h:212
Eigen::Matrix< float, 6, 1 > kai_loc
Filtered velocity in local coordinates.
Definition CDifodo.h:136
std::vector< Eigen::MatrixXf > depth_inter
Definition CDifodo.h:64
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.
Definition CDifodo.h:117
unsigned int cols_i
Definition CDifodo.h:110
std::vector< Eigen::MatrixXf > depth
Matrices that store the point coordinates after downsampling.
Definition CDifodo.h:62
std::vector< Eigen::MatrixXf > xx
Definition CDifodo.h:66
float execution_time
Execution time (ms)
Definition CDifodo.h:183
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.
Definition CDifodo.h:87
float fps
Frames per second (Hz)
Definition CDifodo.h:169
mrpt::math::CMatrixFloat66 getCovariance() const
Get the least-square covariance matrix.
Definition CDifodo.h:230
unsigned int width
Aux variables: size from which the pyramid starts to be built.
Definition CDifodo.h:105
std::vector< Eigen::MatrixXf > depth_old
Definition CDifodo.h:63
Eigen::Matrix< float, 6, 1 > kai_abs
Last filtered velocity in absolute coordinates.
Definition CDifodo.h:133
std::vector< Eigen::MatrixXf > transformations
Transformations of the coarse-to-fine levels.
Definition CDifodo.h:127
Eigen::MatrixXf dv
Definition CDifodo.h:77
float previous_speed_const_weight
Speed filter parameters: Previous_speed_const_weight - Directly weights the previous speed in order t...
Definition CDifodo.h:123
Eigen::Matrix< float, 6, 1 > kai_loc_level
Solution from the solver at a given level.
Definition CDifodo.h:130
void poseUpdate()
Update camera pose and the velocities for the filter.
std::vector< Eigen::MatrixXf > yy_warped
Definition CDifodo.h:73
Eigen::MatrixXf du
Matrices that store the depth derivatives.
Definition CDifodo.h:76
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.
Definition CDifodo.h:215
Eigen::Matrix< float, 6, 1 > kai_loc_old
Definition CDifodo.h:137
unsigned int level
Definition CDifodo.h:118
unsigned int cam_mode
Resolution of the images taken by the range camera.
Definition CDifodo.h:172
std::vector< Eigen::MatrixXf > yy_inter
Definition CDifodo.h:71
std::vector< Eigen::MatrixXf > xx_old
Definition CDifodo.h:68
Eigen::MatrixXf dt
Definition CDifodo.h:78
void getWeights(Eigen::MatrixXf &we)
Get the matrix of weights.
float previous_speed_eig_weight
Default 0.5.
Definition CDifodo.h:124
mrpt::math::CMatrixFloat61 getLastSpeedAbs() const
Get the last camera velocity (vx, vy, vz, wx, wy, wz) expressed in the world reference frame,...
Definition CDifodo.h:227
float getSpeedFilterEigWeight() const
Get the filter eigen-weight of the velocity filter.
Definition CDifodo.h:209
std::vector< Eigen::MatrixXf > xx_warped
Definition CDifodo.h:69
mrpt::poses::CPose3D cam_pose
Camera poses.
Definition CDifodo.h:186
unsigned int cols
Definition CDifodo.h:102
float fovv
Vertical field of view (rad)
Definition CDifodo.h:96
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.
Definition CDifodo.h:197
void getFOV(float &cur_fovh, float &cur_fovv) const
Get the horizontal and vertical field of vision (in degrees)
Definition CDifodo.h:203
Eigen::MatrixXf depth_wf
Matrix that stores the original depth frames with the image resolution.
Definition CDifodo.h:59
bool fast_pyramid
Gaussian masks used to build the pyramid and flag to select accurate or fast pyramid.
Definition CDifodo.h:90
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 ...
Definition CDifodo.h:224
Eigen::MatrixXf weights
Weights for the range flow constraint equations in the least square solution.
Definition CDifodo.h:81
void buildCoordinatesPyramidFast()
void filterLevelSolution()
Method to filter the velocity at each level of the pyramid.
float fovh
Camera properties:
Definition CDifodo.h:95
void setFOV(float new_fovh, float new_fovv)
Set the horizontal and vertical field of vision (in degrees)
std::vector< Eigen::MatrixXf > xx_inter
Definition CDifodo.h:67
unsigned int ctf_levels
Number of coarse-to-fine levels.
Definition CDifodo.h:114
std::vector< Eigen::MatrixXf > yy
Definition CDifodo.h:70
unsigned int rows
The maximum resolution that will be considered by the visual odometry method.
Definition CDifodo.h:101
unsigned int num_valid_points
Num of valid points after removing null pixels.
Definition CDifodo.h:180
virtual void loadFrame()=0
Virtual method to be implemented in derived classes.
Eigen::Matrix4f f_mask
Definition CDifodo.h:91
unsigned int downsample
Downsample the image taken by the range camera.
Definition CDifodo.h:177
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.
Definition CDifodo.h:206
unsigned int rows_i
Aux variables: number of rows and cols at a given level.
Definition CDifodo.h:109
std::vector< Eigen::MatrixXf > yy_old
Definition CDifodo.h:72
mrpt::poses::CPose3D cam_oldpose
Previous camera pose.
Definition CDifodo.h:187
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.



Page generated by Doxygen 1.9.7 for MRPT 1.4.0 SVN: at Tue Jun 13 14:27:49 UTC 2023