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