366 std::vector<FloatImageConstPtr>& pyramid,
370 pyramid.resize(step * nb_levels_);
375#pragma omp parallel for \
378 num_threads(threads_)
380 for (
int i = 0; i < static_cast<int>(input->size()); ++i)
381 (*tmp)[i] = intensity_((*input)[i]);
385 previous->height + 2 * track_height_));
400 derivatives(*img, *g_x, *g_y);
403 previous->height + 2 * track_height_));
415 previous->height + 2 * track_height_));
426 for (
int level = 1; level < nb_levels_; ++level) {
431 downsample(previous, current, g_x, g_y);
434 current->height + 2 * track_height_));
443 pyramid[level * step] = image;
445 new FloatImage(g_x->width + 2 * track_width_, g_x->height + 2 * track_height_));
454 pyramid[level * step + 1] = gradx;
456 new FloatImage(g_y->width + 2 * track_width_, g_y->height + 2 * track_height_));
465 pyramid[level * step + 2] = grady;
478 const Eigen::Array2i& location,
479 const Eigen::Array4f& weight,
480 Eigen::ArrayXXf& win,
481 Eigen::ArrayXXf& grad_x_win,
482 Eigen::ArrayXXf& grad_y_win,
483 Eigen::Array3f& covariance)
const
485 const int step = img.
width;
486 covariance.setZero();
488 for (
int y = 0; y < track_height_; y++) {
489 const float* img_ptr = &(img[0]) + (y + location[1]) * step + location[0];
490 const float* grad_x_ptr = &(grad_x[0]) + (y + location[1]) * step + location[0];
491 const float* grad_y_ptr = &(grad_y[0]) + (y + location[1]) * step + location[0];
493 float* win_ptr = win.data() + y * win.cols();
494 float* grad_x_win_ptr = grad_x_win.data() + y * grad_x_win.cols();
495 float* grad_y_win_ptr = grad_y_win.data() + y * grad_y_win.cols();
497 for (
int x = 0; x < track_width_; ++x, ++grad_x_ptr, ++grad_y_ptr) {
498 *win_ptr++ = img_ptr[x] * weight[0] + img_ptr[x + 1] * weight[1] +
499 img_ptr[x + step] * weight[2] + img_ptr[x + step + 1] * weight[3];
500 float ixval = grad_x_ptr[0] * weight[0] + grad_x_ptr[1] * weight[1] +
501 grad_x_ptr[step] * weight[2] + grad_x_ptr[step + 1] * weight[3];
502 float iyval = grad_y_ptr[0] * weight[0] + grad_y_ptr[1] * weight[1] +
503 grad_y_ptr[step] * weight[2] + grad_y_ptr[step + 1] * weight[3];
505 *grad_x_win_ptr++ = ixval;
506 *grad_y_win_ptr++ = iyval;
508 covariance[0] += ixval * ixval;
509 covariance[1] += ixval * iyval;
510 covariance[2] += iyval * iyval;
519 const Eigen::ArrayXXf& prev,
520 const Eigen::ArrayXXf& prev_grad_x,
521 const Eigen::ArrayXXf& prev_grad_y,
523 const Eigen::Array2i& location,
524 const Eigen::Array4f& weight,
525 Eigen::Array2f& b)
const
527 const int step = next.
width;
529 for (
int y = 0; y < track_height_; y++) {
530 const float* next_ptr = &(next[0]) + (y + location[1]) * step + location[0];
531 const float* prev_ptr = prev.data() + y * prev.cols();
532 const float* prev_grad_x_ptr = prev_grad_x.data() + y * prev_grad_x.cols();
533 const float* prev_grad_y_ptr = prev_grad_y.data() + y * prev_grad_y.cols();
535 for (
int x = 0; x < track_width_; ++x, ++prev_grad_y_ptr, ++prev_grad_x_ptr) {
536 float diff = next_ptr[x] * weight[0] + next_ptr[x + 1] * weight[1] +
537 next_ptr[x + step] * weight[2] + next_ptr[x + step + 1] * weight[3] -
539 b[0] += *prev_grad_x_ptr * diff;
540 b[1] += *prev_grad_y_ptr * diff;
551 const std::vector<FloatImageConstPtr>& prev_pyramid,
552 const std::vector<FloatImageConstPtr>& pyramid,
555 std::vector<int>& status,
556 Eigen::Affine3f& motion)
const
558 std::vector<Eigen::Array2f, Eigen::aligned_allocator<Eigen::Array2f>> next_pts(
559 prev_keypoints->
size());
560 Eigen::Array2f half_win((track_width_ - 1) * 0.5f, (track_height_ - 1) * 0.5f);
562 const int nb_points = prev_keypoints->
size();
563 for (
int level = nb_levels_ - 1; level >= 0; --level) {
564 const FloatImage& prev = *(prev_pyramid[level * 3]);
565 const FloatImage& next = *(pyramid[level * 3]);
566 const FloatImage& grad_x = *(prev_pyramid[level * 3 + 1]);
567 const FloatImage& grad_y = *(prev_pyramid[level * 3 + 2]);
569 Eigen::ArrayXXf prev_win(track_height_, track_width_);
570 Eigen::ArrayXXf grad_x_win(track_height_, track_width_);
571 Eigen::ArrayXXf grad_y_win(track_height_, track_width_);
572 float ratio(1. / (1 << level));
573 for (
int ptidx = 0; ptidx < nb_points; ptidx++) {
574 Eigen::Array2f prev_pt((*prev_keypoints)[ptidx].u * ratio,
575 (*prev_keypoints)[ptidx].v * ratio);
576 Eigen::Array2f next_pt;
577 if (level == nb_levels_ - 1)
580 next_pt = next_pts[ptidx] * 2.f;
582 next_pts[ptidx] = next_pt;
584 Eigen::Array2i iprev_point;
586 iprev_point[0] = std::floor(prev_pt[0]);
587 iprev_point[1] = std::floor(prev_pt[1]);
589 if (iprev_point[0] < -track_width_ ||
590 (std::uint32_t)iprev_point[0] >= grad_x.
width ||
591 iprev_point[1] < -track_height_ ||
592 (std::uint32_t)iprev_point[1] >= grad_y.
height) {
598 float a = prev_pt[0] - iprev_point[0];
599 float b = prev_pt[1] - iprev_point[1];
600 Eigen::Array4f weight;
601 weight[0] = (1.f - a) * (1.f - b);
602 weight[1] = a * (1.f - b);
603 weight[2] = (1.f - a) * b;
604 weight[3] = 1 - weight[0] - weight[1] - weight[2];
606 Eigen::Array3f covar = Eigen::Array3f::Zero();
607 spatialGradient(prev,
617 float det = covar[0] * covar[2] - covar[1] * covar[1];
618 float min_eigenvalue = (covar[2] + covar[0] -
619 std::sqrt((covar[0] - covar[2]) * (covar[0] - covar[2]) +
620 4.f * covar[1] * covar[1])) /
623 if (min_eigenvalue < min_eigenvalue_threshold_ ||
624 det < std::numeric_limits<float>::epsilon()) {
632 Eigen::Array2f prev_delta(0, 0);
633 for (
unsigned int j = 0; j < max_iterations_; j++) {
634 Eigen::Array2i inext_pt = next_pt.floor().cast<
int>();
636 if (inext_pt[0] < -track_width_ || (std::uint32_t)inext_pt[0] >= next.
width ||
637 inext_pt[1] < -track_height_ || (std::uint32_t)inext_pt[1] >= next.
height) {
643 a = next_pt[0] - inext_pt[0];
644 b = next_pt[1] - inext_pt[1];
645 weight[0] = (1.f - a) * (1.f - b);
646 weight[1] = a * (1.f - b);
647 weight[2] = (1.f - a) * b;
648 weight[3] = 1 - weight[0] - weight[1] - weight[2];
650 Eigen::Array2f beta = Eigen::Array2f::Zero();
651 mismatchVector(prev_win, grad_x_win, grad_y_win, next, inext_pt, weight, beta);
653 Eigen::Vector2f delta((covar[1] * beta[1] - covar[2] * beta[0]) * det,
654 (covar[1] * beta[0] - covar[0] * beta[1]) * det);
656 next_pt[0] += delta[0];
657 next_pt[1] += delta[1];
658 next_pts[ptidx] = next_pt + half_win;
660 if (delta.squaredNorm() <= epsilon_)
663 if (j > 0 && std::abs(delta[0] + prev_delta[0]) < 0.01 &&
664 std::abs(delta[1] + prev_delta[1]) < 0.01) {
665 next_pts[ptidx][0] -= delta[0] * 0.5f;
666 next_pts[ptidx][1] -= delta[1] * 0.5f;
674 if (level == 0 && !status[ptidx]) {
675 Eigen::Array2f next_point = next_pts[ptidx] - half_win;
676 Eigen::Array2i inext_point;
678 inext_point[0] = std::floor(next_point[0]);
679 inext_point[1] = std::floor(next_point[1]);
681 if (inext_point[0] < -track_width_ ||
682 (std::uint32_t)inext_point[0] >= next.
width ||
683 inext_point[1] < -track_height_ ||
684 (std::uint32_t)inext_point[1] >= next.
height) {
690 n.
u = next_pts[ptidx][0];
691 n.
v = next_pts[ptidx][1];
694 inext_point[0] = std::floor(next_pts[ptidx][0]);
695 inext_point[1] = std::floor(next_pts[ptidx][1]);
696 iprev_point[0] = std::floor((*prev_keypoints)[ptidx].u);
697 iprev_point[1] = std::floor((*prev_keypoints)[ptidx].v);
698 const PointInT& prev_pt =
699 (*prev_input)[iprev_point[1] * prev_input->width + iprev_point[0]];
700 const PointInT& next_pt =
701 (*input)[inext_point[1] * input->width + inext_point[0]];
702 transformation_computer.
add(
703 prev_pt.getVector3fMap(), next_pt.getVector3fMap(), 1.0);