Main MRPT website > C++ reference for MRPT 1.5.7
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-2017, 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 
19 namespace 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 */
140  void buildCoordinatesPyramid();
141  void buildCoordinatesPyramidFast();
142 
143  /** Warp the second depth image against the first one according to the 3D transformations accumulated up to a given level */
144  void performWarping();
145 
146  /** Calculate the "average" coordinates of the points observed by the camera between two consecutive frames and find the Null measurements */
147  void calculateCoord();
148 
149  /** Calculates the depth derivatives respect to u,v (rows and cols) and t (time) */
150  void calculateDepthDerivatives();
151 
152  /** This method computes the weighting fuction associated to measurement and linearization errors */
153  void computeWeights();
154 
155  /** The Solver. It buils the overdetermined system and gets the least-square solution.
156  * It also calculates the least-square covariance matrix */
157  void solveOneLevel();
158 
159  /** Method to filter the velocity at each level of the pyramid. */
160  void filterLevelSolution();
161 
162  /** Update camera pose and the velocities for the filter */
163  void poseUpdate();
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 */
191  void odometryCalculation();
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
240  CDifodo();
241 
242  };
243  }
244 }
245 
246 
247 
248 #endif
unsigned int cols_i
Definition: CDifodo.h:110
float previous_speed_const_weight
Speed filter parameters: Previous_speed_const_weight - Directly weights the previous speed in order t...
Definition: CDifodo.h:123
unsigned int image_level
Aux varibles: levels of the image pyramid and the solver, respectively.
Definition: CDifodo.h:117
std::vector< Eigen::MatrixXf > xx
Definition: CDifodo.h:66
mrpt::poses::CPose3D cam_oldpose
Previous camera pose.
Definition: CDifodo.h:187
std::vector< Eigen::MatrixXf > depth_warped
Definition: CDifodo.h:65
std::vector< Eigen::MatrixXf > yy_warped
Definition: CDifodo.h:73
GLdouble GLdouble z
Definition: glext.h:3734
float fps
Frames per second (Hz)
Definition: CDifodo.h:169
unsigned int rows_i
Aux variables: number of rows and cols at a given level.
Definition: CDifodo.h:109
float previous_speed_eig_weight
Default 0.5.
Definition: CDifodo.h:124
unsigned int cols
Definition: CDifodo.h:102
std::vector< Eigen::MatrixXf > xx_old
Definition: CDifodo.h:68
std::vector< Eigen::MatrixXf > xx_warped
Definition: CDifodo.h:69
float getSpeedFilterConstWeight() const
Get the filter constant-weight of the velocity filter.
Definition: CDifodo.h:206
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
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
std::vector< Eigen::MatrixXf > transformations
Transformations of the coarse-to-fine levels.
Definition: CDifodo.h:127
unsigned int rows
The maximum resolution that will be considered by the visual odometry method.
Definition: CDifodo.h:101
Eigen::MatrixXf depth_wf
Matrix that stores the original depth frames with the image resolution.
Definition: CDifodo.h:59
mrpt::math::CMatrixFloat66 getCovariance() const
Get the least-square covariance matrix.
Definition: CDifodo.h:230
Eigen::MatrixXf dv
Definition: CDifodo.h:77
std::vector< Eigen::MatrixXf > yy_old
Definition: CDifodo.h:72
float getSpeedFilterEigWeight() const
Get the filter eigen-weight of the velocity filter.
Definition: CDifodo.h:209
bool fast_pyramid
Gaussian masks used to build the pyramid and flag to select accurate or fast pyramid.
Definition: CDifodo.h:90
float execution_time
Execution time (ms)
Definition: CDifodo.h:183
A numeric matrix of compile-time fixed size.
Eigen::MatrixXf weights
Weights for the range flow constraint equations in the least square solution.
Definition: CDifodo.h:81
unsigned int height
Definition: CDifodo.h:106
Eigen::Matrix4f f_mask
Definition: CDifodo.h:91
void getFOV(float &cur_fovh, float &cur_fovv) const
Get the horizontal and vertical field of vision (in degrees)
Definition: CDifodo.h:203
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
Eigen::MatrixXf dt
Definition: CDifodo.h:78
std::vector< Eigen::MatrixXf > depth_inter
Definition: CDifodo.h:64
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
This abstract class implements a method called "Difodo" to perform Visual odometry with range cameras...
Definition: CDifodo.h:55
int JSAMPARRAY int int num_rows
Definition: jpegint.h:370
int JSAMPARRAY int int JDIMENSION num_cols
Definition: jpegint.h:370
unsigned int ctf_levels
Number of coarse-to-fine levels.
Definition: CDifodo.h:114
Eigen::Matrix< float, 6, 1 > kai_loc
Filtered velocity in local coordinates.
Definition: CDifodo.h:136
Eigen::Matrix< float, 6, 1 > kai_loc_level
Solution from the solver at a given level.
Definition: CDifodo.h:130
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
std::vector< Eigen::MatrixXf > yy
Definition: CDifodo.h:70
std::vector< Eigen::MatrixXf > yy_inter
Definition: CDifodo.h:71
Eigen::Matrix< float, 6, 1 > kai_loc_old
Definition: CDifodo.h:137
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:72
unsigned int num_valid_points
Num of valid points after removing null pixels.
Definition: CDifodo.h:180
Eigen::Matrix< float, 6, 1 > kai_abs
Last filtered velocity in absolute coordinates.
Definition: CDifodo.h:133
float fovh
Camera properties:
Definition: CDifodo.h:95
void setSpeedFilterConstWeight(float new_cweight)
Set the filter constant-weight of the velocity filter.
Definition: CDifodo.h:212
mrpt::poses::CPose3D cam_pose
Camera poses.
Definition: CDifodo.h:186
GLenum GLint GLint y
Definition: glext.h:3516
void setSpeedFilterEigWeight(float new_eweight)
Set the filter eigen-weight of the velocity filter.
Definition: CDifodo.h:215
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
Eigen::Matrix< float, 6, 6 > est_cov
Least squares covariance matrix.
Definition: CDifodo.h:87
std::vector< Eigen::MatrixXf > depth
Matrices that store the point coordinates after downsampling.
Definition: CDifodo.h:62
float fovv
Vertical field of view (rad)
Definition: CDifodo.h:96
GLenum GLint x
Definition: glext.h:3516
unsigned int downsample
Downsample the image taken by the range camera.
Definition: CDifodo.h:177
std::vector< Eigen::MatrixXf > xx_inter
Definition: CDifodo.h:67
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
unsigned int level
Definition: CDifodo.h:118
unsigned int cam_mode
Resolution of the images taken by the range camera.
Definition: CDifodo.h:172
Eigen::MatrixXf du
Matrices that store the depth derivatives.
Definition: CDifodo.h:76



Page generated by Doxygen 1.8.14 for MRPT 1.5.7 Git: 5902e14cc Wed Apr 24 15:04:01 2019 +0200 at lun oct 28 01:39:17 CET 2019