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



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: c7a3bec24 Sun Mar 29 18:33:13 2020 +0200 at dom mar 29 18:50:38 CEST 2020