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



Page generated by Doxygen 1.8.17 for MRPT 1.9.9 Git: ad3a9d8ae Tue May 1 23:10:22 2018 -0700 at miƩ 12 jul 2023 10:03:34 CEST