Point Cloud Library (PCL)  1.14.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 std::vector<int>);
73  keypoints_status_->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  delete[] row0;
229  delete[] row1;
230 }
231 
232 ///////////////////////////////////////////////////////////////////////////////////////////////
233 template <typename PointInT, typename IntensityT>
234 void
236  FloatImageConstPtr& output) const
237 {
238  FloatImage smoothed(input->width, input->height);
239  convolve(input, smoothed);
240 
241  int width = (smoothed.width + 1) / 2;
242  int height = (smoothed.height + 1) / 2;
243  std::vector<int> ii(width);
244  for (int i = 0; i < width; ++i)
245  ii[i] = 2 * i;
246 
247  FloatImagePtr down(new FloatImage(width, height));
248  // clang-format off
249 #pragma omp parallel for \
250  default(none) \
251  shared(down, height, output, smoothed, width) \
252  firstprivate(ii) \
253  num_threads(threads_)
254  // clang-format on
255  for (int j = 0; j < height; ++j) {
256  int jj = 2 * j;
257  for (int i = 0; i < width; ++i)
258  (*down)(i, j) = smoothed(ii[i], jj);
259  }
260 
261  output = down;
262 }
263 
264 ///////////////////////////////////////////////////////////////////////////////////////////////
265 template <typename PointInT, typename IntensityT>
266 void
268  const FloatImageConstPtr& input,
269  FloatImageConstPtr& output,
270  FloatImageConstPtr& output_grad_x,
271  FloatImageConstPtr& output_grad_y) const
272 {
273  downsample(input, output);
274  FloatImagePtr grad_x(new FloatImage(input->width, input->height));
275  FloatImagePtr grad_y(new FloatImage(input->width, input->height));
276  derivatives(*output, *grad_x, *grad_y);
277  output_grad_x = grad_x;
278  output_grad_y = grad_y;
279 }
280 
281 ///////////////////////////////////////////////////////////////////////////////////////////////
282 template <typename PointInT, typename IntensityT>
283 void
285  const FloatImageConstPtr& input, FloatImage& output) const
286 {
287  FloatImagePtr tmp(new FloatImage(input->width, input->height));
288  convolveRows(input, *tmp);
289  convolveCols(tmp, output);
290 }
291 
292 ///////////////////////////////////////////////////////////////////////////////////////////////
293 template <typename PointInT, typename IntensityT>
294 void
296  const FloatImageConstPtr& input, FloatImage& output) const
297 {
298  int width = input->width;
299  int height = input->height;
300  int last = input->width - kernel_size_2_;
301  int w = last - 1;
302 
303  // clang-format off
304 #pragma omp parallel for \
305  default(none) \
306  shared(input, height, last, output, w, width) \
307  num_threads(threads_)
308  // clang-format on
309  for (int j = 0; j < height; ++j) {
310  for (int i = kernel_size_2_; i < last; ++i) {
311  double result = 0;
312  for (int k = kernel_last_, l = i - kernel_size_2_; k > -1; --k, ++l)
313  result += kernel_[k] * (*input)(l, j);
314 
315  output(i, j) = static_cast<float>(result);
316  }
317 
318  for (int i = last; i < width; ++i)
319  output(i, j) = output(w, j);
320 
321  for (int i = 0; i < kernel_size_2_; ++i)
322  output(i, j) = output(kernel_size_2_, j);
323  }
324 }
325 
326 ///////////////////////////////////////////////////////////////////////////////////////////////
327 template <typename PointInT, typename IntensityT>
328 void
330  FloatImage& output) const
331 {
332  output = FloatImage(input->width, input->height);
333 
334  int width = input->width;
335  int height = input->height;
336  int last = input->height - kernel_size_2_;
337  int h = last - 1;
338 
339  // clang-format off
340 #pragma omp parallel for \
341  default(none) \
342  shared(input, h, height, last, output, width) \
343  num_threads(threads_)
344  // clang-format on
345  for (int i = 0; i < width; ++i) {
346  for (int j = kernel_size_2_; j < last; ++j) {
347  double result = 0;
348  for (int k = kernel_last_, l = j - kernel_size_2_; k > -1; --k, ++l)
349  result += kernel_[k] * (*input)(i, l);
350  output(i, j) = static_cast<float>(result);
351  }
352 
353  for (int j = last; j < height; ++j)
354  output(i, j) = output(i, h);
355 
356  for (int j = 0; j < kernel_size_2_; ++j)
357  output(i, j) = output(i, kernel_size_2_);
358  }
359 }
360 
361 ///////////////////////////////////////////////////////////////////////////////////////////////
362 template <typename PointInT, typename IntensityT>
363 void
365  const PointCloudInConstPtr& input,
366  std::vector<FloatImageConstPtr>& pyramid,
367  pcl::InterpolationType border_type) const
368 {
369  int step = 3;
370  pyramid.resize(step * nb_levels_);
371 
372  FloatImageConstPtr previous;
373  FloatImagePtr tmp(new FloatImage(input->width, input->height));
374  // clang-format off
375 #pragma omp parallel for \
376  default(none) \
377  shared(input, tmp) \
378  num_threads(threads_)
379  // clang-format on
380  for (int i = 0; i < static_cast<int>(input->size()); ++i)
381  (*tmp)[i] = intensity_((*input)[i]);
382  previous = tmp;
383 
384  FloatImagePtr img(new FloatImage(previous->width + 2 * track_width_,
385  previous->height + 2 * track_height_));
386 
387  pcl::copyPointCloud(*tmp,
388  *img,
389  track_height_,
390  track_height_,
391  track_width_,
392  track_width_,
393  border_type,
394  0.f);
395  pyramid[0] = img;
396 
397  // compute first level gradients
398  FloatImagePtr g_x(new FloatImage(input->width, input->height));
399  FloatImagePtr g_y(new FloatImage(input->width, input->height));
400  derivatives(*img, *g_x, *g_y);
401  // copy to bigger clouds
402  FloatImagePtr grad_x(new FloatImage(previous->width + 2 * track_width_,
403  previous->height + 2 * track_height_));
404  pcl::copyPointCloud(*g_x,
405  *grad_x,
406  track_height_,
407  track_height_,
408  track_width_,
409  track_width_,
411  0.f);
412  pyramid[1] = grad_x;
413 
414  FloatImagePtr grad_y(new FloatImage(previous->width + 2 * track_width_,
415  previous->height + 2 * track_height_));
416  pcl::copyPointCloud(*g_y,
417  *grad_y,
418  track_height_,
419  track_height_,
420  track_width_,
421  track_width_,
423  0.f);
424  pyramid[2] = grad_y;
425 
426  for (int level = 1; level < nb_levels_; ++level) {
427  // compute current level and current level gradients
428  FloatImageConstPtr current;
429  FloatImageConstPtr g_x;
430  FloatImageConstPtr g_y;
431  downsample(previous, current, g_x, g_y);
432  // copy to bigger clouds
433  FloatImagePtr image(new FloatImage(current->width + 2 * track_width_,
434  current->height + 2 * track_height_));
435  pcl::copyPointCloud(*current,
436  *image,
437  track_height_,
438  track_height_,
439  track_width_,
440  track_width_,
441  border_type,
442  0.f);
443  pyramid[level * step] = image;
444  FloatImagePtr gradx(
445  new FloatImage(g_x->width + 2 * track_width_, g_x->height + 2 * track_height_));
446  pcl::copyPointCloud(*g_x,
447  *gradx,
448  track_height_,
449  track_height_,
450  track_width_,
451  track_width_,
453  0.f);
454  pyramid[level * step + 1] = gradx;
455  FloatImagePtr grady(
456  new FloatImage(g_y->width + 2 * track_width_, g_y->height + 2 * track_height_));
457  pcl::copyPointCloud(*g_y,
458  *grady,
459  track_height_,
460  track_height_,
461  track_width_,
462  track_width_,
464  0.f);
465  pyramid[level * step + 2] = grady;
466  // set the new level
467  previous = current;
468  }
469 }
470 
471 ///////////////////////////////////////////////////////////////////////////////////////////////
472 template <typename PointInT, typename IntensityT>
473 void
475  const FloatImage& img,
476  const FloatImage& grad_x,
477  const FloatImage& grad_y,
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
484 {
485  const int step = img.width;
486  covariance.setZero();
487 
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];
492 
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();
496 
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];
504  //!!! store those
505  *grad_x_win_ptr++ = ixval;
506  *grad_y_win_ptr++ = iyval;
507  // covariance components
508  covariance[0] += ixval * ixval;
509  covariance[1] += ixval * iyval;
510  covariance[2] += iyval * iyval;
511  }
512  }
513 }
514 
515 ///////////////////////////////////////////////////////////////////////////////////////////////
516 template <typename PointInT, typename IntensityT>
517 void
519  const Eigen::ArrayXXf& prev,
520  const Eigen::ArrayXXf& prev_grad_x,
521  const Eigen::ArrayXXf& prev_grad_y,
522  const FloatImage& next,
523  const Eigen::Array2i& location,
524  const Eigen::Array4f& weight,
525  Eigen::Array2f& b) const
526 {
527  const int step = next.width;
528  b.setZero();
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();
534 
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] -
538  prev_ptr[x];
539  b[0] += *prev_grad_x_ptr * diff;
540  b[1] += *prev_grad_y_ptr * diff;
541  }
542  }
543 }
544 
545 ///////////////////////////////////////////////////////////////////////////////////////////////
546 template <typename PointInT, typename IntensityT>
547 void
549  const PointCloudInConstPtr& prev_input,
550  const PointCloudInConstPtr& input,
551  const std::vector<FloatImageConstPtr>& prev_pyramid,
552  const std::vector<FloatImageConstPtr>& pyramid,
553  const pcl::PointCloud<pcl::PointUV>::ConstPtr& prev_keypoints,
555  std::vector<int>& status,
556  Eigen::Affine3f& motion) const
557 {
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);
561  pcl::TransformationFromCorrespondences transformation_computer;
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]);
568 
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)
578  next_pt = prev_pt;
579  else
580  next_pt = next_pts[ptidx] * 2.f;
581 
582  next_pts[ptidx] = next_pt;
583 
584  Eigen::Array2i iprev_point;
585  prev_pt -= half_win;
586  iprev_point[0] = std::floor(prev_pt[0]);
587  iprev_point[1] = std::floor(prev_pt[1]);
588 
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) {
593  if (level == 0)
594  status[ptidx] = -1;
595  continue;
596  }
597 
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];
605 
606  Eigen::Array3f covar = Eigen::Array3f::Zero();
607  spatialGradient(prev,
608  grad_x,
609  grad_y,
610  iprev_point,
611  weight,
612  prev_win,
613  grad_x_win,
614  grad_y_win,
615  covar);
616 
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])) /
621  2.f;
622 
623  if (min_eigenvalue < min_eigenvalue_threshold_ ||
624  det < std::numeric_limits<float>::epsilon()) {
625  status[ptidx] = -2;
626  continue;
627  }
628 
629  det = 1.f / det;
630  next_pt -= half_win;
631 
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>();
635 
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) {
638  if (level == 0)
639  status[ptidx] = -1;
640  break;
641  }
642 
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];
649  // compute mismatch vector
650  Eigen::Array2f beta = Eigen::Array2f::Zero();
651  mismatchVector(prev_win, grad_x_win, grad_y_win, next, inext_pt, weight, beta);
652  // optical flow resolution
653  Eigen::Vector2f delta((covar[1] * beta[1] - covar[2] * beta[0]) * det,
654  (covar[1] * beta[0] - covar[0] * beta[1]) * det);
655  // update position
656  next_pt[0] += delta[0];
657  next_pt[1] += delta[1];
658  next_pts[ptidx] = next_pt + half_win;
659 
660  if (delta.squaredNorm() <= epsilon_)
661  break;
662 
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;
667  break;
668  }
669  // update delta
670  prev_delta = delta;
671  }
672 
673  // update tracked points
674  if (level == 0 && !status[ptidx]) {
675  Eigen::Array2f next_point = next_pts[ptidx] - half_win;
676  Eigen::Array2i inext_point;
677 
678  inext_point[0] = std::floor(next_point[0]);
679  inext_point[1] = std::floor(next_point[1]);
680 
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) {
685  status[ptidx] = -1;
686  continue;
687  }
688  // insert valid keypoint
689  pcl::PointUV n;
690  n.u = next_pts[ptidx][0];
691  n.v = next_pts[ptidx][1];
692  keypoints->push_back(n);
693  // add points pair to compute transformation
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);
704  }
705  }
706  }
707  motion = transformation_computer.getTransformation();
708 }
709 
710 ///////////////////////////////////////////////////////////////////////////////////////////////
711 template <typename PointInT, typename IntensityT>
712 void
714 {
715  if (!initialized_)
716  return;
717 
718  std::vector<FloatImageConstPtr> pyramid;
719  computePyramids(input_, pyramid, pcl::BORDER_REFLECT_101);
721  keypoints->reserve(keypoints_->size());
722  std::vector<int> status(keypoints_->size(), 0);
723  track(ref_, input_, ref_pyramid_, pyramid, keypoints_, keypoints, status, motion_);
724  // swap reference and input
725  ref_ = input_;
726  ref_pyramid_ = pyramid;
727  keypoints_ = keypoints;
728  *keypoints_status_ = status;
729 }
730 
731 } // namespace tracking
732 } // namespace pcl
733 
734 #endif
PCL base class.
Definition: pcl_base.h:70
void push_back(const PointT &pt)
Insert a new point in the cloud, at the end of the container.
Definition: point_cloud.h:663
void resize(std::size_t count)
Resizes the container to contain count elements.
Definition: point_cloud.h:462
std::uint32_t width
The point cloud width (if organized as an image-structure).
Definition: point_cloud.h:398
std::uint32_t height
The point cloud height (if organized as an image-structure).
Definition: point_cloud.h:400
std::size_t size() const
Definition: point_cloud.h:443
void reserve(std::size_t n)
Definition: point_cloud.h:445
shared_ptr< PointCloud< PointT > > Ptr
Definition: point_cloud.h:413
shared_ptr< const PointCloud< PointT > > ConstPtr
Definition: point_cloud.h:414
Calculates a transformation based on corresponding 3D points.
Eigen::Affine3f getTransformation()
Calculate the transformation that will best transform the points into their correspondences.
void add(const Eigen::Vector3f &point, const Eigen::Vector3f &corresponding_point, float weight=1.0)
Add a new sample.
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
Definition: pyramidal_klt.h:69
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 &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
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 allocation whil...
FloatImage::ConstPtr FloatImageConstPtr
Definition: pyramidal_klt.h:72
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.
Definition: io.hpp:142
InterpolationType
Definition: io.h:255
@ BORDER_REFLECT_101
Definition: io.h:258
@ BORDER_CONSTANT
Definition: io.h:256
PointIndices::ConstPtr PointIndicesConstPtr
Definition: PointIndices.h:24
A 2D point structure representing pixel image coordinates.