Point Cloud Library (PCL)  1.11.1-dev
pyramidal_klt.hpp
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Point Cloud Library (PCL) - www.pointclouds.org
5  * Copyright (c) 2014-, Open Perception, Inc.
6  *
7  * All rights reserved.
8  *
9  * Redistribution and use in source and binary forms, with or without
10  * modification, are permitted provided that the following conditions
11  * are met:
12  *
13  * * Redistributions of source code must retain the above copyright
14  * notice, this list of conditions and the following disclaimer.
15  * * Redistributions in binary form must reproduce the above
16  * copyright notice, this list of conditions and the following
17  * disclaimer in the documentation and/or other materials provided
18  * with the distribution.
19  * * Neither the name of Willow Garage, Inc. nor the names of its
20  * contributors may be used to endorse or promote products derived
21  * from this software without specific prior written permission.
22  *
23  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
24  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
25  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
26  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
27  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
28  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
29  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
30  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
31  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
32  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
33  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
34  * POSSIBILITY OF SUCH DAMAGE.
35  *
36  */
37 
38 #ifndef PCL_TRACKING_IMPL_PYRAMIDAL_KLT_HPP
39 #define PCL_TRACKING_IMPL_PYRAMIDAL_KLT_HPP
40 
41 #include <pcl/common/io.h>
42 #include <pcl/common/time.h>
43 #include <pcl/common/utils.h>
44 
45 namespace pcl {
46 namespace tracking {
47 ///////////////////////////////////////////////////////////////////////////////////////////////
48 template <typename PointInT, typename IntensityT>
49 inline void
51 {
52  track_width_ = width;
53  track_height_ = height;
54 }
55 
56 ///////////////////////////////////////////////////////////////////////////////////////////////
57 template <typename PointInT, typename IntensityT>
58 inline void
61 {
62  if (keypoints->size() <= keypoints_nbr_)
63  keypoints_ = keypoints;
64  else {
66  p->reserve(keypoints_nbr_);
67  for (std::size_t i = 0; i < keypoints_nbr_; ++i)
68  p->push_back((*keypoints)[i]);
69  keypoints_ = p;
70  }
71 
72  keypoints_status_.reset(new pcl::PointIndices);
73  keypoints_status_->indices.resize(keypoints_->size(), 0);
74 }
75 
76 ///////////////////////////////////////////////////////////////////////////////////////////////
77 template <typename PointInT, typename IntensityT>
78 inline void
80  const pcl::PointIndicesConstPtr& points)
81 {
82  assert((input_ || ref_) && "[PyramidalKLTTracker] CALL setInputCloud FIRST!");
83 
85  keypoints->reserve(keypoints_nbr_);
86  for (std::size_t i = 0; i < keypoints_nbr_; ++i) {
87  pcl::PointUV uv;
88  uv.u = points->indices[i] % input_->width;
89  uv.v = points->indices[i] / input_->width;
90  keypoints->push_back(uv);
91  }
92  setPointsToTrack(keypoints);
93 }
94 
95 ///////////////////////////////////////////////////////////////////////////////////////////////
96 template <typename PointInT, typename IntensityT>
97 bool
99 {
100  // std::cout << ">>> [PyramidalKLTTracker::initCompute]" << std::endl;
102  PCL_ERROR("[%s::initCompute] PCLBase::Init failed.\n", tracker_name_.c_str());
103  return (false);
104  }
105 
106  if (!input_->isOrganized()) {
107  PCL_ERROR(
108  "[pcl::tracking::%s::initCompute] Need an organized point cloud to proceed!\n",
109  tracker_name_.c_str());
110  return (false);
111  }
112 
113  if (!keypoints_ || keypoints_->empty()) {
114  PCL_ERROR("[pcl::tracking::%s::initCompute] No keypoints aborting!\n",
115  tracker_name_.c_str());
116  return (false);
117  }
118 
119  // This is the first call
120  if (!ref_) {
121  ref_ = input_;
122  // std::cout << "First run!!!" << std::endl;
123 
124  if ((track_height_ * track_width_) % 2 == 0) {
125  PCL_ERROR(
126  "[pcl::tracking::%s::initCompute] Tracking window (%dx%d) must be odd!\n",
127  tracker_name_.c_str(),
128  track_width_,
129  track_height_);
130  return (false);
131  }
132 
133  if (track_height_ < 3 || track_width_ < 3) {
134  PCL_ERROR(
135  "[pcl::tracking::%s::initCompute] Tracking window (%dx%d) must be >= 3x3!\n",
136  tracker_name_.c_str(),
137  track_width_,
138  track_height_);
139  return (false);
140  }
141 
142  track_width_2_ = track_width_ / 2;
143  track_height_2_ = track_height_ / 2;
144 
145  if (nb_levels_ < 2) {
146  PCL_ERROR("[pcl::tracking::%s::initCompute] Number of pyramid levels should be "
147  "at least 2!\n",
148  tracker_name_.c_str());
149  return (false);
150  }
151 
152  if (nb_levels_ > 5) {
153  PCL_ERROR("[pcl::tracking::%s::initCompute] Number of pyramid levels should not "
154  "exceed 5!\n",
155  tracker_name_.c_str());
156  return (false);
157  }
158 
159  computePyramids(ref_, ref_pyramid_, pcl::BORDER_REFLECT_101);
160  return (true);
161  }
162 
163  initialized_ = true;
164 
165  return (true);
166 }
167 
168 ///////////////////////////////////////////////////////////////////////////////////////////////
169 template <typename PointInT, typename IntensityT>
170 void
172  FloatImage& grad_x,
173  FloatImage& grad_y) const
174 {
175  // std::cout << ">>> derivatives" << std::endl;
176  ////////////////////////////////////////////////////////
177  // Use Shcarr operator to compute derivatives. //
178  // Vertical kernel +3 +10 +3 = [1 0 -1]T * [3 10 3] //
179  // 0 0 0 //
180  // -3 -10 -3 //
181  // Horizontal kernel +3 0 -3 = [3 10 3]T * [1 0 -1] //
182  // +10 0 -10 //
183  // +3 0 -3 //
184  ////////////////////////////////////////////////////////
185  if (grad_x.size() != src.size() || grad_x.width != src.width ||
186  grad_x.height != src.height)
187  grad_x = FloatImage(src.width, src.height);
188  if (grad_y.size() != src.size() || grad_y.width != src.width ||
189  grad_y.height != src.height)
190  grad_y = FloatImage(src.width, src.height);
191 
192  int height = src.height, width = src.width;
193  float* row0 = new float[src.width + 2];
194  float* row1 = new float[src.width + 2];
195  float* trow0 = row0;
196  ++trow0;
197  float* trow1 = row1;
198  ++trow1;
199  const float* src_ptr = &(src[0]);
200 
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;
204  const float* srow2 =
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]);
208 
209  // do vertical convolution
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];
213  }
214 
215  // make border
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];
221 
222  // do horizontal convolution and store results
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;
226  }
227  }
228 }
229 
230 ///////////////////////////////////////////////////////////////////////////////////////////////
231 template <typename PointInT, typename IntensityT>
232 void
234  FloatImageConstPtr& output) const
235 {
236  FloatImage smoothed(input->width, input->height);
237  convolve(input, smoothed);
238 
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)
243  ii[i] = 2 * i;
244 
245  FloatImagePtr down(new FloatImage(width, height));
246  // clang-format off
247 #pragma omp parallel for \
248  default(none) \
249  shared(down, height, output, smoothed, width) \
250  firstprivate(ii) \
251  num_threads(threads_)
252  // clang-format on
253  for (int j = 0; j < height; ++j) {
254  int jj = 2 * j;
255  for (int i = 0; i < width; ++i)
256  (*down)(i, j) = smoothed(ii[i], jj);
257  }
258 
259  output = down;
260 }
261 
262 ///////////////////////////////////////////////////////////////////////////////////////////////
263 template <typename PointInT, typename IntensityT>
264 void
266  const FloatImageConstPtr& input,
267  FloatImageConstPtr& output,
268  FloatImageConstPtr& output_grad_x,
269  FloatImageConstPtr& output_grad_y) const
270 {
271  downsample(input, output);
272  FloatImagePtr grad_x(new FloatImage(input->width, input->height));
273  FloatImagePtr grad_y(new FloatImage(input->width, input->height));
274  derivatives(*output, *grad_x, *grad_y);
275  output_grad_x = grad_x;
276  output_grad_y = grad_y;
277 }
278 
279 ///////////////////////////////////////////////////////////////////////////////////////////////
280 template <typename PointInT, typename IntensityT>
281 void
283  const FloatImageConstPtr& input, FloatImage& output) const
284 {
285  FloatImagePtr tmp(new FloatImage(input->width, input->height));
286  convolveRows(input, *tmp);
287  convolveCols(tmp, output);
288 }
289 
290 ///////////////////////////////////////////////////////////////////////////////////////////////
291 template <typename PointInT, typename IntensityT>
292 void
294  const FloatImageConstPtr& input, FloatImage& output) const
295 {
296  int width = input->width;
297  int height = input->height;
298  int last = input->width - kernel_size_2_;
299  int w = last - 1;
300 
301  // clang-format off
302 #pragma omp parallel for \
303  default(none) \
304  shared(input, height, last, output, w, width) \
305  num_threads(threads_)
306  // clang-format on
307  for (int j = 0; j < height; ++j) {
308  for (int i = kernel_size_2_; i < last; ++i) {
309  double result = 0;
310  for (int k = kernel_last_, l = i - kernel_size_2_; k > -1; --k, ++l)
311  result += kernel_[k] * (*input)(l, j);
312 
313  output(i, j) = static_cast<float>(result);
314  }
315 
316  for (int i = last; i < width; ++i)
317  output(i, j) = output(w, j);
318 
319  for (int i = 0; i < kernel_size_2_; ++i)
320  output(i, j) = output(kernel_size_2_, j);
321  }
322 }
323 
324 ///////////////////////////////////////////////////////////////////////////////////////////////
325 template <typename PointInT, typename IntensityT>
326 void
328  FloatImage& output) const
329 {
330  output = FloatImage(input->width, input->height);
331 
332  int width = input->width;
333  int height = input->height;
334  int last = input->height - kernel_size_2_;
335  int h = last - 1;
336 
337  // clang-format off
338 #pragma omp parallel for \
339  default(none) \
340  shared(input, h, height, last, output, width) \
341  num_threads(threads_)
342  // clang-format on
343  for (int i = 0; i < width; ++i) {
344  for (int j = kernel_size_2_; j < last; ++j) {
345  double result = 0;
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);
349  }
350 
351  for (int j = last; j < height; ++j)
352  output(i, j) = output(i, h);
353 
354  for (int j = 0; j < kernel_size_2_; ++j)
355  output(i, j) = output(i, kernel_size_2_);
356  }
357 }
358 
359 ///////////////////////////////////////////////////////////////////////////////////////////////
360 template <typename PointInT, typename IntensityT>
361 void
363  const PointCloudInConstPtr& input,
364  std::vector<FloatImageConstPtr>& pyramid,
365  pcl::InterpolationType border_type) const
366 {
367  int step = 3;
368  pyramid.resize(step * nb_levels_);
369 
370  FloatImageConstPtr previous;
371  FloatImagePtr tmp(new FloatImage(input->width, input->height));
372  // clang-format off
373 #pragma omp parallel for \
374  default(none) \
375  shared(input, tmp) \
376  num_threads(threads_)
377  // clang-format on
378  for (int i = 0; i < static_cast<int>(input->size()); ++i)
379  (*tmp)[i] = intensity_((*input)[i]);
380  previous = tmp;
381 
382  FloatImagePtr img(new FloatImage(previous->width + 2 * track_width_,
383  previous->height + 2 * track_height_));
384 
385  pcl::copyPointCloud(*tmp,
386  *img,
387  track_height_,
388  track_height_,
389  track_width_,
390  track_width_,
391  border_type,
392  0.f);
393  pyramid[0] = img;
394 
395  // compute first level gradients
396  FloatImagePtr g_x(new FloatImage(input->width, input->height));
397  FloatImagePtr g_y(new FloatImage(input->width, input->height));
398  derivatives(*img, *g_x, *g_y);
399  // copy to bigger clouds
400  FloatImagePtr grad_x(new FloatImage(previous->width + 2 * track_width_,
401  previous->height + 2 * track_height_));
402  pcl::copyPointCloud(*g_x,
403  *grad_x,
404  track_height_,
405  track_height_,
406  track_width_,
407  track_width_,
409  0.f);
410  pyramid[1] = grad_x;
411 
412  FloatImagePtr grad_y(new FloatImage(previous->width + 2 * track_width_,
413  previous->height + 2 * track_height_));
414  pcl::copyPointCloud(*g_y,
415  *grad_y,
416  track_height_,
417  track_height_,
418  track_width_,
419  track_width_,
421  0.f);
422  pyramid[2] = grad_y;
423 
424  for (int level = 1; level < nb_levels_; ++level) {
425  // compute current level and current level gradients
426  FloatImageConstPtr current;
427  FloatImageConstPtr g_x;
428  FloatImageConstPtr g_y;
429  downsample(previous, current, g_x, g_y);
430  // copy to bigger clouds
431  FloatImagePtr image(new FloatImage(current->width + 2 * track_width_,
432  current->height + 2 * track_height_));
433  pcl::copyPointCloud(*current,
434  *image,
435  track_height_,
436  track_height_,
437  track_width_,
438  track_width_,
439  border_type,
440  0.f);
441  pyramid[level * step] = image;
442  FloatImagePtr gradx(
443  new FloatImage(g_x->width + 2 * track_width_, g_x->height + 2 * track_height_));
444  pcl::copyPointCloud(*g_x,
445  *gradx,
446  track_height_,
447  track_height_,
448  track_width_,
449  track_width_,
451  0.f);
452  pyramid[level * step + 1] = gradx;
453  FloatImagePtr grady(
454  new FloatImage(g_y->width + 2 * track_width_, g_y->height + 2 * track_height_));
455  pcl::copyPointCloud(*g_y,
456  *grady,
457  track_height_,
458  track_height_,
459  track_width_,
460  track_width_,
462  0.f);
463  pyramid[level * step + 2] = grady;
464  // set the new level
465  previous = current;
466  }
467 }
468 
469 ///////////////////////////////////////////////////////////////////////////////////////////////
470 template <typename PointInT, typename IntensityT>
471 void
473  const FloatImage& img,
474  const FloatImage& grad_x,
475  const FloatImage& grad_y,
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
482 {
483  const int step = img.width;
484  covariance.setZero();
485 
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];
490 
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();
494 
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];
502  //!!! store those
503  *grad_x_win_ptr++ = ixval;
504  *grad_y_win_ptr++ = iyval;
505  // covariance components
506  covariance[0] += ixval * ixval;
507  covariance[1] += ixval * iyval;
508  covariance[2] += iyval * iyval;
509  }
510  }
511 }
512 
513 ///////////////////////////////////////////////////////////////////////////////////////////////
514 template <typename PointInT, typename IntensityT>
515 void
517  const Eigen::ArrayXXf& prev,
518  const Eigen::ArrayXXf& prev_grad_x,
519  const Eigen::ArrayXXf& prev_grad_y,
520  const FloatImage& next,
521  const Eigen::Array2i& location,
522  const Eigen::Array4f& weight,
523  Eigen::Array2f& b) const
524 {
525  const int step = next.width;
526  b.setZero();
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();
532 
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] -
536  prev_ptr[x];
537  b[0] += *prev_grad_x_ptr * diff;
538  b[1] += *prev_grad_y_ptr * diff;
539  }
540  }
541 }
542 
543 ///////////////////////////////////////////////////////////////////////////////////////////////
544 template <typename PointInT, typename IntensityT>
545 void
547  const PointCloudInConstPtr& prev_input,
548  const PointCloudInConstPtr& input,
549  const std::vector<FloatImageConstPtr>& prev_pyramid,
550  const std::vector<FloatImageConstPtr>& pyramid,
551  const pcl::PointCloud<pcl::PointUV>::ConstPtr& prev_keypoints,
553  std::vector<int>& status,
554  Eigen::Affine3f& motion) const
555 {
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);
559  pcl::TransformationFromCorrespondences transformation_computer;
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]);
566 
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)
576  next_pt = prev_pt;
577  else
578  next_pt = next_pts[ptidx] * 2.f;
579 
580  next_pts[ptidx] = next_pt;
581 
582  Eigen::Array2i iprev_point;
583  prev_pt -= half_win;
584  iprev_point[0] = std::floor(prev_pt[0]);
585  iprev_point[1] = std::floor(prev_pt[1]);
586 
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) {
591  if (level == 0)
592  status[ptidx] = -1;
593  continue;
594  }
595 
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];
603 
604  Eigen::Array3f covar = Eigen::Array3f::Zero();
605  spatialGradient(prev,
606  grad_x,
607  grad_y,
608  iprev_point,
609  weight,
610  prev_win,
611  grad_x_win,
612  grad_y_win,
613  covar);
614 
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])) /
619  2.f;
620 
621  if (min_eigenvalue < min_eigenvalue_threshold_ ||
622  det < std::numeric_limits<float>::epsilon()) {
623  status[ptidx] = -2;
624  continue;
625  }
626 
627  det = 1.f / det;
628  next_pt -= half_win;
629 
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>();
633 
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) {
636  if (level == 0)
637  status[ptidx] = -1;
638  break;
639  }
640 
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];
647  // compute mismatch vector
648  Eigen::Array2f beta = Eigen::Array2f::Zero();
649  mismatchVector(prev_win, grad_x_win, grad_y_win, next, inext_pt, weight, beta);
650  // optical flow resolution
651  Eigen::Vector2f delta((covar[1] * beta[1] - covar[2] * beta[0]) * det,
652  (covar[1] * beta[0] - covar[0] * beta[1]) * det);
653  // update position
654  next_pt[0] += delta[0];
655  next_pt[1] += delta[1];
656  next_pts[ptidx] = next_pt + half_win;
657 
658  if (delta.squaredNorm() <= epsilon_)
659  break;
660 
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;
665  break;
666  }
667  // update delta
668  prev_delta = delta;
669  }
670 
671  // update tracked points
672  if (level == 0 && !status[ptidx]) {
673  Eigen::Array2f next_point = next_pts[ptidx] - half_win;
674  Eigen::Array2i inext_point;
675 
676  inext_point[0] = std::floor(next_point[0]);
677  inext_point[1] = std::floor(next_point[1]);
678 
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) {
683  status[ptidx] = -1;
684  continue;
685  }
686  // insert valid keypoint
687  pcl::PointUV n;
688  n.u = next_pts[ptidx][0];
689  n.v = next_pts[ptidx][1];
690  keypoints->push_back(n);
691  // add points pair to compute transformation
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);
702  }
703  }
704  }
705  motion = transformation_computer.getTransformation();
706 }
707 
708 ///////////////////////////////////////////////////////////////////////////////////////////////
709 template <typename PointInT, typename IntensityT>
710 void
712 {
713  if (!initialized_)
714  return;
715 
716  std::vector<FloatImageConstPtr> pyramid;
717  computePyramids(input_, pyramid, pcl::BORDER_REFLECT_101);
719  keypoints->reserve(keypoints_->size());
720  std::vector<int> status(keypoints_->size(), 0);
721  track(ref_, input_, ref_pyramid_, pyramid, keypoints_, keypoints, status, motion_);
722  // swap reference and input
723  ref_ = input_;
724  ref_pyramid_ = pyramid;
725  keypoints_ = keypoints;
726  keypoints_status_->indices = status;
727 }
728 
729 } // namespace tracking
730 } // namespace pcl
731 
732 #endif
pcl::PointUV
A 2D point structure representing pixel image coordinates.
Definition: point_types.hpp:761
pcl::tracking::PyramidalKLTTracker::mismatchVector
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
Definition: pyramidal_klt.hpp:516
pcl
Definition: convolution.h:46
pcl::PointCloud::height
std::uint32_t height
The point cloud height (if organized as an image-structure).
Definition: point_cloud.h:394
pcl::tracking::PyramidalKLTTracker::convolveCols
void convolveCols(const FloatImageConstPtr &input, FloatImage &output) const
Convolve image columns.
Definition: pyramidal_klt.hpp:327
pcl::PointUV::u
float u
Definition: point_types.hpp:763
pcl::tracking::PyramidalKLTTracker::downsample
void downsample(const FloatImageConstPtr &input, FloatImageConstPtr &output) const
downsample input
Definition: pyramidal_klt.hpp:233
pcl::PointUV::v
float v
Definition: point_types.hpp:764
pcl::tracking::PyramidalKLTTracker::derivatives
void derivatives(const FloatImage &src, FloatImage &grad_x, FloatImage &grad_y) const
compute Scharr derivatives of a source cloud.
Definition: pyramidal_klt.hpp:171
pcl::BORDER_CONSTANT
@ BORDER_CONSTANT
Definition: io.h:223
pcl::PCLBase
PCL base class.
Definition: pcl_base.h:69
pcl::tracking::PyramidalKLTTracker::initCompute
bool initCompute() override
This method should get called before starting the actual computation.
Definition: pyramidal_klt.hpp:98
pcl::PointCloud< pcl::PointUV >
pcl::copyPointCloud
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.
Definition: io.hpp:122
pcl::TransformationFromCorrespondences
Calculates a transformation based on corresponding 3D points.
Definition: transformation_from_correspondences.h:49
pcl::BORDER_REFLECT_101
@ BORDER_REFLECT_101
Definition: io.h:225
pcl::tracking::PyramidalKLTTracker::FloatImageConstPtr
FloatImage::ConstPtr FloatImageConstPtr
Definition: pyramidal_klt.h:72
pcl::PointCloud::width
std::uint32_t width
The point cloud width (if organized as an image-structure).
Definition: point_cloud.h:392
pcl::tracking::PyramidalKLTTracker::computeTracking
void computeTracking() override
Abstract tracking method.
Definition: pyramidal_klt.hpp:711
pcl::PointIndicesConstPtr
PointIndices::ConstPtr PointIndicesConstPtr
Definition: PointIndices.h:25
pcl::tracking::PyramidalKLTTracker::convolveRows
void convolveRows(const FloatImageConstPtr &input, FloatImage &output) const
Convolve image rows.
Definition: pyramidal_klt.hpp:293
pcl::tracking::PyramidalKLTTracker::computePyramids
virtual void computePyramids(const PointCloudInConstPtr &input, std::vector< FloatImageConstPtr > &pyramid, pcl::InterpolationType border_type) const
Compute the pyramidal representation of an image.
Definition: pyramidal_klt.hpp:362
pcl::tracking::PyramidalKLTTracker::setTrackingWindowSize
void setTrackingWindowSize(int width, int height)
set the tracking window size
Definition: pyramidal_klt.hpp:50
pcl::tracking::PyramidalKLTTracker::convolve
void convolve(const FloatImageConstPtr &input, FloatImage &output) const
Separately convolve image with decomposable convolution kernel.
Definition: pyramidal_klt.hpp:282
pcl::PointCloud::reserve
void reserve(std::size_t n)
Definition: point_cloud.h:439
pcl::tracking::PyramidalKLTTracker::PointCloudInConstPtr
typename PointCloudIn::ConstPtr PointCloudInConstPtr
Definition: pyramidal_klt.h:69
pcl::InterpolationType
InterpolationType
Definition: io.h:221
pcl::tracking::PyramidalKLTTracker::FloatImagePtr
FloatImage::Ptr FloatImagePtr
Definition: pyramidal_klt.h:71
pcl::PointCloud::resize
void resize(std::size_t count)
Resizes the container to contain count elements.
Definition: point_cloud.h:456
pcl::PointIndices
Definition: PointIndices.h:11
pcl::TransformationFromCorrespondences::getTransformation
Eigen::Affine3f getTransformation()
Calculate the transformation that will best transform the points into their correspondences.
Definition: transformation_from_correspondences.hpp:77
pcl::PointCloud::size
std::size_t size() const
Definition: point_cloud.h:437
pcl::tracking::PyramidalKLTTracker::track
virtual void track(const PointCloudInConstPtr &previous_input, const PointCloudInConstPtr &current_input, const std::vector< FloatImageConstPtr > &previous_pyramid, const std::vector< FloatImageConstPtr > &current_pyramid, const pcl::PointCloud< pcl::PointUV >::ConstPtr &previous_keypoints, pcl::PointCloud< pcl::PointUV >::Ptr &current_keypoints, std::vector< int > &status, Eigen::Affine3f &motion) const
Definition: pyramidal_klt.hpp:546
pcl::TransformationFromCorrespondences::add
void add(const Eigen::Vector3f &point, const Eigen::Vector3f &corresponding_point, float weight=1.0)
Add a new sample.
Definition: transformation_from_correspondences.hpp:58
pcl::PointCloud::Ptr
shared_ptr< PointCloud< PointT > > Ptr
Definition: point_cloud.h:407
time.h
pcl::tracking::PyramidalKLTTracker::setPointsToTrack
void setPointsToTrack(const pcl::PointIndicesConstPtr &points)
Provide a pointer to points to track.
Definition: pyramidal_klt.hpp:79
pcl::PointCloud::ConstPtr
shared_ptr< const PointCloud< PointT > > ConstPtr
Definition: point_cloud.h:408
pcl::PointCloud::push_back
void push_back(const PointT &pt)
Insert a new point in the cloud, at the end of the container.
Definition: point_cloud.h:642
pcl::tracking::PyramidalKLTTracker::spatialGradient
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...
Definition: pyramidal_klt.hpp:472