16 #include <Eigen/Dense>
23 using namespace Eigen;
31 fovh =
M_PIf * 58.6f / 180.0f;
32 fovv =
M_PIf * 45.6f / 180.0f;
36 m_width = 640 / (cam_mode * downsample);
37 m_height = 480 / (cam_mode * downsample);
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);
57 for (
unsigned int i = 0; i < pyr_levels; i++)
59 unsigned int s =
static_cast<unsigned int>(pow(2.,
int(i)));
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);
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);
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);
77 transformations[i].resize(4, 4);
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);
87 depth_wf.setSize(m_height, m_width);
91 previous_speed_const_weight = 0.05f;
92 previous_speed_eig_weight = 0.5f;
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;
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;
115 const float max_depth_dif = 0.1f;
118 depth_old.swap(depth);
127 unsigned int pyr_levels =
128 round(log(
float(m_width / cols)) / log(2.f)) + ctf_levels;
131 for (
unsigned int i = 0; i < pyr_levels; i++)
133 unsigned int s =
static_cast<unsigned int>(pow(2.,
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;
140 if (i == 0) depth[i].swap(depth_wf);
146 for (
unsigned int u = 0; u < cols_i; u++)
147 for (
unsigned int v = 0; v < rows_i; v++)
149 const int u2 = 2 * u;
150 const int v2 = 2 * v;
151 const float dcenter = depth[i_1](v2, u2);
154 if ((v > 0) && (v < rows_i - 1) && (u > 0) &&
162 for (
int l = -2; l < 3; l++)
163 for (
int k = -2; k < 3; k++)
165 const float abs_dif = abs(
166 depth[i_1](v2 + k, u2 + l) - dcenter);
167 if (abs_dif < max_depth_dif)
170 g_mask[2 + k][2 + l] *
171 (max_depth_dif - abs_dif);
174 aux_w * depth[i_1](v2 + k, u2 + l);
177 depth[i](v, u) =
sum / weight;
181 float min_depth = 10.f;
182 for (
int l = -2; l < 3; l++)
183 for (
int k = -2; k < 3; k++)
185 const float d = depth[i_1](v2 + k, u2 + l);
186 if ((d > 0.f) && (d < min_depth))
190 if (min_depth < 10.f)
191 depth[i](v, u) = min_depth;
193 depth[i](v, u) = 0.f;
205 for (
int l = -2; l < 3; l++)
206 for (
int k = -2; k < 3; k++)
208 const int indv = v2 + k, indu = u2 + l;
209 if ((indv >= 0) && (indv < rows_i2) &&
210 (indu >= 0) && (indu < cols_i2))
212 const float abs_dif = abs(
213 depth[i_1](indv, indu) - dcenter);
214 if (abs_dif < max_depth_dif)
217 g_mask[2 + k][2 + l] *
218 (max_depth_dif - abs_dif);
221 aux_w * depth[i_1](indv, indu);
225 depth[i](v, u) =
sum / weight;
229 float min_depth = 10.f;
230 for (
int l = -2; l < 3; l++)
231 for (
int k = -2; k < 3; k++)
233 const int indv = v2 + k, indu = u2 + l;
234 if ((indv >= 0) && (indv < rows_i2) &&
235 (indu >= 0) && (indu < cols_i2))
237 const float d = depth[i_1](indv, indu);
238 if ((d > 0.f) && (d < min_depth))
243 if (min_depth < 10.f)
244 depth[i](v, u) = min_depth;
246 depth[i](v, u) = 0.f;
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);
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)
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;
274 const float max_depth_dif = 0.1f;
277 depth_old.swap(depth);
286 unsigned int pyr_levels =
287 round(log(
float(m_width / cols)) / log(2.f)) + ctf_levels;
290 for (
unsigned int i = 0; i < pyr_levels; i++)
292 unsigned int s =
static_cast<unsigned int>(pow(2.,
int(i)));
293 cols_i = m_width / s;
294 rows_i = m_height / s;
297 const int i_1 = i - 1;
299 if (i == 0) depth[i].swap(depth_wf);
305 for (
unsigned int u = 0; u < cols_i; u++)
306 for (
unsigned int v = 0; v < rows_i; v++)
308 const int u2 = 2 * u;
309 const int v2 = 2 * v;
312 if ((v > 0) && (v < rows_i - 1) && (u > 0) &&
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),
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])
339 for (
unsigned char k = 0; k < 16; k++)
341 const float abs_dif =
342 std::abs(d_block(k) - dcenter);
343 if (abs_dif < max_depth_dif)
346 f_mask[k] * (max_depth_dif - abs_dif);
348 sum += aux_w * d_block(k);
351 if (weight > 0) depth[i](v, u) =
sum / weight;
354 depth[i](v, u) = 0.f;
360 const Matrix2f d_block = depth[i_1].block<2, 2>(v2, u2);
361 const float new_d = 0.25f * d_block.array().sum();
363 depth[i](v, u) = 0.f;
365 depth[i](v, u) = new_d;
371 const float inv_f_i = 2.f * tan(0.5f * fovh) / float(cols_i);
372 const float disp_u_i = 0.5f * (cols_i - 1);
373 const float disp_v_i = 0.5f * (rows_i - 1);
375 for (
unsigned int u = 0; u < cols_i; u++)
376 for (
unsigned int v = 0; v < rows_i; v++)
377 if (depth[i](v, u) > 0.f)
379 xx[i](v, u) = (u - disp_u_i) * depth[i](v, u) * inv_f_i;
380 yy[i](v, u) = (v - disp_v_i) * depth[i](v, u) * inv_f_i;
393 const float f = float(cols_i) / (2.f * tan(0.5f * fovh));
394 const float disp_u_i = 0.5f * float(cols_i - 1);
395 const float disp_v_i = 0.5f * float(rows_i - 1);
399 acu_trans.setIdentity();
400 for (
unsigned int i = 1; i <= level; i++)
401 acu_trans = transformations[i - 1].asEigen() * acu_trans;
403 MatrixXf wacu(rows_i, cols_i);
405 depth_warped[image_level].fill(0.f);
407 const auto cols_lim = float(cols_i - 1);
408 const auto rows_lim = float(rows_i - 1);
412 for (
unsigned int j = 0; j < cols_i; j++)
413 for (
unsigned int i = 0; i < rows_i; i++)
415 const float z = depth[image_level](i, j);
420 const float depth_w = acu_trans(0, 0) * z +
421 acu_trans(0, 1) * xx[image_level](i, j) +
422 acu_trans(0, 2) * yy[image_level](i, j) +
424 const float x_w = acu_trans(1, 0) * z +
425 acu_trans(1, 1) * xx[image_level](i, j) +
426 acu_trans(1, 2) * yy[image_level](i, j) +
428 const float y_w = acu_trans(2, 0) * z +
429 acu_trans(2, 1) * xx[image_level](i, j) +
430 acu_trans(2, 2) * yy[image_level](i, j) +
434 const float uwarp = f * x_w / depth_w + disp_u_i;
435 const float vwarp = f * y_w / depth_w + disp_v_i;
439 if ((uwarp >= 0.f) && (uwarp < cols_lim) && (vwarp >= 0.f) &&
442 const int uwarp_l =
static_cast<int>(uwarp);
443 const int uwarp_r =
static_cast<int>(uwarp_l + 1);
444 const int vwarp_d =
static_cast<int>(vwarp);
445 const int vwarp_u =
static_cast<int>(vwarp_d + 1);
446 const float delta_r = float(uwarp_r) - uwarp;
447 const float delta_l = uwarp - float(uwarp_l);
448 const float delta_u = float(vwarp_u) - vwarp;
449 const float delta_d = vwarp - float(vwarp_d);
452 if (std::abs(
round(uwarp) - uwarp) +
453 std::abs(
round(vwarp) - vwarp) <
456 depth_warped[image_level](
round(vwarp),
round(uwarp)) +=
463 depth_warped[image_level](vwarp_u, uwarp_r) +=
465 wacu(vwarp_u, uwarp_r) += w_ur;
468 depth_warped[image_level](vwarp_u, uwarp_l) +=
470 wacu(vwarp_u, uwarp_l) += w_ul;
473 depth_warped[image_level](vwarp_d, uwarp_r) +=
475 wacu(vwarp_d, uwarp_r) += w_dr;
478 depth_warped[image_level](vwarp_d, uwarp_l) +=
480 wacu(vwarp_d, uwarp_l) += w_dl;
487 const float inv_f_i = 1.f / f;
488 for (
unsigned int u = 0; u < cols_i; u++)
489 for (
unsigned int v = 0; v < rows_i; v++)
491 if (wacu(v, u) > 0.f)
493 depth_warped[image_level](v, u) /= wacu(v, u);
494 xx_warped[image_level](v, u) =
495 (u - disp_u_i) * depth_warped[image_level](v, u) * inv_f_i;
496 yy_warped[image_level](v, u) =
497 (v - disp_v_i) * depth_warped[image_level](v, u) * inv_f_i;
501 depth_warped[image_level](v, u) = 0.f;
502 xx_warped[image_level](v, u) = 0.f;
503 yy_warped[image_level](v, u) = 0.f;
510 null.resize(rows_i, cols_i);
512 num_valid_points = 0;
514 for (
unsigned int u = 0; u < cols_i; u++)
515 for (
unsigned int v = 0; v < rows_i; v++)
517 if ((depth_old[image_level](v, u)) == 0.f ||
518 (depth_warped[image_level](v, u) == 0.f))
520 depth_inter[image_level](v, u) = 0.f;
521 xx_inter[image_level](v, u) = 0.f;
522 yy_inter[image_level](v, u) = 0.f;
527 depth_inter[image_level](v, u) =
528 0.5f * (depth_old[image_level](v, u) +
529 depth_warped[image_level](v, u));
530 xx_inter[image_level](v, u) =
532 (xx_old[image_level](v, u) + xx_warped[image_level](v, u));
533 yy_inter[image_level](v, u) =
535 (yy_old[image_level](v, u) + yy_warped[image_level](v, u));
537 if ((u > 0) && (v > 0) && (u < cols_i - 1) && (v < rows_i - 1))
545 dt.resize(rows_i, cols_i);
547 du.resize(rows_i, cols_i);
549 dv.resize(rows_i, cols_i);
553 MatrixXf rx_ninv(rows_i, cols_i);
554 MatrixXf ry_ninv(rows_i, cols_i);
558 for (
unsigned int u = 0; u < cols_i - 1; u++)
559 for (
unsigned int v = 0; v < rows_i; v++)
560 if (
null(v, u) ==
false)
562 rx_ninv(v, u) = sqrtf(
564 xx_inter[image_level](v, u + 1) -
565 xx_inter[image_level](v, u)) +
567 depth_inter[image_level](v, u + 1) -
568 depth_inter[image_level](v, u)));
571 for (
unsigned int u = 0; u < cols_i; u++)
572 for (
unsigned int v = 0; v < rows_i - 1; v++)
573 if (
null(v, u) ==
false)
575 ry_ninv(v, u) = sqrtf(
577 yy_inter[image_level](v + 1, u) -
578 yy_inter[image_level](v, u)) +
580 depth_inter[image_level](v + 1, u) -
581 depth_inter[image_level](v, u)));
585 for (
unsigned int v = 0; v < rows_i; v++)
587 for (
unsigned int u = 1; u < cols_i - 1; u++)
588 if (
null(v, u) ==
false)
590 (rx_ninv(v, u - 1) * (depth_inter[image_level](v, u + 1) -
591 depth_inter[image_level](v, u)) +
592 rx_ninv(v, u) * (depth_inter[image_level](v, u) -
593 depth_inter[image_level](v, u - 1))) /
594 (rx_ninv(v, u) + rx_ninv(v, u - 1));
597 du(v, cols_i - 1) = du(v, cols_i - 2);
600 for (
unsigned int u = 0; u < cols_i; u++)
602 for (
unsigned int v = 1; v < rows_i - 1; v++)
603 if (
null(v, u) ==
false)
605 (ry_ninv(v - 1, u) * (depth_inter[image_level](v + 1, u) -
606 depth_inter[image_level](v, u)) +
607 ry_ninv(v, u) * (depth_inter[image_level](v, u) -
608 depth_inter[image_level](v - 1, u))) /
609 (ry_ninv(v, u) + ry_ninv(v - 1, u));
612 dv(rows_i - 1, u) = dv(rows_i - 2, u);
616 for (
unsigned int u = 0; u < cols_i; u++)
617 for (
unsigned int v = 0; v < rows_i; v++)
618 if (
null(v, u) ==
false)
619 dt(v, u) =
d2f(fps) * (depth_warped[image_level](v, u) -
620 depth_old[image_level](v, u));
625 weights.resize(rows_i, cols_i);
634 acu_trans.setIdentity();
635 for (
unsigned int i = 0; i < level; i++)
636 acu_trans = transformations[i].asEigen() * acu_trans;
648 const float f_inv = float(cols_i) / (2.f * tan(0.5f * fovh));
649 const float kz2 = 8.122e-12f;
652 const float kduv = 20e-5f;
653 const float kdt = kduv / square<double, float>(fps);
654 const float k2dt = 5e-6f;
655 const float k2duv = 5e-6f;
657 for (
unsigned int u = 1; u < cols_i - 1; u++)
658 for (
unsigned int v = 1; v < rows_i - 1; v++)
659 if (
null(v, u) ==
false)
663 const float z = depth_inter[image_level](v, u);
664 const float inv_d = 1.f / z;
667 const float z2 = z * z;
668 const float z4 = z2 * z2;
681 const float var44 = kz2 * z4 * square<double, float>(fps);
682 const float var55 = kz2 * z4 * 0.25f;
683 const float var66 = var55;
703 const float j4 = 1.f;
705 xx_inter[image_level](v, u) * inv_d * inv_d * f_inv *
707 yy_inter[image_level](v, u) * kai_level[4] -
708 xx_inter[image_level](v, u) * kai_level[5]) +
710 (-kai_level[1] - z * kai_level[5] +
711 yy_inter[image_level](v, u) * kai_level[3]);
713 yy_inter[image_level](v, u) * inv_d * inv_d * f_inv *
715 yy_inter[image_level](v, u) * kai_level[4] -
716 xx_inter[image_level](v, u) * kai_level[5]) +
718 (-kai_level[2] + z * kai_level[4] -
719 xx_inter[image_level](v, u) * kai_level[3]);
728 const float error_m =
729 j4 * j4 * var44 + j5 * j5 * var55 + j6 * j6 * var66;
733 const float ini_du = depth_old[image_level](v, u + 1) -
734 depth_old[image_level](v, u - 1);
735 const float ini_dv = depth_old[image_level](v + 1, u) -
736 depth_old[image_level](v - 1, u);
737 const float final_du = depth_warped[image_level](v, u + 1) -
738 depth_warped[image_level](v, u - 1);
739 const float final_dv = depth_warped[image_level](v + 1, u) -
740 depth_warped[image_level](v - 1, u);
742 const float dut = ini_du - final_du;
743 const float dvt = ini_dv - final_dv;
744 const float duu = du(v, u + 1) - du(v, u - 1);
745 const float dvv = dv(v + 1, u) - dv(v - 1, u);
750 const float error_l =
757 weights(v, u) = sqrt(1.f / (error_m + error_l));
761 const float inv_max = 1.f / weights.maxCoeff();
767 MatrixXf
A(num_valid_points, 6);
768 MatrixXf B(num_valid_points, 1);
769 unsigned int cont = 0;
776 const float f_inv = float(cols_i) / (2.f * tan(0.5f * fovh));
778 for (
unsigned int u = 1; u < cols_i - 1; u++)
779 for (
unsigned int v = 1; v < rows_i - 1; v++)
780 if (
null(v, u) ==
false)
783 const float d = depth_inter[image_level](v, u);
784 const float inv_d = 1.f / d;
785 const float x = xx_inter[image_level](v, u);
786 const float y = yy_inter[image_level](v, u);
787 const float dycomp = du(v, u) * f_inv * inv_d;
788 const float dzcomp = dv(v, u) * f_inv * inv_d;
789 const float tw = weights(v, u);
793 tw * (1.f + dycomp * x * inv_d + dzcomp * y * inv_d);
794 A(cont, 1) = tw * (-dycomp);
795 A(cont, 2) = tw * (-dzcomp);
796 A(cont, 3) = tw * (dycomp * y - dzcomp * x);
797 A(cont, 4) = tw * (y + dycomp * inv_d * y * x +
798 dzcomp * (y * y * inv_d + d));
799 A(cont, 5) = tw * (-x - dycomp * (x * x * inv_d + d) -
800 dzcomp * inv_d * y * x);
801 B(cont, 0) = tw * (-dt(v, u));
807 const MatrixXf AtA =
A.transpose() *
A;
808 const MatrixXf AtB =
A.transpose() * B;
809 VectorXf Var = AtA.ldlt().solve(AtB);
813 for (
unsigned int k = 0; k < 6; k++) res += Var(k) *
A.col(k);
816 (1.f / float(num_valid_points - 6)) * AtA.inverse() * res.squaredNorm();
820 kai_loc_level.fromVector(Var);
831 buildCoordinatesPyramidFast();
833 buildCoordinatesPyramid();
836 for (
unsigned int i = 0; i < ctf_levels; i++)
839 transformations[i].setIdentity();
843 static_cast<unsigned int>(pow(2.f,
int(ctf_levels - (i + 1))));
847 ctf_levels - i +
round(log(
float(m_width / cols)) / log(2.f)) - 1;
852 depth_warped[image_level] = depth[image_level];
853 xx_warped[image_level] = xx[image_level];
854 yy_warped[image_level] = yy[image_level];
863 calculateDepthDerivatives();
869 if (num_valid_points > 6) solveOneLevel();
872 filterLevelSolution();
879 execution_time =
d2f(1000 * clock.
Tac());
887 std::vector<float> eigenVals;
889 if (!est_cov.eig_symmetric(Bii, eigenVals))
892 <<
"\n Eigensolver couldn't find a solution. Pose is not updated\n";
910 acu_trans.setIdentity();
911 for (
unsigned int i = 0; i < level; i++)
912 acu_trans = transformations[i].asEigen() * acu_trans;
916 acu_trans_vec *= fps;
919 kai_loc_sub -= kai_level_acu.asEigen().cast<
float>();
929 Bii.
asEigen().colPivHouseholderQr().solve(kai_loc_sub);
933 const float cf = previous_speed_eig_weight * expf(-
int(level)),
934 df = previous_speed_const_weight * expf(-
int(level));
936 for (
unsigned int i = 0; i < 6; i++)
938 (kai_b.asEigen()(i) + (cf * eigenVals[i] + df) * kai_b_old(i)) /
939 (1.f + cf * eigenVals[i] + df);
943 Bii.
asEigen().inverse().colPivHouseholderQr().solve(kai_b_fil);
960 acu_trans.setIdentity();
961 for (
unsigned int i = 1; i <= ctf_levels; i++)
962 acu_trans = transformations[i - 1].asEigen() * acu_trans;
968 acu_trans_vec *= fps;
970 kai_loc.fromVector(kai_level_acu.
cast_float());
987 cam_pose.getRotationMatrix(inv_trans);
996 kai_abs.vx = v_abs.x();
997 kai_abs.vy = v_abs.y();
998 kai_abs.vz = v_abs.z();
1000 kai_abs.wx = w_abs.x();
1001 kai_abs.wy = w_abs.y();
1002 kai_abs.wz = w_abs.z();
1006 cam_oldpose = cam_pose;
1008 cam_pose = cam_pose + pose_aux;
1013 cam_pose.getRotationMatrix(inv_trans);
1014 const auto old_vtrans =
1015 (inv_trans.
asEigen().inverse() *
1019 (inv_trans.
asEigen().inverse() *
1023 kai_loc_old.vx = old_vtrans.x();
1024 kai_loc_old.vy = old_vtrans.y();
1025 kai_loc_old.vz = old_vtrans.z();
1027 kai_loc_old.wx = old_w.x();
1028 kai_loc_old.wy = old_w.y();
1029 kai_loc_old.wz = old_w.z();
1034 fovh =
M_PI * new_fovh / 180.0;
1035 fovv =
M_PI * new_fovv / 180.0;
1052 cur_du.
resize(rows, cols);
1053 cur_dv.
resize(rows, cols);
1054 cur_dt.
resize(rows, cols);