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



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: 836b840ab Mon Nov 18 00:58:29 2019 +0100 at lun nov 18 01:00:14 CET 2019