Main MRPT website > C++ reference for MRPT 1.5.6
CDifodo.cpp
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 #include "vision-precomp.h" // Precompiled headers
11 
12 #include <mrpt/vision/CDifodo.h>
13 #include <mrpt/utils/utils_defs.h>
14 #include <mrpt/utils/CTicTac.h>
15 #include <mrpt/utils/round.h>
16 
17 using namespace mrpt;
18 using namespace mrpt::vision;
19 using namespace mrpt::math;
20 using namespace std;
21 using namespace Eigen;
22 using mrpt::utils::round;
23 using mrpt::math::square;
24 
26 {
27  rows = 60;
28  cols = 80;
29  fovh = M_PIf*58.6f/180.0f;
30  fovv = M_PIf*45.6f/180.0f;
31  cam_mode = 1; // (1 - 640 x 480, 2 - 320 x 240, 4 - 160 x 120)
32  downsample = 1;
33  ctf_levels = 1;
34  width = 640/(cam_mode*downsample);
35  height = 480/(cam_mode*downsample);
36  fast_pyramid = true;
37 
38  //Resize pyramid
39  const unsigned int pyr_levels = round(log(float(width/cols))/log(2.f)) + ctf_levels;
40  depth.resize(pyr_levels);
41  depth_old.resize(pyr_levels);
42  depth_inter.resize(pyr_levels);
43  depth_warped.resize(pyr_levels);
44  xx.resize(pyr_levels);
45  xx_inter.resize(pyr_levels);
46  xx_old.resize(pyr_levels);
47  xx_warped.resize(pyr_levels);
48  yy.resize(pyr_levels);
49  yy_inter.resize(pyr_levels);
50  yy_old.resize(pyr_levels);
51  yy_warped.resize(pyr_levels);
52  transformations.resize(pyr_levels);
53 
54  for (unsigned int i = 0; i<pyr_levels; i++)
55  {
56  unsigned int s = pow(2.f,int(i));
57  cols_i = width/s; rows_i = height/s;
58  depth[i].resize(rows_i, cols_i);
59  depth_inter[i].resize(rows_i, cols_i);
60  depth_old[i].resize(rows_i, cols_i);
61  depth[i].assign(0.0f);
62  depth_old[i].assign(0.0f);
63  xx[i].resize(rows_i, cols_i);
64  xx_inter[i].resize(rows_i, cols_i);
65  xx_old[i].resize(rows_i, cols_i);
66  xx[i].assign(0.0f);
67  xx_old[i].assign(0.0f);
68  yy[i].resize(rows_i, cols_i);
69  yy_inter[i].resize(rows_i, cols_i);
70  yy_old[i].resize(rows_i, cols_i);
71  yy[i].assign(0.0f);
72  yy_old[i].assign(0.0f);
73  transformations[i].resize(4,4);
74 
75  if (cols_i <= cols)
76  {
77  depth_warped[i].resize(rows_i,cols_i);
78  xx_warped[i].resize(rows_i,cols_i);
79  yy_warped[i].resize(rows_i,cols_i);
80  }
81  }
82 
83  depth_wf.setSize(height,width);
84 
85  fps = 30.f; //In Hz
86 
87  previous_speed_const_weight = 0.05f;
88  previous_speed_eig_weight = 0.5f;
89  kai_loc_old.assign(0.f);
90  num_valid_points = 0;
91 
92  //Compute gaussian mask
93  VectorXf v_mask(4);
94  v_mask(0) = 1.f; v_mask(1) = 2.f; v_mask(2) = 2.f; v_mask(3) = 1.f;
95  for (unsigned int i = 0; i<4; i++)
96  for (unsigned int j = 0; j<4; j++)
97  f_mask(i,j) = v_mask(i)*v_mask(j)/36.f;
98 
99  //Compute gaussian mask
100  float v_mask2[5] = {1,4,6,4,1};
101  for (unsigned int i = 0; i<5; i++)
102  for (unsigned int j = 0; j<5; j++)
103  g_mask[i][j] = v_mask2[i]*v_mask2[j]/256.f;
104 }
105 
107 {
108  const float max_depth_dif = 0.1f;
109 
110  //Push coordinates back
111  depth_old.swap(depth);
112  xx_old.swap(xx);
113  yy_old.swap(yy);
114 
115  //The number of levels of the pyramid does not match the number of levels used
116  //in the odometry computation (because we might want to finish with lower resolutions)
117 
118  unsigned int pyr_levels = round(log(float(width/cols))/log(2.f)) + ctf_levels;
119 
120  //Generate levels
121  for (unsigned int i = 0; i<pyr_levels; i++)
122  {
123  unsigned int s = pow(2.f,int(i));
124  cols_i = width/s;
125  rows_i = height/s;
126  const int rows_i2 = 2*rows_i;
127  const int cols_i2 = 2*cols_i;
128  const int i_1 = i-1;
129 
130  if (i == 0)
131  depth[i].swap(depth_wf);
132 
133  // Downsampling
134  //-----------------------------------------------------------------------------
135  else
136  {
137  for (unsigned int u = 0; u < cols_i; u++)
138  for (unsigned int v = 0; v < rows_i; v++)
139  {
140  const int u2 = 2*u;
141  const int v2 = 2*v;
142  const float dcenter = depth[i_1](v2,u2);
143 
144  //Inner pixels
145  if ((v>0)&&(v<rows_i-1)&&(u>0)&&(u<cols_i-1))
146  {
147  if (dcenter > 0.f)
148  {
149  float sum = 0.f;
150  float weight = 0.f;
151 
152  for (int l = -2; l<3; l++)
153  for (int k = -2; k<3; k++)
154  {
155  const float abs_dif = abs(depth[i_1](v2+k,u2+l)-dcenter);
156  if (abs_dif < max_depth_dif)
157  {
158  const float aux_w = g_mask[2+k][2+l]*(max_depth_dif - abs_dif);
159  weight += aux_w;
160  sum += aux_w*depth[i_1](v2+k,u2+l);
161  }
162  }
163  depth[i](v,u) = sum/weight;
164  }
165  else
166  {
167  float min_depth = 10.f;
168  for (int l = -2; l<3; l++)
169  for (int k = -2; k<3; k++)
170  {
171  const float d = depth[i_1](v2+k,u2+l);
172  if ((d > 0.f)&&(d < min_depth))
173  min_depth = d;
174  }
175 
176  if (min_depth < 10.f)
177  depth[i](v,u) = min_depth;
178  else
179  depth[i](v,u) = 0.f;
180  }
181  }
182 
183  //Boundary
184  else
185  {
186  if (dcenter > 0.f)
187  {
188  float sum = 0.f;
189  float weight = 0.f;
190 
191  for (int l = -2; l<3; l++)
192  for (int k = -2; k<3; k++)
193  {
194  const int indv = v2+k,indu = u2+l;
195  if ((indv>=0)&&(indv<rows_i2)&&(indu>=0)&&(indu<cols_i2))
196  {
197  const float abs_dif = abs(depth[i_1](indv,indu)-dcenter);
198  if (abs_dif < max_depth_dif)
199  {
200  const float aux_w = g_mask[2+k][2+l]*(max_depth_dif - abs_dif);
201  weight += aux_w;
202  sum += aux_w*depth[i_1](indv,indu);
203  }
204  }
205  }
206  depth[i](v,u) = sum/weight;
207  }
208  else
209  {
210  float min_depth = 10.f;
211  for (int l = -2; l<3; l++)
212  for (int k = -2; k<3; k++)
213  {
214  const int indv = v2+k,indu = u2+l;
215  if ((indv>=0)&&(indv<rows_i2)&&(indu>=0)&&(indu<cols_i2))
216  {
217  const float d = depth[i_1](indv,indu);
218  if ((d > 0.f)&&(d < min_depth))
219  min_depth = d;
220  }
221  }
222 
223  if (min_depth < 10.f)
224  depth[i](v,u) = min_depth;
225  else
226  depth[i](v,u) = 0.f;
227  }
228  }
229  }
230  }
231 
232  //Calculate coordinates "xy" of the points
233  const float inv_f_i = 2.f*tan(0.5f*fovh)/float(cols_i);
234  const float disp_u_i = 0.5f*(cols_i-1);
235  const float disp_v_i = 0.5f*(rows_i-1);
236 
237  for (unsigned int u = 0; u < cols_i; u++)
238  for (unsigned int v = 0; v < rows_i; v++)
239  if (depth[i](v,u) > 0.f)
240  {
241  xx[i](v,u) = (u - disp_u_i)*depth[i](v,u)*inv_f_i;
242  yy[i](v,u) = (v - disp_v_i)*depth[i](v,u)*inv_f_i;
243  }
244  else
245  {
246  xx[i](v,u) = 0.f;
247  yy[i](v,u) = 0.f;
248  }
249  }
250 }
251 
253 {
254  const float max_depth_dif = 0.1f;
255 
256  //Push coordinates back
257  depth_old.swap(depth);
258  xx_old.swap(xx);
259  yy_old.swap(yy);
260 
261  //The number of levels of the pyramid does not match the number of levels used
262  //in the odometry computation (because we might want to finish with lower resolutions)
263 
264  unsigned int pyr_levels = round(log(float(width/cols))/log(2.f)) + ctf_levels;
265 
266  //Generate levels
267  for (unsigned int i = 0; i<pyr_levels; i++)
268  {
269  unsigned int s = pow(2.f,int(i));
270  cols_i = width/s;
271  rows_i = height/s;
272  //const int rows_i2 = 2*rows_i;
273  //const int cols_i2 = 2*cols_i;
274  const int i_1 = i-1;
275 
276  if (i == 0)
277  depth[i].swap(depth_wf);
278 
279  // Downsampling
280  //-----------------------------------------------------------------------------
281  else
282  {
283  for (unsigned int u = 0; u < cols_i; u++)
284  for (unsigned int v = 0; v < rows_i; v++)
285  {
286  const int u2 = 2*u;
287  const int v2 = 2*v;
288 
289  //Inner pixels
290  if ((v>0)&&(v<rows_i-1)&&(u>0)&&(u<cols_i-1))
291  {
292  const Matrix4f d_block = depth[i_1].block<4,4>(v2-1,u2-1);
293  float depths[4] = {d_block(5),d_block(6),d_block(9),d_block(10)};
294  float dcenter;
295 
296  //Sort the array (try to find a good/representative value)
297  for (signed char k = 2; k>=0; k--)
298  if (depths[k+1] < depths[k])
299  std::swap(depths[k+1],depths[k]);
300  for (unsigned char k = 1; k<3; k++)
301  if (depths[k] > depths[k+1])
302  std::swap(depths[k+1],depths[k]);
303  if (depths[2] < depths[1])
304  dcenter = depths[1];
305  else
306  dcenter = depths[2];
307 
308  if (dcenter > 0.f)
309  {
310  float sum = 0.f;
311  float weight = 0.f;
312 
313  for (unsigned char k = 0; k<16; k++)
314  {
315  const float abs_dif = abs(d_block(k) - dcenter);
316  if (abs_dif < max_depth_dif)
317  {
318  const float aux_w = f_mask(k)*(max_depth_dif - abs_dif);
319  weight += aux_w;
320  sum += aux_w*d_block(k);
321  }
322  }
323  depth[i](v,u) = sum/weight;
324  }
325  else
326  depth[i](v,u) = 0.f;
327 
328  }
329 
330  //Boundary
331  else
332  {
333  const Matrix2f d_block = depth[i_1].block<2,2>(v2,u2);
334  const float new_d = 0.25f*d_block.sumAll();
335  if (new_d < 0.4f)
336  depth[i](v,u) = 0.f;
337  else
338  depth[i](v,u) = new_d;
339  }
340  }
341  }
342 
343  //Calculate coordinates "xy" of the points
344  const float inv_f_i = 2.f*tan(0.5f*fovh)/float(cols_i);
345  const float disp_u_i = 0.5f*(cols_i-1);
346  const float disp_v_i = 0.5f*(rows_i-1);
347 
348  for (unsigned int u = 0; u < cols_i; u++)
349  for (unsigned int v = 0; v < rows_i; v++)
350  if (depth[i](v,u) > 0.f)
351  {
352  xx[i](v,u) = (u - disp_u_i)*depth[i](v,u)*inv_f_i;
353  yy[i](v,u) = (v - disp_v_i)*depth[i](v,u)*inv_f_i;
354  }
355  else
356  {
357  xx[i](v,u) = 0.f;
358  yy[i](v,u) = 0.f;
359  }
360  }
361 }
362 
364 {
365  //Camera parameters (which also depend on the level resolution)
366  const float f = float(cols_i)/(2.f*tan(0.5f*fovh));
367  const float disp_u_i = 0.5f*float(cols_i-1);
368  const float disp_v_i = 0.5f*float(rows_i-1);
369 
370  //Rigid transformation estimated up to the present level
371  Matrix4f acu_trans;
372  acu_trans.setIdentity();
373  for (unsigned int i=1; i<=level; i++)
374  acu_trans = transformations[i-1]*acu_trans;
375 
376  MatrixXf wacu(rows_i,cols_i);
377  wacu.assign(0.f);
378  depth_warped[image_level].assign(0.f);
379 
380  const float cols_lim = float(cols_i-1);
381  const float rows_lim = float(rows_i-1);
382 
383  // Warping loop
384  //---------------------------------------------------------
385  for (unsigned int j = 0; j<cols_i; j++)
386  for (unsigned int i = 0; i<rows_i; i++)
387  {
388  const float z = depth[image_level](i,j);
389 
390  if (z > 0.f)
391  {
392  //Transform point to the warped reference frame
393  const float depth_w = acu_trans(0,0)*z + acu_trans(0,1)*xx[image_level](i,j) + acu_trans(0,2)*yy[image_level](i,j) + acu_trans(0,3);
394  const float x_w = acu_trans(1,0)*z + acu_trans(1,1)*xx[image_level](i,j) + acu_trans(1,2)*yy[image_level](i,j) + acu_trans(1,3);
395  const float y_w = acu_trans(2,0)*z + acu_trans(2,1)*xx[image_level](i,j) + acu_trans(2,2)*yy[image_level](i,j) + acu_trans(2,3);
396 
397  //Calculate warping
398  const float uwarp = f*x_w/depth_w + disp_u_i;
399  const float vwarp = f*y_w/depth_w + disp_v_i;
400 
401  //The warped pixel (which is not integer in general) contributes to all the surrounding ones
402  if (( uwarp >= 0.f)&&( uwarp < cols_lim)&&( vwarp >= 0.f)&&( vwarp < rows_lim))
403  {
404  const int uwarp_l = uwarp;
405  const int uwarp_r = uwarp_l + 1;
406  const int vwarp_d = vwarp;
407  const int vwarp_u = vwarp_d + 1;
408  const float delta_r = float(uwarp_r) - uwarp;
409  const float delta_l = uwarp - float(uwarp_l);
410  const float delta_u = float(vwarp_u) - vwarp;
411  const float delta_d = vwarp - float(vwarp_d);
412 
413  //Warped pixel very close to an integer value
414  if (abs(round(uwarp) - uwarp) + abs(round(vwarp) - vwarp) < 0.05f)
415  {
416  depth_warped[image_level](round(vwarp), round(uwarp)) += depth_w;
417  wacu(round(vwarp), round(uwarp)) += 1.f;
418  }
419  else
420  {
421  const float w_ur = square(delta_l) + square(delta_d);
422  depth_warped[image_level](vwarp_u,uwarp_r) += w_ur*depth_w;
423  wacu(vwarp_u,uwarp_r) += w_ur;
424 
425  const float w_ul = square(delta_r) + square(delta_d);
426  depth_warped[image_level](vwarp_u,uwarp_l) += w_ul*depth_w;
427  wacu(vwarp_u,uwarp_l) += w_ul;
428 
429  const float w_dr = square(delta_l) + square(delta_u);
430  depth_warped[image_level](vwarp_d,uwarp_r) += w_dr*depth_w;
431  wacu(vwarp_d,uwarp_r) += w_dr;
432 
433  const float w_dl = square(delta_r) + square(delta_u);
434  depth_warped[image_level](vwarp_d,uwarp_l) += w_dl*depth_w;
435  wacu(vwarp_d,uwarp_l) += w_dl;
436  }
437  }
438  }
439  }
440 
441  //Scale the averaged depth and compute spatial coordinates
442  const float inv_f_i = 1.f/f;
443  for (unsigned int u = 0; u<cols_i; u++)
444  for (unsigned int v = 0; v<rows_i; v++)
445  {
446  if (wacu(v,u) > 0.f)
447  {
448  depth_warped[image_level](v,u) /= wacu(v,u);
449  xx_warped[image_level](v,u) = (u - disp_u_i)*depth_warped[image_level](v,u)*inv_f_i;
450  yy_warped[image_level](v,u) = (v - disp_v_i)*depth_warped[image_level](v,u)*inv_f_i;
451  }
452  else
453  {
454  depth_warped[image_level](v,u) = 0.f;
455  xx_warped[image_level](v,u) = 0.f;
456  yy_warped[image_level](v,u) = 0.f;
457  }
458  }
459 }
460 
462 {
463  null.resize(rows_i, cols_i);
464  null.assign(false);
465  num_valid_points = 0;
466 
467  for (unsigned int u = 0; u < cols_i; u++)
468  for (unsigned int v = 0; v < rows_i; v++)
469  {
470  if ((depth_old[image_level](v,u)) == 0.f || (depth_warped[image_level](v,u) == 0.f))
471  {
472  depth_inter[image_level](v,u) = 0.f;
473  xx_inter[image_level](v,u) = 0.f;
474  yy_inter[image_level](v,u) = 0.f;
475  null(v, u) = true;
476  }
477  else
478  {
479  depth_inter[image_level](v,u) = 0.5f*(depth_old[image_level](v,u) + depth_warped[image_level](v,u));
480  xx_inter[image_level](v,u) = 0.5f*(xx_old[image_level](v,u) + xx_warped[image_level](v,u));
481  yy_inter[image_level](v,u) = 0.5f*(yy_old[image_level](v,u) + yy_warped[image_level](v,u));
482  null(v, u) = false;
483  if ((u>0)&&(v>0)&&(u<cols_i-1)&&(v<rows_i-1))
484  num_valid_points++;
485  }
486  }
487 }
488 
490 {
491  dt.resize(rows_i,cols_i); dt.assign(0.f);
492  du.resize(rows_i,cols_i); du.assign(0.f);
493  dv.resize(rows_i,cols_i); dv.assign(0.f);
494 
495  //Compute connectivity
496  MatrixXf rx_ninv(rows_i,cols_i);
497  MatrixXf ry_ninv(rows_i,cols_i);
498  rx_ninv.assign(1.f); ry_ninv.assign(1.f);
499 
500  for (unsigned int u = 0; u < cols_i-1; u++)
501  for (unsigned int v = 0; v < rows_i; v++)
502  if (null(v,u) == false)
503  {
504  rx_ninv(v,u) = sqrtf(square(xx_inter[image_level](v,u+1) - xx_inter[image_level](v,u))
505  + square(depth_inter[image_level](v,u+1) - depth_inter[image_level](v,u)));
506  }
507 
508  for (unsigned int u = 0; u < cols_i; u++)
509  for (unsigned int v = 0; v < rows_i-1; v++)
510  if (null(v,u) == false)
511  {
512  ry_ninv(v,u) = sqrtf(square(yy_inter[image_level](v+1,u) - yy_inter[image_level](v,u))
513  + square(depth_inter[image_level](v+1,u) - depth_inter[image_level](v,u)));
514  }
515 
516 
517  //Spatial derivatives
518  for (unsigned int v = 0; v < rows_i; v++)
519  {
520  for (unsigned int u = 1; u < cols_i-1; u++)
521  if (null(v,u) == false)
522  du(v,u) = (rx_ninv(v,u-1)*(depth_inter[image_level](v,u+1)-depth_inter[image_level](v,u)) + rx_ninv(v,u)*(depth_inter[image_level](v,u) - depth_inter[image_level](v,u-1)))/(rx_ninv(v,u)+rx_ninv(v,u-1));
523 
524  du(v,0) = du(v,1);
525  du(v,cols_i-1) = du(v,cols_i-2);
526  }
527 
528  for (unsigned int u = 0; u < cols_i; u++)
529  {
530  for (unsigned int v = 1; v < rows_i-1; v++)
531  if (null(v,u) == false)
532  dv(v,u) = (ry_ninv(v-1,u)*(depth_inter[image_level](v+1,u)-depth_inter[image_level](v,u)) + ry_ninv(v,u)*(depth_inter[image_level](v,u) - depth_inter[image_level](v-1,u)))/(ry_ninv(v,u)+ry_ninv(v-1,u));
533 
534  dv(0,u) = dv(1,u);
535  dv(rows_i-1,u) = dv(rows_i-2,u);
536  }
537 
538  //Temporal derivative
539  for (unsigned int u = 0; u < cols_i; u++)
540  for (unsigned int v = 0; v < rows_i; v++)
541  if (null(v,u) == false)
542  dt(v,u) = fps*(depth_warped[image_level](v,u) - depth_old[image_level](v,u));
543 }
544 
546 {
547  weights.resize(rows_i, cols_i);
548  weights.assign(0.f);
549 
550  //Obtain the velocity associated to the rigid transformation estimated up to the present level
551  Matrix<float,6,1> kai_level = kai_loc_old;
552 
553  Matrix4f acu_trans;
554  acu_trans.setIdentity();
555  for (unsigned int i=0; i<level; i++)
556  acu_trans = transformations[i]*acu_trans;
557 
558  //Alternative way to compute the log
559  CMatrixDouble44 mat_aux = acu_trans.cast<double>();
560  poses::CPose3D aux(mat_aux);
561  CArrayDouble<6> kai_level_acu = aux.ln()*fps;
562  kai_level -= kai_level_acu.cast<float>();
563 
564  //Parameters for the measurement error
565  const float f_inv = float(cols_i)/(2.f*tan(0.5f*fovh));
566  const float kz2 = 8.122e-12f; //square(1.425e-5) / 25
567 
568  //Parameters for linearization error
569  const float kduv = 20e-5f;
570  const float kdt = kduv/square(fps);
571  const float k2dt = 5e-6f;
572  const float k2duv = 5e-6f;
573 
574  for (unsigned int u = 1; u < cols_i-1; u++)
575  for (unsigned int v = 1; v < rows_i-1; v++)
576  if (null(v,u) == false)
577  {
578  // Compute measurment error (simplified)
579  //-----------------------------------------------------------------------
580  const float z = depth_inter[image_level](v,u);
581  const float inv_d = 1.f/z;
582  //const float dycomp = du2(v,u)*f_inv_y*inv_d;
583  //const float dzcomp = dv2(v,u)*f_inv_z*inv_d;
584  const float z2 = z*z;
585  const float z4 = z2*z2;
586 
587  //const float var11 = kz2*z4;
588  //const float var12 = kz2*xx_inter[image_level](v,u)*z2*depth_inter[image_level](v,u);
589  //const float var13 = kz2*yy_inter[image_level](v,u)*z2*depth_inter[image_level](v,u);
590  //const float var22 = kz2*square(xx_inter[image_level](v,u))*z2;
591  //const float var23 = kz2*xx_inter[image_level](v,u)*yy_inter[image_level](v,u)*z2;
592  //const float var33 = kz2*square(yy_inter[image_level](v,u))*z2;
593  const float var44 = kz2*z4*square(fps);
594  const float var55 = kz2*z4*0.25f;
595  const float var66 = var55;
596 
597  //const float j1 = -2.f*inv_d*inv_d*(xx_inter[image_level](v,u)*dycomp + yy_inter[image_level](v,u)*dzcomp)*(kai_level[0] + yy_inter[image_level](v,u)*kai_level[4] - xx_inter[image_level](v,u)*kai_level[5])
598  // + inv_d*dycomp*(kai_level[1] - yy_inter[image_level](v,u)*kai_level[3]) + inv_d*dzcomp*(kai_level[2] + xx_inter[image_level](v,u)*kai_level[3]);
599  //const float j2 = inv_d*dycomp*(kai_level[0] + yy_inter[image_level](v,u)*kai_level[4] - 2.f*xx_inter[image_level](v,u)*kai_level[5]) - dzcomp*kai_level[3];
600  //const float j3 = inv_d*dzcomp*(kai_level[0] + 2.f*yy_inter[image_level](v,u)*kai_level[4] - xx_inter[image_level](v,u)*kai_level[5]) + dycomp*kai_level[3];
601 
602  const float j4 = 1.f;
603  const float j5 = xx_inter[image_level](v,u)*inv_d*inv_d*f_inv*(kai_level[0] + yy_inter[image_level](v,u)*kai_level[4] - xx_inter[image_level](v,u)*kai_level[5])
604  + inv_d*f_inv*(-kai_level[1] - z*kai_level[5] + yy_inter[image_level](v,u)*kai_level[3]);
605  const float j6 = yy_inter[image_level](v,u)*inv_d*inv_d*f_inv*(kai_level[0] + yy_inter[image_level](v,u)*kai_level[4] - xx_inter[image_level](v,u)*kai_level[5])
606  + inv_d*f_inv*(-kai_level[2] + z*kai_level[4] - xx_inter[image_level](v,u)*kai_level[3]);
607 
608  //error_measurement(v,u) = j1*(j1*var11+j2*var12+j3*var13) + j2*(j1*var12+j2*var22+j3*var23)
609  // +j3*(j1*var13+j2*var23+j3*var33) + j4*j4*var44 + j5*j5*var55 + j6*j6*var66;
610 
611  const float error_m = j4*j4*var44 + j5*j5*var55 + j6*j6*var66;
612 
613 
614  // Compute linearization error
615  //-----------------------------------------------------------------------
616  const float ini_du = depth_old[image_level](v,u+1) - depth_old[image_level](v,u-1);
617  const float ini_dv = depth_old[image_level](v+1,u) - depth_old[image_level](v-1,u);
618  const float final_du = depth_warped[image_level](v,u+1) - depth_warped[image_level](v,u-1);
619  const float final_dv = depth_warped[image_level](v+1,u) - depth_warped[image_level](v-1,u);
620 
621  const float dut = ini_du - final_du;
622  const float dvt = ini_dv - final_dv;
623  const float duu = du(v,u+1) - du(v,u-1);
624  const float dvv = dv(v+1,u) - dv(v-1,u);
625  const float dvu = dv(v,u+1) - dv(v,u-1); //Completely equivalent to compute duv
626 
627  const float error_l = kdt*square(dt(v,u)) + kduv*(square(du(v,u)) + square(dv(v,u))) + k2dt*(square(dut) + square(dvt))
628  + k2duv*(square(duu) + square(dvv) + square(dvu));
629 
630  //Weight
631  weights(v,u) = sqrt(1.f/(error_m + error_l));
632  }
633 
634  //Normalize weights in the range [0,1]
635  const float inv_max = 1.f/weights.maximum();
636  weights = inv_max*weights;
637 }
638 
640 {
641  MatrixXf A(num_valid_points,6);
642  MatrixXf B(num_valid_points,1);
643  unsigned int cont = 0;
644 
645  //Fill the matrix A and the vector B
646  //The order of the unknowns is (vz, vx, vy, wz, wx, wy)
647  //The points order will be (1,1), (1,2)...(1,cols-1), (2,1), (2,2)...(row-1,cols-1).
648 
649  const float f_inv = float(cols_i)/(2.f*tan(0.5f*fovh));
650 
651  for (unsigned int u = 1; u < cols_i-1; u++)
652  for (unsigned int v = 1; v < rows_i-1; v++)
653  if (null(v,u) == false)
654  {
655  // Precomputed expressions
656  const float d = depth_inter[image_level](v,u);
657  const float inv_d = 1.f/d;
658  const float x = xx_inter[image_level](v,u);
659  const float y = yy_inter[image_level](v,u);
660  const float dycomp = du(v,u)*f_inv*inv_d;
661  const float dzcomp = dv(v,u)*f_inv*inv_d;
662  const float tw = weights(v,u);
663 
664  //Fill the matrix A
665  A(cont, 0) = tw*(1.f + dycomp*x*inv_d + dzcomp*y*inv_d);
666  A(cont, 1) = tw*(-dycomp);
667  A(cont, 2) = tw*(-dzcomp);
668  A(cont, 3) = tw*(dycomp*y - dzcomp*x);
669  A(cont, 4) = tw*(y + dycomp*inv_d*y*x + dzcomp*(y*y*inv_d + d));
670  A(cont, 5) = tw*(-x - dycomp*(x*x*inv_d + d) - dzcomp*inv_d*y*x);
671  B(cont,0) = tw*(-dt(v,u));
672 
673  cont++;
674  }
675 
676  //Solve the linear system of equations using weighted least squares
677  MatrixXf AtA, AtB;
678  AtA.multiply_AtA(A);
679  AtB.multiply_AtB(A,B);
680  MatrixXf Var = AtA.ldlt().solve(AtB);
681 
682  //Covariance matrix calculation
683  MatrixXf res = -B;
684  for (unsigned int k = 0; k<6; k++)
685  res += Var(k)*A.col(k);
686 
687  est_cov = (1.f/float(num_valid_points-6))*AtA.inverse()*res.squaredNorm();
688 
689  //Update last velocity in local coordinates
690  kai_loc_level = Var;
691 }
692 
694 {
695  //Clock to measure the runtime
696  utils::CTicTac clock;
697  clock.Tic();
698 
699  //Build the gaussian pyramid
700  if (fast_pyramid) buildCoordinatesPyramidFast();
701  else buildCoordinatesPyramid();
702 
703  //Coarse-to-fines scheme
704  for (unsigned int i=0; i<ctf_levels; i++)
705  {
706  //Previous computations
707  transformations[i].setIdentity();
708 
709  level = i;
710  unsigned int s = pow(2.f,int(ctf_levels-(i+1)));
711  cols_i = cols/s; rows_i = rows/s;
712  image_level = ctf_levels - i + round(log(float(width/cols))/log(2.f)) - 1;
713 
714  //1. Perform warping
715  if (i == 0)
716  {
717  depth_warped[image_level] = depth[image_level];
718  xx_warped[image_level] = xx[image_level];
719  yy_warped[image_level] = yy[image_level];
720  }
721  else
722  performWarping();
723 
724  //2. Calculate inter coords and find null measurements
725  calculateCoord();
726 
727  //3. Compute derivatives
728  calculateDepthDerivatives();
729 
730  //4. Compute weights
731  computeWeights();
732 
733  //5. Solve odometry
734  if (num_valid_points > 6)
735  solveOneLevel();
736 
737  //6. Filter solution
738  filterLevelSolution();
739  }
740 
741  //Update poses
742  poseUpdate();
743 
744  //Save runtime
745  execution_time = 1000.f*clock.Tac();
746 }
747 
749 {
750  // Calculate Eigenvalues and Eigenvectors
751  //----------------------------------------------------------
752  SelfAdjointEigenSolver<MatrixXf> eigensolver(est_cov);
753  if (eigensolver.info() != Success)
754  {
755  printf("\n Eigensolver couldn't find a solution. Pose is not updated");
756  return;
757  }
758 
759  //First, we have to describe both the new linear and angular velocities in the "eigenvector" basis
760  //-------------------------------------------------------------------------------------------------
761  Matrix<float,6,6> Bii;
762  Matrix<float,6,1> kai_b;
763  Bii = eigensolver.eigenvectors();
764  kai_b = Bii.colPivHouseholderQr().solve(kai_loc_level);
765 
766  //Second, we have to describe both the old linear and angular velocities in the "eigenvector" basis
767  //-------------------------------------------------------------------------------------------------
768  Matrix<float,6,1> kai_loc_sub = kai_loc_old;
769 
770  //Important: we have to substract the previous levels' solutions from the old velocity.
771  Matrix4f acu_trans;
772  acu_trans.setIdentity();
773  for (unsigned int i=0; i<level; i++)
774  acu_trans = transformations[i]*acu_trans;
775 
776  CMatrixDouble44 mat_aux = acu_trans.cast<double>();
777  poses::CPose3D aux(mat_aux);
778  CArrayDouble<6> kai_level_acu = aux.ln()*fps;
779  kai_loc_sub -= kai_level_acu.cast<float>();
780 
781  //Matrix<float, 4, 4> log_trans = fps*acu_trans.log();
782  //kai_loc_sub(0) -= log_trans(0,3); kai_loc_sub(1) -= log_trans(1,3); kai_loc_sub(2) -= log_trans(2,3);
783  //kai_loc_sub(3) += log_trans(1,2); kai_loc_sub(4) -= log_trans(0,2); kai_loc_sub(5) += log_trans(0,1);
784 
785  //Transform that local representation to the "eigenvector" basis
786  Matrix<float,6,1> kai_b_old;
787  kai_b_old = Bii.colPivHouseholderQr().solve(kai_loc_sub);
788 
789  // Filter velocity
790  //--------------------------------------------------------------------------------
791  const float cf = previous_speed_eig_weight*expf(-int(level)), df = previous_speed_const_weight*expf(-int(level));
792  Matrix<float,6,1> kai_b_fil;
793  for (unsigned int i=0; i<6; i++)
794  kai_b_fil(i) = (kai_b(i) + (cf*eigensolver.eigenvalues()(i,0) + df)*kai_b_old(i))/(1.f + cf*eigensolver.eigenvalues()(i) + df);
795 
796  //Transform filtered velocity to the local reference frame
797  Matrix<float, 6, 1> kai_loc_fil = Bii.inverse().colPivHouseholderQr().solve(kai_b_fil);
798 
799  //Compute the rigid transformation
800  mrpt::math::CArrayDouble<6> aux_vel = kai_loc_fil.cast<double>()/fps;
801  poses::CPose3D aux1, aux2;
802  CMatrixDouble44 trans;
803  aux2 = aux1.exp(aux_vel);
804  aux2.getHomogeneousMatrix(trans);
805  transformations[level] = trans.cast<float>();
806 }
807 
809 {
810  //First, compute the overall transformation
811  //---------------------------------------------------
812  Matrix4f acu_trans;
813  acu_trans.setIdentity();
814  for (unsigned int i=1; i<=ctf_levels; i++)
815  acu_trans = transformations[i-1]*acu_trans;
816 
817 
818  //Compute the new estimates in the local and absolutes reference frames
819  //---------------------------------------------------------------------
820  CMatrixDouble44 mat_aux = acu_trans.cast<double>();
821  poses::CPose3D aux(mat_aux);
822  CArrayDouble<6> kai_level_acu = aux.ln()*fps;
823  kai_loc = kai_level_acu.cast<float>();
824 
825  //---------------------------------------------------------------------------------------------
826  //Directly from Eigen:
827  //- Eigen 3.1.0 needed for Matrix::log()
828  //- The line "#include <unsupported/Eigen/MatrixFunctions>" should be uncommented (CDifodo.h)
829  //
830  //Matrix<float, 4, 4> log_trans = fps*acu_trans.log();
831  //kai_loc(0) = log_trans(0,3); kai_loc(1) = log_trans(1,3); kai_loc(2) = log_trans(2,3);
832  //kai_loc(3) = -log_trans(1,2); kai_loc(4) = log_trans(0,2); kai_loc(5) = -log_trans(0,1);
833  //---------------------------------------------------------------------------------------------
834 
835  CMatrixDouble33 inv_trans;
836  CMatrixFloat31 v_abs, w_abs;
837 
838  cam_pose.getRotationMatrix(inv_trans);
839  v_abs = inv_trans.cast<float>()*kai_loc.topRows(3);
840  w_abs = inv_trans.cast<float>()*kai_loc.bottomRows(3);
841  kai_abs.topRows<3>() = v_abs;
842  kai_abs.bottomRows<3>() = w_abs;
843 
844 
845  // Update poses
846  //-------------------------------------------------------
847  cam_oldpose = cam_pose;
848  CMatrixDouble44 aux_acu = acu_trans;
849  poses::CPose3D pose_aux(aux_acu);
850  cam_pose = cam_pose + pose_aux;
851 
852 
853  //Compute the velocity estimate in the new ref frame (to be used by the filter in the next iteration)
854  //---------------------------------------------------------------------------------------------------
855  cam_pose.getRotationMatrix(inv_trans);
856  kai_loc_old.topRows<3>() = inv_trans.inverse().cast<float>()*kai_abs.topRows(3);
857  kai_loc_old.bottomRows<3>() = inv_trans.inverse().cast<float>()*kai_abs.bottomRows(3);
858 }
859 
860 void CDifodo::setFOV(float new_fovh, float new_fovv)
861 {
862  fovh = M_PI*new_fovh/180.0;
863  fovv = M_PI*new_fovv/180.0;
864 }
865 
866 void CDifodo::getPointsCoord(MatrixXf &x, MatrixXf &y, MatrixXf &z)
867 {
868  x.resize(rows,cols);
869  y.resize(rows,cols);
870  z.resize(rows,cols);
871 
872  z = depth_inter[0];
873  x = xx_inter[0];
874  y = yy_inter[0];
875 }
876 
877 void CDifodo::getDepthDerivatives(MatrixXf &cur_du, MatrixXf &cur_dv, MatrixXf &cur_dt)
878 {
879  cur_du.resize(rows,cols);
880  cur_dv.resize(rows,cols);
881  cur_dt.resize(rows,cols);
882 
883  cur_du = du;
884  cur_dv = dv;
885  cur_dt = dt;
886 }
887 
888 void CDifodo::getWeights(MatrixXf &w)
889 {
890  w.resize(rows,cols);
891  w = weights;
892 }
893 
894 
GLdouble GLdouble u2
Definition: glext.h:4993
const GLbyte * weights
Definition: glext.h:4150
GLdouble GLdouble z
Definition: glext.h:3734
void buildCoordinatesPyramid()
Create the gaussian image pyramid according to the number of coarse-to-fine levels.
Definition: CDifodo.cpp:106
void calculateDepthDerivatives()
Calculates the depth derivatives respect to u,v (rows and cols) and t (time)
Definition: CDifodo.cpp:489
GLint GLint GLsizei GLsizei GLsizei depth
Definition: glext.h:3545
STL namespace.
#define M_PI
Definition: bits.h:78
GLdouble s
Definition: glext.h:3602
void Tic()
Starts the stopwatch.
Definition: CTicTac.cpp:77
GLenum GLsizei width
Definition: glext.h:3513
void computeWeights()
This method computes the weighting fuction associated to measurement and linearization errors...
Definition: CDifodo.cpp:545
GLubyte GLubyte GLubyte GLubyte w
Definition: glext.h:3962
void solveOneLevel()
The Solver.
Definition: CDifodo.cpp:639
T square(const T x)
Inline function for the square of a number.
Definition: bits.h:52
void setFOV(float new_fovh, float new_fovv)
Set the horizontal and vertical field of vision (in degrees)
Definition: CDifodo.cpp:860
void odometryCalculation()
This method performs all the necessary steps to estimate the camera velocity once the new image is re...
Definition: CDifodo.cpp:693
void calculateCoord()
Calculate the "average" coordinates of the points observed by the camera between two consecutive fram...
Definition: CDifodo.cpp:461
A numeric matrix of compile-time fixed size.
This base provides a set of functions for maths stuff.
Definition: CArrayNumeric.h:19
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:877
#define M_PIf
void getWeights(Eigen::MatrixXf &we)
Get the matrix of weights.
Definition: CDifodo.cpp:888
This class implements a high-performance stopwatch.
Definition: CTicTac.h:24
CONTAINER::Scalar sum(const CONTAINER &v)
Computes the sum of all the elements.
Classes for computer vision, detectors, features, etc.
void filterLevelSolution()
Method to filter the velocity at each level of the pyramid.
Definition: CDifodo.cpp:748
void poseUpdate()
Update camera pose and the velocities for the filter.
Definition: CDifodo.cpp:808
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:866
const GLdouble * v
Definition: glext.h:3603
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:72
T square(const T x)
Inline function for the square of a number.
GLint level
Definition: glext.h:3545
double Tac()
Stops the stopwatch.
Definition: CTicTac.cpp:92
A partial specialization of CArrayNumeric for double numbers.
Definition: CArrayNumeric.h:74
int round(const T value)
Returns the closer integer (int) to x.
Definition: round.h:26
GLenum GLint GLint y
Definition: glext.h:3516
GLfloat GLfloat GLfloat v2
Definition: glext.h:3923
GLuint res
Definition: glext.h:6298
GLenum GLint x
Definition: glext.h:3516
void getRotationMatrix(mrpt::math::CMatrixDouble33 &ROT) const
Get the 3x3 rotation matrix.
Definition: CPose3D.h:176
GLenum GLsizei GLsizei height
Definition: glext.h:3523
void buildCoordinatesPyramidFast()
Definition: CDifodo.cpp:252
void performWarping()
Warp the second depth image against the first one according to the 3D transformations accumulated up ...
Definition: CDifodo.cpp:363



Page generated by Doxygen 1.8.14 for MRPT 1.5.6 Git: 4c65e8431 Tue Apr 24 08:18:17 2018 +0200 at lun oct 28 01:35:26 CET 2019