Point Cloud Library (PCL)  1.11.0
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/time.h>
42 #include <pcl/common/utils.h>
43 #include <pcl/common/io.h>
44 #include <pcl/common/utils.h>
45 
46 
47 namespace pcl
48 {
49 
50 namespace tracking
51 {
52 
53 template <typename PointInT, typename IntensityT> inline void
55 {
56  track_width_ = width;
57  track_height_ = height;
58 }
59 
60 
61 template <typename PointInT, typename IntensityT> inline void
63 {
64  if (keypoints->size () <= keypoints_nbr_)
65  keypoints_ = keypoints;
66  else
67  {
69  p->reserve (keypoints_nbr_);
70  for (std::size_t i = 0; i < keypoints_nbr_; ++i)
71  p->push_back (keypoints->points[i]);
72  keypoints_ = p;
73  }
74 
75  keypoints_status_.reset (new pcl::PointIndices);
76  keypoints_status_->indices.resize (keypoints_->size (), 0);
77 }
78 
79 
80 template <typename PointInT, typename IntensityT> inline void
82 {
83  assert ((input_ || ref_) && "[pcl::tracking::PyramidalKLTTracker] CALL setInputCloud FIRST!");
84 
86  keypoints->reserve (keypoints_nbr_);
87  for (std::size_t i = 0; i < keypoints_nbr_; ++i)
88  {
89  pcl::PointUV uv;
90  uv.u = points->indices[i] % input_->width;
91  uv.v = points->indices[i] / input_->width;
92  keypoints->push_back (uv);
93  }
94  setPointsToTrack (keypoints);
95 }
96 
97 
98 template <typename PointInT, typename IntensityT> bool
100 {
101  // std::cout << ">>> [PyramidalKLTTracker::initCompute]" << std::endl;
103  {
104  PCL_ERROR ("[pcl::tracking::%s::initCompute] PCLBase::Init failed.\n",
105  tracker_name_.c_str ());
106  return (false);
107  }
108 
109  if (!input_->isOrganized ())
110  {
111  PCL_ERROR ("[pcl::tracking::%s::initCompute] Need an organized point cloud to proceed!",
112  tracker_name_.c_str ());
113  return (false);
114  }
115 
116  if (!keypoints_ || keypoints_->empty ())
117  {
118  PCL_ERROR ("[pcl::tracking::%s::initCompute] No keypoints aborting!",
119  tracker_name_.c_str ());
120  return (false);
121  }
122 
123  // This is the first call
124  if (!ref_)
125  {
126  ref_ = input_;
127  // std::cout << "First run!!!" << std::endl;
128 
129  if ((track_height_ * track_width_)%2 == 0)
130  {
131  PCL_ERROR ("[pcl::tracking::%s::initCompute] Tracking window (%dx%d) must be odd!\n",
132  tracker_name_.c_str (), track_width_, track_height_);
133  return (false);
134  }
135 
136  if (track_height_ < 3 || track_width_ < 3)
137  {
138  PCL_ERROR ("[pcl::tracking::%s::initCompute] Tracking window (%dx%d) must be >= 3x3!\n",
139  tracker_name_.c_str (), track_width_, track_height_);
140  return (false);
141  }
142 
143  track_width_2_ = track_width_ / 2;
144  track_height_2_ = track_height_ / 2;
145 
146  if (nb_levels_ < 2)
147  {
148  PCL_ERROR ("[pcl::tracking::%s::initCompute] Number of pyramid levels should be at least 2!",
149  tracker_name_.c_str ());
150  return (false);
151  }
152 
153  if (nb_levels_ > 5)
154  {
155  PCL_ERROR ("[pcl::tracking::%s::initCompute] Number of pyramid levels should not exceed 5!",
156  tracker_name_.c_str ());
157  return (false);
158  }
159 
160  computePyramids (ref_, ref_pyramid_, pcl::BORDER_REFLECT_101);
161  return (true);
162  }
163 
164  initialized_ = true;
165 
166  return (true);
167 }
168 
169 
170 template <typename PointInT, typename IntensityT> void
172 {
173  // std::cout << ">>> derivatives" << std::endl;
174  ////////////////////////////////////////////////////////
175  // Use Shcarr operator to compute derivatives. //
176  // Vertical kernel +3 +10 +3 = [1 0 -1]T * [3 10 3] //
177  // 0 0 0 //
178  // -3 -10 -3 //
179  // Horizontal kernel +3 0 -3 = [3 10 3]T * [1 0 -1] //
180  // +10 0 -10 //
181  // +3 0 -3 //
182  ////////////////////////////////////////////////////////
183  if (grad_x.size () != src.size () || grad_x.width != src.width || grad_x.height != src.height)
184  grad_x = FloatImage (src.width, src.height);
185  if (grad_y.size () != src.size () || grad_y.width != src.width || grad_y.height != src.height)
186  grad_y = FloatImage (src.width, src.height);
187 
188  int height = src.height, width = src.width;
189  float *row0 = new float [src.width + 2];
190  float *row1 = new float [src.width + 2];
191  float *trow0 = row0; ++trow0;
192  float *trow1 = row1; ++trow1;
193  const float* src_ptr = &(src.points[0]);
194 
195  for (int y = 0; y < height; y++)
196  {
197  const float* srow0 = src_ptr + (y > 0 ? y-1 : height > 1 ? 1 : 0) * width;
198  const float* srow1 = src_ptr + y * width;
199  const float* srow2 = src_ptr + (y < height-1 ? y+1 : height > 1 ? height-2 : 0) * width;
200  float* grad_x_row = &(grad_x.points[y * width]);
201  float* grad_y_row = &(grad_y.points[y * width]);
202 
203  // do vertical convolution
204  for (int x = 0; x < width; x++)
205  {
206  trow0[x] = (srow0[x] + srow2[x])*3 + srow1[x]*10;
207  trow1[x] = srow2[x] - srow0[x];
208  }
209 
210  // make border
211  int x0 = width > 1 ? 1 : 0, x1 = width > 1 ? width-2 : 0;
212  trow0[-1] = trow0[x0]; trow0[width] = trow0[x1];
213  trow1[-1] = trow1[x0]; trow1[width] = trow1[x1];
214 
215  // do horizontal convolution and store results
216  for (int x = 0; x < width; x++)
217  {
218  grad_x_row[x] = trow0[x+1] - trow0[x-1];
219  grad_y_row[x] = (trow1[x+1] + trow1[x-1])*3 + trow1[x]*10;
220  }
221  }
222 }
223 
224 
225 template <typename PointInT, typename IntensityT> void
227  FloatImageConstPtr& output) const
228 {
229  FloatImage smoothed (input->width, input->height);
230  convolve (input, smoothed);
231 
232  int width = (smoothed.width +1) / 2;
233  int height = (smoothed.height +1) / 2;
234  std::vector<int> ii (width);
235  for (int i = 0; i < width; ++i)
236  ii[i] = 2 * i;
237 
238  FloatImagePtr down (new FloatImage (width, height));
239 #pragma omp parallel for \
240  default(none) \
241  shared(down, height, output, smoothed, width) \
242  firstprivate(ii) \
243  num_threads(threads_)
244  for (int j = 0; j < height; ++j)
245  {
246  int jj = 2*j;
247  for (int i = 0; i < width; ++i)
248  (*down) (i,j) = smoothed (ii[i],jj);
249  }
250 
251  output = down;
252 }
253 
254 
255 template <typename PointInT, typename IntensityT> void
257  FloatImageConstPtr& output,
258  FloatImageConstPtr& output_grad_x,
259  FloatImageConstPtr& output_grad_y) const
260 {
261  downsample (input, output);
262  FloatImagePtr grad_x (new FloatImage (input->width, input->height));
263  FloatImagePtr grad_y (new FloatImage (input->width, input->height));
264  derivatives (*output, *grad_x, *grad_y);
265  output_grad_x = grad_x;
266  output_grad_y = grad_y;
267 }
268 
269 
270 template <typename PointInT, typename IntensityT> void
272 {
273  FloatImagePtr tmp (new FloatImage (input->width, input->height));
274  convolveRows (input, *tmp);
275  convolveCols (tmp, output);
276 }
277 
278 
279 template <typename PointInT, typename IntensityT> void
281 {
282  int width = input->width;
283  int height = input->height;
284  int last = input->width - kernel_size_2_;
285  int w = last - 1;
286 
287 #pragma omp parallel for \
288  default(none) \
289  shared(input, height, last, output, w, width) \
290  num_threads(threads_)
291  for (int j = 0; j < height; ++j)
292  {
293  for (int i = kernel_size_2_; i < last; ++i)
294  {
295  double result = 0;
296  for (int k = kernel_last_, l = i - kernel_size_2_; k > -1; --k, ++l)
297  result+= kernel_[k] * (*input) (l,j);
298 
299  output (i,j) = static_cast<float> (result);
300  }
301 
302  for (int i = last; i < width; ++i)
303  output (i,j) = output (w, j);
304 
305  for (int i = 0; i < kernel_size_2_; ++i)
306  output (i,j) = output (kernel_size_2_, j);
307  }
308 }
309 
310 
311 template <typename PointInT, typename IntensityT> void
313 {
314  output = FloatImage (input->width, input->height);
315 
316  int width = input->width;
317  int height = input->height;
318  int last = input->height - kernel_size_2_;
319  int h = last -1;
320 
321 #pragma omp parallel for \
322  default(none) \
323  shared(input, h, height, last, output, width) \
324  num_threads(threads_)
325  for (int i = 0; i < width; ++i)
326  {
327  for (int j = kernel_size_2_; j < last; ++j)
328  {
329  double result = 0;
330  for (int k = kernel_last_, l = j - kernel_size_2_; k > -1; --k, ++l)
331  result += kernel_[k] * (*input) (i,l);
332  output (i,j) = static_cast<float> (result);
333  }
334 
335  for (int j = last; j < height; ++j)
336  output (i,j) = output (i,h);
337 
338  for (int j = 0; j < kernel_size_2_; ++j)
339  output (i,j) = output (i, kernel_size_2_);
340  }
341 }
342 
343 
344 template <typename PointInT, typename IntensityT> void
346  std::vector<FloatImageConstPtr>& pyramid,
347  pcl::InterpolationType border_type) const
348 {
349  int step = 3;
350  pyramid.resize (step * nb_levels_);
351 
352  FloatImageConstPtr previous;
353  FloatImagePtr tmp (new FloatImage (input->width, input->height));
354 #pragma omp parallel for \
355  default(none) \
356  shared(input, tmp) \
357  num_threads(threads_)
358  for (int i = 0; i < static_cast<int> (input->size ()); ++i)
359  tmp->points[i] = intensity_ (input->points[i]);
360  previous = tmp;
361 
362  FloatImagePtr img (new FloatImage (previous->width + 2*track_width_,
363  previous->height + 2*track_height_));
364 
365  pcl::copyPointCloud (*tmp, *img, track_height_, track_height_, track_width_, track_width_,
366  border_type, 0.f);
367  pyramid[0] = img;
368 
369  // compute first level gradients
370  FloatImagePtr g_x (new FloatImage (input->width, input->height));
371  FloatImagePtr g_y (new FloatImage (input->width, input->height));
372  derivatives (*img, *g_x, *g_y);
373  // copy to bigger clouds
374  FloatImagePtr grad_x (new FloatImage (previous->width + 2*track_width_,
375  previous->height + 2*track_height_));
376  pcl::copyPointCloud (*g_x, *grad_x, track_height_, track_height_, track_width_, track_width_,
377  pcl::BORDER_CONSTANT, 0.f);
378  pyramid[1] = grad_x;
379 
380  FloatImagePtr grad_y (new FloatImage (previous->width + 2*track_width_,
381  previous->height + 2*track_height_));
382  pcl::copyPointCloud (*g_y, *grad_y, track_height_, track_height_, track_width_, track_width_,
383  pcl::BORDER_CONSTANT, 0.f);
384  pyramid[2] = grad_y;
385 
386  for (int level = 1; level < nb_levels_; ++level)
387  {
388  // compute current level and current level gradients
389  FloatImageConstPtr current;
390  FloatImageConstPtr g_x;
391  FloatImageConstPtr g_y;
392  downsample (previous, current, g_x, g_y);
393  // copy to bigger clouds
394  FloatImagePtr image (new FloatImage (current->width + 2*track_width_,
395  current->height + 2*track_height_));
396  pcl::copyPointCloud (*current, *image, track_height_, track_height_, track_width_, track_width_,
397  border_type, 0.f);
398  pyramid[level*step] = image;
399  FloatImagePtr gradx (new FloatImage (g_x->width + 2*track_width_, g_x->height + 2*track_height_));
400  pcl::copyPointCloud (*g_x, *gradx, track_height_, track_height_, track_width_, track_width_,
401  pcl::BORDER_CONSTANT, 0.f);
402  pyramid[level*step + 1] = gradx;
403  FloatImagePtr grady (new FloatImage (g_y->width + 2*track_width_, g_y->height + 2*track_height_));
404  pcl::copyPointCloud (*g_y, *grady, track_height_, track_height_, track_width_, track_width_,
405  pcl::BORDER_CONSTANT, 0.f);
406  pyramid[level*step + 2] = grady;
407  // set the new level
408  previous = current;
409  }
410 }
411 
412 
413 template <typename PointInT, typename IntensityT> void
415  const FloatImage& grad_x,
416  const FloatImage& grad_y,
417  const Eigen::Array2i& location,
418  const Eigen::Array4f& weight,
419  Eigen::ArrayXXf& win,
420  Eigen::ArrayXXf& grad_x_win,
421  Eigen::ArrayXXf& grad_y_win,
422  Eigen::Array3f &covariance) const
423 {
424  const int step = img.width;
425  covariance.setZero ();
426 
427  for (int y = 0; y < track_height_; y++)
428  {
429  const float* img_ptr = &(img.points[0]) + (y + location[1])*step + location[0];
430  const float* grad_x_ptr = &(grad_x.points[0]) + (y + location[1])*step + location[0];
431  const float* grad_y_ptr = &(grad_y.points[0]) + (y + location[1])*step + location[0];
432 
433  float* win_ptr = win.data () + y*win.cols ();
434  float* grad_x_win_ptr = grad_x_win.data () + y*grad_x_win.cols ();
435  float* grad_y_win_ptr = grad_y_win.data () + y*grad_y_win.cols ();
436 
437  for (int x =0; x < track_width_; ++x, ++grad_x_ptr, ++grad_y_ptr)
438  {
439  *win_ptr++ = img_ptr[x]*weight[0] + img_ptr[x+1]*weight[1] + img_ptr[x+step]*weight[2] + img_ptr[x+step+1]*weight[3];
440  float ixval = grad_x_ptr[0]*weight[0] + grad_x_ptr[1]*weight[1] + grad_x_ptr[step]*weight[2] + grad_x_ptr[step+1]*weight[3];
441  float iyval = grad_y_ptr[0]*weight[0] + grad_y_ptr[1]*weight[1] + grad_y_ptr[step]*weight[2] + grad_y_ptr[step+1]*weight[3];
442  //!!! store those
443  *grad_x_win_ptr++ = ixval;
444  *grad_y_win_ptr++ = iyval;
445  //covariance components
446  covariance[0] += ixval*ixval;
447  covariance[1] += ixval*iyval;
448  covariance[2] += iyval*iyval;
449  }
450  }
451 }
452 
453 
454 template <typename PointInT, typename IntensityT> void
456  const Eigen::ArrayXXf& prev_grad_x,
457  const Eigen::ArrayXXf& prev_grad_y,
458  const FloatImage& next,
459  const Eigen::Array2i& location,
460  const Eigen::Array4f& weight,
461  Eigen::Array2f &b) const
462 {
463  const int step = next.width;
464  b.setZero ();
465  for (int y = 0; y < track_height_; y++)
466  {
467  const float* next_ptr = &(next.points[0]) + (y + location[1])*step + location[0];
468  const float* prev_ptr = prev.data () + y*prev.cols ();
469  const float* prev_grad_x_ptr = prev_grad_x.data () + y*prev_grad_x.cols ();
470  const float* prev_grad_y_ptr = prev_grad_y.data () + y*prev_grad_y.cols ();
471 
472  for (int x = 0; x < track_width_; ++x, ++prev_grad_y_ptr, ++prev_grad_x_ptr)
473  {
474  float diff = next_ptr[x]*weight[0] + next_ptr[x+1]*weight[1]
475  + next_ptr[x+step]*weight[2] + next_ptr[x+step+1]*weight[3] - prev_ptr[x];
476  b[0] += *prev_grad_x_ptr * diff;
477  b[1] += *prev_grad_y_ptr * diff;
478  }
479  }
480 }
481 
482 
483 template <typename PointInT, typename IntensityT> void
485  const PointCloudInConstPtr& input,
486  const std::vector<FloatImageConstPtr>& prev_pyramid,
487  const std::vector<FloatImageConstPtr>& pyramid,
488  const pcl::PointCloud<pcl::PointUV>::ConstPtr& prev_keypoints,
490  std::vector<int>& status,
491  Eigen::Affine3f& motion) const
492 {
493  std::vector<Eigen::Array2f, Eigen::aligned_allocator<Eigen::Array2f> > next_pts (prev_keypoints->size ());
494  Eigen::Array2f half_win ((track_width_-1)*0.5f, (track_height_-1)*0.5f);
495  pcl::TransformationFromCorrespondences transformation_computer;
496  const int nb_points = prev_keypoints->size ();
497  for (int level = nb_levels_ - 1; level >= 0; --level)
498  {
499  const FloatImage& prev = *(prev_pyramid[level*3]);
500  const FloatImage& next = *(pyramid[level*3]);
501  const FloatImage& grad_x = *(prev_pyramid[level*3+1]);
502  const FloatImage& grad_y = *(prev_pyramid[level*3+2]);
503 
504  Eigen::ArrayXXf prev_win (track_height_, track_width_);
505  Eigen::ArrayXXf grad_x_win (track_height_, track_width_);
506  Eigen::ArrayXXf grad_y_win (track_height_, track_width_);
507  float ratio (1./(1 << level));
508  for (int ptidx = 0; ptidx < nb_points; ptidx++)
509  {
510  Eigen::Array2f prev_pt (prev_keypoints->points[ptidx].u * ratio,
511  prev_keypoints->points[ptidx].v * ratio);
512  Eigen::Array2f next_pt;
513  if (level == nb_levels_ -1)
514  next_pt = prev_pt;
515  else
516  next_pt = next_pts[ptidx]*2.f;
517 
518  next_pts[ptidx] = next_pt;
519 
520  Eigen::Array2i iprev_point;
521  prev_pt -= half_win;
522  iprev_point[0] = std::floor (prev_pt[0]);
523  iprev_point[1] = std::floor (prev_pt[1]);
524 
525  if (iprev_point[0] < -track_width_ || (std::uint32_t) iprev_point[0] >= grad_x.width ||
526  iprev_point[1] < -track_height_ || (std::uint32_t) iprev_point[1] >= grad_y.height)
527  {
528  if (level == 0)
529  status [ptidx] = -1;
530  continue;
531  }
532 
533  float a = prev_pt[0] - iprev_point[0];
534  float b = prev_pt[1] - iprev_point[1];
535  Eigen::Array4f weight;
536  weight[0] = (1.f - a)*(1.f - b);
537  weight[1] = a*(1.f - b);
538  weight[2] = (1.f - a)*b;
539  weight[3] = 1 - weight[0] - weight[1] - weight[2];
540 
541  Eigen::Array3f covar = Eigen::Array3f::Zero ();
542  spatialGradient (prev, grad_x, grad_y, iprev_point, weight, prev_win, grad_x_win, grad_y_win, covar);
543 
544  float det = covar[0]*covar[2] - covar[1]*covar[1];
545  float min_eigenvalue = (covar[2] + covar[0] - std::sqrt ((covar[0]-covar[2])*(covar[0]-covar[2]) + 4.f*covar[1]*covar[1]))/2.f;
546 
547  if (min_eigenvalue < min_eigenvalue_threshold_ || det < std::numeric_limits<float>::epsilon ())
548  {
549  status[ptidx] = -2;
550  continue;
551  }
552 
553  det = 1.f/det;
554  next_pt -= half_win;
555 
556  Eigen::Array2f prev_delta (0, 0);
557  for (unsigned int j = 0; j < max_iterations_; j++)
558  {
559  Eigen::Array2i inext_pt = next_pt.floor ().cast<int> ();
560 
561  if (inext_pt[0] < -track_width_ || (std::uint32_t) inext_pt[0] >= next.width ||
562  inext_pt[1] < -track_height_ || (std::uint32_t) inext_pt[1] >= next.height)
563  {
564  if (level == 0)
565  status[ptidx] = -1;
566  break;
567  }
568 
569  a = next_pt[0] - inext_pt[0];
570  b = next_pt[1] - inext_pt[1];
571  weight[0] = (1.f - a)*(1.f - b);
572  weight[1] = a*(1.f - b);
573  weight[2] = (1.f - a)*b;
574  weight[3] = 1 - weight[0] - weight[1] - weight[2];
575  // compute mismatch vector
576  Eigen::Array2f beta = Eigen::Array2f::Zero ();
577  mismatchVector (prev_win, grad_x_win, grad_y_win, next, inext_pt, weight, beta);
578  // optical flow resolution
579  Eigen::Vector2f delta ((covar[1]*beta[1] - covar[2]*beta[0])*det, (covar[1]*beta[0] - covar[0]*beta[1])*det);
580  // update position
581  next_pt[0] += delta[0]; next_pt[1] += delta[1];
582  next_pts[ptidx] = next_pt + half_win;
583 
584  if (delta.squaredNorm () <= epsilon_)
585  break;
586 
587  if (j > 0 && std::abs (delta[0] + prev_delta[0]) < 0.01 &&
588  std::abs (delta[1] + prev_delta[1]) < 0.01 )
589  {
590  next_pts[ptidx][0] -= delta[0]*0.5f;
591  next_pts[ptidx][1] -= delta[1]*0.5f;
592  break;
593  }
594  // update delta
595  prev_delta = delta;
596  }
597 
598  // update tracked points
599  if (level == 0 && !status[ptidx])
600  {
601  Eigen::Array2f next_point = next_pts[ptidx] - half_win;
602  Eigen::Array2i inext_point;
603 
604  inext_point[0] = std::floor (next_point[0]);
605  inext_point[1] = std::floor (next_point[1]);
606 
607  if (inext_point[0] < -track_width_ || (std::uint32_t) inext_point[0] >= next.width ||
608  inext_point[1] < -track_height_ || (std::uint32_t) inext_point[1] >= next.height)
609  {
610  status[ptidx] = -1;
611  continue;
612  }
613  // insert valid keypoint
614  pcl::PointUV n;
615  n.u = next_pts[ptidx][0];
616  n.v = next_pts[ptidx][1];
617  keypoints->push_back (n);
618  // add points pair to compute transformation
619  inext_point[0] = std::floor (next_pts[ptidx][0]);
620  inext_point[1] = std::floor (next_pts[ptidx][1]);
621  iprev_point[0] = std::floor (prev_keypoints->points[ptidx].u);
622  iprev_point[1] = std::floor (prev_keypoints->points[ptidx].v);
623  const PointInT& prev_pt = prev_input->points[iprev_point[1]*prev_input->width + iprev_point[0]];
624  const PointInT& next_pt = input->points[inext_point[1]*input->width + inext_point[0]];
625  transformation_computer.add (prev_pt.getVector3fMap (), next_pt.getVector3fMap (), 1.0);
626  }
627  }
628  }
629 
630  motion = transformation_computer.getTransformation ();
631 }
632 
633 
634 template <typename PointInT, typename IntensityT> void
636 {
637  if (!initialized_)
638  return;
639 
640  std::vector<FloatImageConstPtr> pyramid;
641  computePyramids (input_, pyramid, pcl::BORDER_REFLECT_101);
643  keypoints->reserve (keypoints_->size ());
644  std::vector<int> status (keypoints_->size (), 0);
645  track (ref_, input_, ref_pyramid_, pyramid, keypoints_, keypoints, status, motion_);
646  //swap reference and input
647  ref_ = input_;
648  ref_pyramid_ = pyramid;
649  keypoints_ = keypoints;
650  keypoints_status_->indices = status;
651 }
652 
653 } // namespace tracking
654 } // namespace pcl
655 
656 #endif
657 
pcl::PointUV
A 2D point structure representing pixel image coordinates.
Definition: point_types.hpp:762
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:455
pcl
Definition: convolution.h:46
pcl::uint32_t
std::uint32_t uint32_t
Definition: types.h:58
pcl::PointCloud::height
std::uint32_t height
The point cloud height (if organized as an image-structure).
Definition: point_cloud.h:415
pcl::tracking::PyramidalKLTTracker::convolveCols
void convolveCols(const FloatImageConstPtr &input, FloatImage &output) const
Convolve image columns.
Definition: pyramidal_klt.hpp:312
pcl::PointCloud::points
std::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
Definition: point_cloud.h:410
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:226
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::PointCloud::resize
void resize(std::size_t n)
Resize the cloud.
Definition: point_cloud.h:455
pcl::BORDER_CONSTANT
@ BORDER_CONSTANT
Definition: io.h:257
pcl::PCLBase
PCL base class.
Definition: pcl_base.h:70
pcl::tracking::PyramidalKLTTracker::initCompute
bool initCompute() override
This method should get called before starting the actual computation.
Definition: pyramidal_klt.hpp:99
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:121
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:259
pcl::tracking::PyramidalKLTTracker::FloatImageConstPtr
FloatImage::ConstPtr FloatImageConstPtr
Definition: pyramidal_klt.h:73
pcl::PointCloud::width
std::uint32_t width
The point cloud width (if organized as an image-structure).
Definition: point_cloud.h:413
pcl::tracking::PyramidalKLTTracker::computeTracking
void computeTracking() override
Abstract tracking method.
Definition: pyramidal_klt.hpp:635
pcl::PointIndicesConstPtr
PointIndices::ConstPtr PointIndicesConstPtr
Definition: PointIndices.h:27
pcl::tracking::PyramidalKLTTracker::convolveRows
void convolveRows(const FloatImageConstPtr &input, FloatImage &output) const
Convolve image rows.
Definition: pyramidal_klt.hpp:280
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:345
pcl::tracking::PyramidalKLTTracker::setTrackingWindowSize
void setTrackingWindowSize(int width, int height)
set the tracking window size
Definition: pyramidal_klt.hpp:54
pcl::tracking::PyramidalKLTTracker::convolve
void convolve(const FloatImageConstPtr &input, FloatImage &output) const
Separately convolve image with decomposable convolution kernel.
Definition: pyramidal_klt.hpp:271
pcl::PointCloud::reserve
void reserve(std::size_t n)
Definition: point_cloud.h:449
pcl::tracking::PyramidalKLTTracker::PointCloudInConstPtr
typename PointCloudIn::ConstPtr PointCloudInConstPtr
Definition: pyramidal_klt.h:70
pcl::InterpolationType
InterpolationType
Definition: io.h:256
pcl::tracking::PyramidalKLTTracker::FloatImagePtr
FloatImage::Ptr FloatImagePtr
Definition: pyramidal_klt.h:72
pcl::PointIndices
Definition: PointIndices.h:14
pcl::TransformationFromCorrespondences::getTransformation
Eigen::Affine3f getTransformation()
Calculate the transformation that will best transform the points into their correspondences.
Definition: transformation_from_correspondences.hpp:78
pcl::PointCloud::size
std::size_t size() const
Definition: point_cloud.h:448
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:484
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:59
pcl::PointCloud::Ptr
shared_ptr< PointCloud< PointT > > Ptr
Definition: point_cloud.h:428
time.h
pcl::tracking::PyramidalKLTTracker::setPointsToTrack
void setPointsToTrack(const pcl::PointIndicesConstPtr &points)
Provide a pointer to points to track.
Definition: pyramidal_klt.hpp:81
pcl::PointCloud::ConstPtr
shared_ptr< const PointCloud< PointT > > ConstPtr
Definition: point_cloud.h:429
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:480
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:414