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