38 #ifndef PCL_TRACKING_IMPL_PYRAMIDAL_KLT_HPP
39 #define PCL_TRACKING_IMPL_PYRAMIDAL_KLT_HPP
41 #include <pcl/common/io.h>
43 #include <pcl/common/utils.h>
48 template <
typename Po
intInT,
typename IntensityT>
53 track_height_ = height;
57 template <
typename Po
intInT,
typename IntensityT>
62 if (keypoints->
size() <= keypoints_nbr_)
63 keypoints_ = keypoints;
67 for (std::size_t i = 0; i < keypoints_nbr_; ++i)
72 keypoints_status_.reset(
new std::vector<int>);
73 keypoints_status_->
resize(keypoints_->size(), 0);
77 template <
typename Po
intInT,
typename IntensityT>
82 assert((input_ || ref_) &&
"[PyramidalKLTTracker] CALL setInputCloud FIRST!");
85 keypoints->
reserve(keypoints_nbr_);
86 for (std::size_t i = 0; i < keypoints_nbr_; ++i) {
88 uv.
u = points->indices[i] % input_->width;
89 uv.
v = points->indices[i] / input_->width;
92 setPointsToTrack(keypoints);
96 template <
typename Po
intInT,
typename IntensityT>
102 PCL_ERROR(
"[%s::initCompute] PCLBase::Init failed.\n", tracker_name_.c_str());
106 if (!input_->isOrganized()) {
108 "[pcl::tracking::%s::initCompute] Need an organized point cloud to proceed!\n",
109 tracker_name_.c_str());
113 if (!keypoints_ || keypoints_->empty()) {
114 PCL_ERROR(
"[pcl::tracking::%s::initCompute] No keypoints aborting!\n",
115 tracker_name_.c_str());
124 if ((track_height_ * track_width_) % 2 == 0) {
126 "[pcl::tracking::%s::initCompute] Tracking window (%dx%d) must be odd!\n",
127 tracker_name_.c_str(),
133 if (track_height_ < 3 || track_width_ < 3) {
135 "[pcl::tracking::%s::initCompute] Tracking window (%dx%d) must be >= 3x3!\n",
136 tracker_name_.c_str(),
142 track_width_2_ = track_width_ / 2;
143 track_height_2_ = track_height_ / 2;
145 if (nb_levels_ < 2) {
146 PCL_ERROR(
"[pcl::tracking::%s::initCompute] Number of pyramid levels should be "
148 tracker_name_.c_str());
152 if (nb_levels_ > 5) {
153 PCL_ERROR(
"[pcl::tracking::%s::initCompute] Number of pyramid levels should not "
155 tracker_name_.c_str());
169 template <
typename Po
intInT,
typename IntensityT>
193 float* row0 =
new float[src.
width + 2];
194 float* row1 =
new float[src.
width + 2];
199 const float* src_ptr = &(src[0]);
201 for (
int y = 0; y < height; y++) {
202 const float* srow0 = src_ptr + (y > 0 ? y - 1 : height > 1 ? 1 : 0) * width;
203 const float* srow1 = src_ptr + y * width;
205 src_ptr + (y < height - 1 ? y + 1 : height > 1 ? height - 2 : 0) * width;
206 float* grad_x_row = &(grad_x[y * width]);
207 float* grad_y_row = &(grad_y[y * width]);
210 for (
int x = 0; x < width; x++) {
211 trow0[x] = (srow0[x] + srow2[x]) * 3 + srow1[x] * 10;
212 trow1[x] = srow2[x] - srow0[x];
216 int x0 = width > 1 ? 1 : 0, x1 = width > 1 ? width - 2 : 0;
217 trow0[-1] = trow0[x0];
218 trow0[width] = trow0[x1];
219 trow1[-1] = trow1[x0];
220 trow1[width] = trow1[x1];
223 for (
int x = 0; x < width; x++) {
224 grad_x_row[x] = trow0[x + 1] - trow0[x - 1];
225 grad_y_row[x] = (trow1[x + 1] + trow1[x - 1]) * 3 + trow1[x] * 10;
231 template <
typename Po
intInT,
typename IntensityT>
236 FloatImage smoothed(input->width, input->height);
237 convolve(input, smoothed);
239 int width = (smoothed.
width + 1) / 2;
240 int height = (smoothed.
height + 1) / 2;
241 std::vector<int> ii(width);
242 for (
int i = 0; i < width; ++i)
247 #pragma omp parallel for \
249 shared(down, height, output, smoothed, width) \
251 num_threads(threads_)
253 for (
int j = 0; j < height; ++j) {
255 for (
int i = 0; i < width; ++i)
256 (*down)(i, j) = smoothed(ii[i], jj);
263 template <
typename Po
intInT,
typename IntensityT>
271 downsample(input, output);
274 derivatives(*output, *grad_x, *grad_y);
275 output_grad_x = grad_x;
276 output_grad_y = grad_y;
280 template <
typename Po
intInT,
typename IntensityT>
286 convolveRows(input, *tmp);
287 convolveCols(tmp, output);
291 template <
typename Po
intInT,
typename IntensityT>
296 int width = input->width;
297 int height = input->height;
298 int last = input->width - kernel_size_2_;
302 #pragma omp parallel for \
304 shared(input, height, last, output, w, width) \
305 num_threads(threads_)
307 for (
int j = 0; j < height; ++j) {
308 for (
int i = kernel_size_2_; i < last; ++i) {
310 for (
int k = kernel_last_, l = i - kernel_size_2_; k > -1; --k, ++l)
311 result += kernel_[k] * (*input)(l, j);
313 output(i, j) =
static_cast<float>(result);
316 for (
int i = last; i < width; ++i)
317 output(i, j) = output(w, j);
319 for (
int i = 0; i < kernel_size_2_; ++i)
320 output(i, j) = output(kernel_size_2_, j);
325 template <
typename Po
intInT,
typename IntensityT>
330 output =
FloatImage(input->width, input->height);
332 int width = input->width;
333 int height = input->height;
334 int last = input->height - kernel_size_2_;
338 #pragma omp parallel for \
340 shared(input, h, height, last, output, width) \
341 num_threads(threads_)
343 for (
int i = 0; i < width; ++i) {
344 for (
int j = kernel_size_2_; j < last; ++j) {
346 for (
int k = kernel_last_, l = j - kernel_size_2_; k > -1; --k, ++l)
347 result += kernel_[k] * (*input)(i, l);
348 output(i, j) =
static_cast<float>(result);
351 for (
int j = last; j < height; ++j)
352 output(i, j) = output(i, h);
354 for (
int j = 0; j < kernel_size_2_; ++j)
355 output(i, j) = output(i, kernel_size_2_);
360 template <
typename Po
intInT,
typename IntensityT>
364 std::vector<FloatImageConstPtr>& pyramid,
368 pyramid.resize(step * nb_levels_);
373 #pragma omp parallel for \
376 num_threads(threads_)
378 for (
int i = 0; i < static_cast<int>(input->size()); ++i)
379 (*tmp)[i] = intensity_((*input)[i]);
383 previous->height + 2 * track_height_));
398 derivatives(*img, *g_x, *g_y);
401 previous->height + 2 * track_height_));
413 previous->height + 2 * track_height_));
424 for (
int level = 1; level < nb_levels_; ++level) {
429 downsample(previous, current, g_x, g_y);
432 current->height + 2 * track_height_));
441 pyramid[level * step] = image;
443 new FloatImage(g_x->width + 2 * track_width_, g_x->height + 2 * track_height_));
452 pyramid[level * step + 1] = gradx;
454 new FloatImage(g_y->width + 2 * track_width_, g_y->height + 2 * track_height_));
463 pyramid[level * step + 2] = grady;
470 template <
typename Po
intInT,
typename IntensityT>
476 const Eigen::Array2i& location,
477 const Eigen::Array4f& weight,
478 Eigen::ArrayXXf& win,
479 Eigen::ArrayXXf& grad_x_win,
480 Eigen::ArrayXXf& grad_y_win,
481 Eigen::Array3f& covariance)
const
483 const int step = img.
width;
484 covariance.setZero();
486 for (
int y = 0; y < track_height_; y++) {
487 const float* img_ptr = &(img[0]) + (y + location[1]) * step + location[0];
488 const float* grad_x_ptr = &(grad_x[0]) + (y + location[1]) * step + location[0];
489 const float* grad_y_ptr = &(grad_y[0]) + (y + location[1]) * step + location[0];
491 float* win_ptr = win.data() + y * win.cols();
492 float* grad_x_win_ptr = grad_x_win.data() + y * grad_x_win.cols();
493 float* grad_y_win_ptr = grad_y_win.data() + y * grad_y_win.cols();
495 for (
int x = 0; x < track_width_; ++x, ++grad_x_ptr, ++grad_y_ptr) {
496 *win_ptr++ = img_ptr[x] * weight[0] + img_ptr[x + 1] * weight[1] +
497 img_ptr[x + step] * weight[2] + img_ptr[x + step + 1] * weight[3];
498 float ixval = grad_x_ptr[0] * weight[0] + grad_x_ptr[1] * weight[1] +
499 grad_x_ptr[step] * weight[2] + grad_x_ptr[step + 1] * weight[3];
500 float iyval = grad_y_ptr[0] * weight[0] + grad_y_ptr[1] * weight[1] +
501 grad_y_ptr[step] * weight[2] + grad_y_ptr[step + 1] * weight[3];
503 *grad_x_win_ptr++ = ixval;
504 *grad_y_win_ptr++ = iyval;
506 covariance[0] += ixval * ixval;
507 covariance[1] += ixval * iyval;
508 covariance[2] += iyval * iyval;
514 template <
typename Po
intInT,
typename IntensityT>
517 const Eigen::ArrayXXf& prev,
518 const Eigen::ArrayXXf& prev_grad_x,
519 const Eigen::ArrayXXf& prev_grad_y,
521 const Eigen::Array2i& location,
522 const Eigen::Array4f& weight,
523 Eigen::Array2f& b)
const
525 const int step = next.
width;
527 for (
int y = 0; y < track_height_; y++) {
528 const float* next_ptr = &(next[0]) + (y + location[1]) * step + location[0];
529 const float* prev_ptr = prev.data() + y * prev.cols();
530 const float* prev_grad_x_ptr = prev_grad_x.data() + y * prev_grad_x.cols();
531 const float* prev_grad_y_ptr = prev_grad_y.data() + y * prev_grad_y.cols();
533 for (
int x = 0; x < track_width_; ++x, ++prev_grad_y_ptr, ++prev_grad_x_ptr) {
534 float diff = next_ptr[x] * weight[0] + next_ptr[x + 1] * weight[1] +
535 next_ptr[x + step] * weight[2] + next_ptr[x + step + 1] * weight[3] -
537 b[0] += *prev_grad_x_ptr * diff;
538 b[1] += *prev_grad_y_ptr * diff;
544 template <
typename Po
intInT,
typename IntensityT>
549 const std::vector<FloatImageConstPtr>& prev_pyramid,
550 const std::vector<FloatImageConstPtr>& pyramid,
553 std::vector<int>& status,
554 Eigen::Affine3f& motion)
const
556 std::vector<Eigen::Array2f, Eigen::aligned_allocator<Eigen::Array2f>> next_pts(
557 prev_keypoints->
size());
558 Eigen::Array2f half_win((track_width_ - 1) * 0.5f, (track_height_ - 1) * 0.5f);
560 const int nb_points = prev_keypoints->
size();
561 for (
int level = nb_levels_ - 1; level >= 0; --level) {
562 const FloatImage& prev = *(prev_pyramid[level * 3]);
563 const FloatImage& next = *(pyramid[level * 3]);
564 const FloatImage& grad_x = *(prev_pyramid[level * 3 + 1]);
565 const FloatImage& grad_y = *(prev_pyramid[level * 3 + 2]);
567 Eigen::ArrayXXf prev_win(track_height_, track_width_);
568 Eigen::ArrayXXf grad_x_win(track_height_, track_width_);
569 Eigen::ArrayXXf grad_y_win(track_height_, track_width_);
570 float ratio(1. / (1 << level));
571 for (
int ptidx = 0; ptidx < nb_points; ptidx++) {
572 Eigen::Array2f prev_pt((*prev_keypoints)[ptidx].u * ratio,
573 (*prev_keypoints)[ptidx].v * ratio);
574 Eigen::Array2f next_pt;
575 if (level == nb_levels_ - 1)
578 next_pt = next_pts[ptidx] * 2.f;
580 next_pts[ptidx] = next_pt;
582 Eigen::Array2i iprev_point;
584 iprev_point[0] = std::floor(prev_pt[0]);
585 iprev_point[1] = std::floor(prev_pt[1]);
587 if (iprev_point[0] < -track_width_ ||
588 (std::uint32_t)iprev_point[0] >= grad_x.
width ||
589 iprev_point[1] < -track_height_ ||
590 (std::uint32_t)iprev_point[1] >= grad_y.
height) {
596 float a = prev_pt[0] - iprev_point[0];
597 float b = prev_pt[1] - iprev_point[1];
598 Eigen::Array4f weight;
599 weight[0] = (1.f - a) * (1.f - b);
600 weight[1] = a * (1.f - b);
601 weight[2] = (1.f - a) * b;
602 weight[3] = 1 - weight[0] - weight[1] - weight[2];
604 Eigen::Array3f covar = Eigen::Array3f::Zero();
605 spatialGradient(prev,
615 float det = covar[0] * covar[2] - covar[1] * covar[1];
616 float min_eigenvalue = (covar[2] + covar[0] -
617 std::sqrt((covar[0] - covar[2]) * (covar[0] - covar[2]) +
618 4.f * covar[1] * covar[1])) /
621 if (min_eigenvalue < min_eigenvalue_threshold_ ||
622 det < std::numeric_limits<float>::epsilon()) {
630 Eigen::Array2f prev_delta(0, 0);
631 for (
unsigned int j = 0; j < max_iterations_; j++) {
632 Eigen::Array2i inext_pt = next_pt.floor().cast<
int>();
634 if (inext_pt[0] < -track_width_ || (std::uint32_t)inext_pt[0] >= next.
width ||
635 inext_pt[1] < -track_height_ || (std::uint32_t)inext_pt[1] >= next.
height) {
641 a = next_pt[0] - inext_pt[0];
642 b = next_pt[1] - inext_pt[1];
643 weight[0] = (1.f - a) * (1.f - b);
644 weight[1] = a * (1.f - b);
645 weight[2] = (1.f - a) * b;
646 weight[3] = 1 - weight[0] - weight[1] - weight[2];
648 Eigen::Array2f beta = Eigen::Array2f::Zero();
649 mismatchVector(prev_win, grad_x_win, grad_y_win, next, inext_pt, weight, beta);
651 Eigen::Vector2f delta((covar[1] * beta[1] - covar[2] * beta[0]) * det,
652 (covar[1] * beta[0] - covar[0] * beta[1]) * det);
654 next_pt[0] += delta[0];
655 next_pt[1] += delta[1];
656 next_pts[ptidx] = next_pt + half_win;
658 if (delta.squaredNorm() <= epsilon_)
661 if (j > 0 && std::abs(delta[0] + prev_delta[0]) < 0.01 &&
662 std::abs(delta[1] + prev_delta[1]) < 0.01) {
663 next_pts[ptidx][0] -= delta[0] * 0.5f;
664 next_pts[ptidx][1] -= delta[1] * 0.5f;
672 if (level == 0 && !status[ptidx]) {
673 Eigen::Array2f next_point = next_pts[ptidx] - half_win;
674 Eigen::Array2i inext_point;
676 inext_point[0] = std::floor(next_point[0]);
677 inext_point[1] = std::floor(next_point[1]);
679 if (inext_point[0] < -track_width_ ||
680 (std::uint32_t)inext_point[0] >= next.
width ||
681 inext_point[1] < -track_height_ ||
682 (std::uint32_t)inext_point[1] >= next.
height) {
688 n.
u = next_pts[ptidx][0];
689 n.
v = next_pts[ptidx][1];
692 inext_point[0] = std::floor(next_pts[ptidx][0]);
693 inext_point[1] = std::floor(next_pts[ptidx][1]);
694 iprev_point[0] = std::floor((*prev_keypoints)[ptidx].u);
695 iprev_point[1] = std::floor((*prev_keypoints)[ptidx].v);
696 const PointInT& prev_pt =
697 (*prev_input)[iprev_point[1] * prev_input->width + iprev_point[0]];
698 const PointInT& next_pt =
699 (*input)[inext_point[1] * input->width + inext_point[0]];
700 transformation_computer.
add(
701 prev_pt.getVector3fMap(), next_pt.getVector3fMap(), 1.0);
709 template <
typename Po
intInT,
typename IntensityT>
716 std::vector<FloatImageConstPtr> pyramid;
719 keypoints->
reserve(keypoints_->size());
720 std::vector<int> status(keypoints_->size(), 0);
721 track(ref_, input_, ref_pyramid_, pyramid, keypoints_, keypoints, status, motion_);
724 ref_pyramid_ = pyramid;
725 keypoints_ = keypoints;
726 *keypoints_status_ = status;
void push_back(const PointT &pt)
Insert a new point in the cloud, at the end of the container.
void resize(std::size_t count)
Resizes the container to contain count elements.
std::uint32_t width
The point cloud width (if organized as an image-structure).
std::uint32_t height
The point cloud height (if organized as an image-structure).
void reserve(std::size_t n)
shared_ptr< PointCloud< PointT > > Ptr
shared_ptr< const PointCloud< PointT > > ConstPtr
void mismatchVector(const Eigen::ArrayXXf &prev, const Eigen::ArrayXXf &prev_grad_x, const Eigen::ArrayXXf &prev_grad_y, const FloatImage &next, const Eigen::Array2i &location, const Eigen::Array4f &weights, Eigen::Array2f &b) const
void derivatives(const FloatImage &src, FloatImage &grad_x, FloatImage &grad_y) const
compute Scharr derivatives of a source cloud.
bool initCompute() override
This method should get called before starting the actual computation.
typename PointCloudIn::ConstPtr PointCloudInConstPtr
FloatImage::Ptr FloatImagePtr
void setPointsToTrack(const pcl::PointIndicesConstPtr &points)
Provide a pointer to points to track.
void computeTracking() override
Abstract tracking method.
void setTrackingWindowSize(int width, int height)
set the tracking window size
virtual void computePyramids(const PointCloudInConstPtr &input, std::vector< FloatImageConstPtr > &pyramid, pcl::InterpolationType border_type) const
Compute the pyramidal representation of an image.
void downsample(const FloatImageConstPtr &input, FloatImageConstPtr &output) const
downsample input
virtual void track(const PointCloudInConstPtr &previous_input, const PointCloudInConstPtr ¤t_input, const std::vector< FloatImageConstPtr > &previous_pyramid, const std::vector< FloatImageConstPtr > ¤t_pyramid, const pcl::PointCloud< pcl::PointUV >::ConstPtr &previous_keypoints, pcl::PointCloud< pcl::PointUV >::Ptr ¤t_keypoints, std::vector< int > &status, Eigen::Affine3f &motion) const
virtual void spatialGradient(const FloatImage &img, const FloatImage &grad_x, const FloatImage &grad_y, const Eigen::Array2i &location, const Eigen::Array4f &weights, Eigen::ArrayXXf &win, Eigen::ArrayXXf &grad_x_win, Eigen::ArrayXXf &grad_y_win, Eigen::Array3f &covariance) const
extract the patch from the previous image, previous image gradients surrounding pixel alocation while...
FloatImage::ConstPtr FloatImageConstPtr
void convolveRows(const FloatImageConstPtr &input, FloatImage &output) const
Convolve image rows.
void convolveCols(const FloatImageConstPtr &input, FloatImage &output) const
Convolve image columns.
void convolve(const FloatImageConstPtr &input, FloatImage &output) const
Separately convolve image with decomposable convolution kernel.
Define methods for measuring time spent in code blocks.
void copyPointCloud(const pcl::PointCloud< PointInT > &cloud_in, pcl::PointCloud< PointOutT > &cloud_out)
Copy all the fields from a given point cloud into a new point cloud.
PointIndices::ConstPtr PointIndicesConstPtr
A 2D point structure representing pixel image coordinates.