Point Cloud Library (PCL)  1.12.1-dev
pyramidal_klt.h
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Point Cloud Library (PCL) - www.pointclouds.org
5  * Copyright (c) 2014-, Open Perception.
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 #pragma once
39 
40 #include <pcl/common/intensity.h>
41 #include <pcl/common/transformation_from_correspondences.h>
42 #include <pcl/tracking/tracker.h>
43 #include <pcl/memory.h>
44 #include <pcl/pcl_macros.h>
45 #include <pcl/point_types.h>
46 
47 namespace pcl {
48 namespace tracking {
49 /** Pyramidal Kanade Lucas Tomasi tracker.
50  * This is an implementation of the Pyramidal Kanade Lucas Tomasi tracker that
51  * operates on organized 3D keypoints with color/intensity information (this is
52  * the default behaviour but you can alterate it by providing another operator
53  * as second template argument). It is an affine tracker that iteratively
54  * computes the optical flow to find the best guess for a point p at t given its
55  * location at t-1. User is advised to respect the Tomasi condition: the
56  * response computed is the maximum eigenvalue of the second moment matrix but
57  * no restrictin are applied to points to track so you can use a detector of
58  * your choice to indicate points to track.
59  *
60  * \author Nizar Sallem
61  */
62 template <typename PointInT,
64 class PyramidalKLTTracker : public Tracker<PointInT, Eigen::Affine3f> {
65 public:
68  using PointCloudInPtr = typename PointCloudIn::Ptr;
69  using PointCloudInConstPtr = typename PointCloudIn::ConstPtr;
73  using Ptr = shared_ptr<PyramidalKLTTracker<PointInT, IntensityT>>;
74  using ConstPtr = shared_ptr<const PyramidalKLTTracker<PointInT, IntensityT>>;
75 
77  using TrackerBase::input_;
79 
80  /** Constructor */
81  PyramidalKLTTracker(int nb_levels = 5,
82  int tracking_window_width = 7,
83  int tracking_window_height = 7)
84  : ref_()
85  , nb_levels_(nb_levels)
86  , track_width_(tracking_window_width)
87  , track_height_(tracking_window_height)
88  , threads_(0)
89  , initialized_(false)
90  {
91  tracker_name_ = "PyramidalKLTTracker";
92  accuracy_ = 0.1;
93  epsilon_ = 1e-3;
94  max_iterations_ = 10;
95  keypoints_nbr_ = 100;
97  kernel_ << 1.f / 16, 1.f / 4, 3.f / 8, 1.f / 4, 1.f / 16;
98  kernel_size_2_ = kernel_.size() / 2;
99  kernel_last_ = kernel_.size() - 1;
100  }
101 
102  /** Destructor */
103  ~PyramidalKLTTracker() override = default;
104 
105  /** \brief Set the number of pyramid levels
106  * \param levels desired number of pyramid levels
107  */
108  inline void
110  {
111  nb_levels_ = levels;
112  }
113 
114  /** \return the number of pyramid levels */
115  inline int
117  {
118  return (nb_levels_);
119  }
120 
121  /** Set accuracy
122  * \param[in] accuracy desired accuracy.
123  */
124  inline void
125  setAccuracy(float accuracy)
126  {
127  accuracy_ = accuracy;
128  }
129 
130  /** \return the accuracy */
131  inline float
132  getAccuracy() const
133  {
134  return (accuracy_);
135  }
136 
137  /** Set epsilon
138  * \param[in] epsilon desired epsilon.
139  */
140  inline void
141  setEpsilon(float epsilon)
142  {
143  epsilon_ = epsilon;
144  }
145 
146  /** \return the epsilon */
147  inline float
148  getEpsilon() const
149  {
150  return (epsilon_);
151  }
152 
153  /** \brief Set the maximum number of points to track after sorting detected keypoints
154  * according to their response measure.
155  * \param[in] number the desired number of points to detect.
156  */
157  inline void
158  setNumberOfKeypoints(std::size_t number)
159  {
160  keypoints_nbr_ = number;
161  }
162 
163  /** \return the maximum number of keypoints to keep */
164  inline std::size_t
166  {
167  return (keypoints_nbr_);
168  }
169 
170  /** \brief set the tracking window size
171  * \param[in] width the tracking window width
172  * \param[in] height the tracking window height
173  */
174  inline void
175  setTrackingWindowSize(int width, int height);
176 
177  /** \brief Set tracking window width */
178  inline void
180  {
181  track_width_ = width;
182  };
183 
184  /** \return the tracking window size */
185  inline int
187  {
188  return (track_width_);
189  }
190 
191  /** \brief Set tracking window height */
192  inline void
194  {
195  track_height_ = height;
196  };
197 
198  /** \return the tracking window size */
199  inline int
201  {
202  return (track_height_);
203  }
204 
205  /** \brief Initialize the scheduler and set the number of threads to use.
206  * \param nr_threads the number of hardware threads to use (0 sets the value
207  * back to automatic).
208  */
209  inline void
210  setNumberOfThreads(unsigned int nr_threads = 0)
211  {
212  threads_ = nr_threads;
213  }
214 
215  /** \brief Get a pointer of the cloud at t-1. */
216  inline PointCloudInConstPtr
218  {
219  return (ref_);
220  }
221 
222  /** \brief Set the maximum number of iterations in the Lucas Kanade loop.
223  * \param[in] max the desired maximum number of iterations
224  */
225  inline void
226  setMaxIterationsNumber(unsigned int max)
227  {
228  max_iterations_ = max;
229  }
230 
231  /** \return the maximum iterations number */
232  inline unsigned int
234  {
235  return (max_iterations_);
236  }
237 
238  /** \brief Provide a pointer to points to track.
239  * \param points the const boost shared pointer to a PointIndices message
240  */
241  inline void
243 
244  /** \brief Provide a pointer to points to track.
245  * \param points the const boost shared pointer to a PointIndices message
246  */
247  inline void
249 
250  /** \return a pointer to the points successfully tracked. */
253  {
254  return (keypoints_);
255  };
256 
257  /** \return the status of points to track.
258  * Status == 0 --> points successfully tracked;
259  * Status < 0 --> point is lost;
260  * Status == -1 --> point is out of bond;
261  * Status == -2 --> optical flow can not be computed for this point.
262  */
263  PCL_DEPRECATED(1, 15, "use getStatusOfPointsToTrack instead")
264  inline pcl::PointIndicesConstPtr
266  {
268  res->indices.insert(
269  res->indices.end(), keypoints_status_->begin(), keypoints_status_->end());
270  return (res);
271  }
272 
273  /** \return the status of points to track.
274  * Status == 0 --> points successfully tracked;
275  * Status < 0 --> point is lost;
276  * Status == -1 --> point is out of bond;
277  * Status == -2 --> optical flow can not be computed for this point.
278  */
279  inline pcl::shared_ptr<const std::vector<int>>
281  {
282  return (keypoints_status_);
283  }
284 
285  /** \brief Return the computed transformation from tracked points. */
286  Eigen::Affine3f
287  getResult() const override
288  {
289  return (motion_);
290  }
291 
292  /** \return initialization state */
293  bool
295  {
296  return (initialized_);
297  }
298 
299 protected:
300  bool
301  initCompute() override;
302 
303  /** \brief compute Scharr derivatives of a source cloud.
304  * \param[in] src the image for which gradients are to be computed
305  * \param[out] grad_x image gradient along X direction
306  * \param[out] grad_y image gradient along Y direction
307  */
308  void
309  derivatives(const FloatImage& src, FloatImage& grad_x, FloatImage& grad_y) const;
310 
311  /** \brief downsample input
312  * \param[in] input the image to downsample
313  * \param[out] output the downsampled image
314  */
315  void
316  downsample(const FloatImageConstPtr& input, FloatImageConstPtr& output) const;
317 
318  /** \brief downsample input and compute output gradients.
319  * \param[in] input the image to downsample
320  * \param[out] output the downsampled image
321  * \param[out] output_grad_x downsampled image gradient along X direction
322  * \param[out] output_grad_y downsampled image gradient along Y direction
323  */
324  void
325  downsample(const FloatImageConstPtr& input,
326  FloatImageConstPtr& output,
327  FloatImageConstPtr& output_grad_x,
328  FloatImageConstPtr& output_grad_y) const;
329 
330  /** \brief Separately convolve image with decomposable convolution kernel.
331  * \param[in] input input the image to convolve
332  * \param[out] output output the convolved image
333  */
334  void
335  convolve(const FloatImageConstPtr& input, FloatImage& output) const;
336 
337  /** \brief Convolve image columns.
338  * \param[in] input input the image to convolve
339  * \param[out] output output the convolved image
340  */
341  void
342  convolveCols(const FloatImageConstPtr& input, FloatImage& output) const;
343 
344  /** \brief Convolve image rows.
345  * \param[in] input input the image to convolve
346  * \param[out] output output the convolved image
347  */
348  void
349  convolveRows(const FloatImageConstPtr& input, FloatImage& output) const;
350 
351  /** \brief extract the patch from the previous image, previous image gradients
352  * surrounding pixel alocation while interpolating image and gradients data
353  * and compute covariation matrix of derivatives.
354  * \param[in] img original image
355  * \param[in] grad_x original image gradient along X direction
356  * \param[in] grad_y original image gradient along Y direction
357  * \param[in] location pixel at the center of the patch
358  * \param[in] weights bilinear interpolation weights at this location computed from
359  * subpixel location
360  * \param[out] win patch with interpolated intensity values
361  * \param[out] grad_x_win patch with interpolated gradient along X values
362  * \param[out] grad_y_win patch with interpolated gradient along Y values
363  * \param[out] covariance covariance matrix coefficients
364  */
365  virtual void
366  spatialGradient(const FloatImage& img,
367  const FloatImage& grad_x,
368  const FloatImage& grad_y,
369  const Eigen::Array2i& location,
370  const Eigen::Array4f& weights,
371  Eigen::ArrayXXf& win,
372  Eigen::ArrayXXf& grad_x_win,
373  Eigen::ArrayXXf& grad_y_win,
374  Eigen::Array3f& covariance) const;
375  void
376  mismatchVector(const Eigen::ArrayXXf& prev,
377  const Eigen::ArrayXXf& prev_grad_x,
378  const Eigen::ArrayXXf& prev_grad_y,
379  const FloatImage& next,
380  const Eigen::Array2i& location,
381  const Eigen::Array4f& weights,
382  Eigen::Array2f& b) const;
383 
384  /** \brief Compute the pyramidal representation of an image.
385  * \param[in] input the input cloud
386  * \param[out] pyramid computed pyramid levels along with their respective
387  * gradients
388  * \param[in] border_type
389  */
390  virtual void
392  std::vector<FloatImageConstPtr>& pyramid,
393  pcl::InterpolationType border_type) const;
394 
395  virtual void
396  track(const PointCloudInConstPtr& previous_input,
397  const PointCloudInConstPtr& current_input,
398  const std::vector<FloatImageConstPtr>& previous_pyramid,
399  const std::vector<FloatImageConstPtr>& current_pyramid,
400  const pcl::PointCloud<pcl::PointUV>::ConstPtr& previous_keypoints,
401  pcl::PointCloud<pcl::PointUV>::Ptr& current_keypoints,
402  std::vector<int>& status,
403  Eigen::Affine3f& motion) const;
404 
405  void
406  computeTracking() override;
407 
408  /** \brief input pyranid at t-1 */
409  std::vector<FloatImageConstPtr> ref_pyramid_;
410  /** \brief point cloud at t-1 */
412  /** \brief number of pyramid levels */
414  /** \brief detected keypoints 2D coordinates */
416  /** \brief status of keypoints of t-1 at t */
417  pcl::shared_ptr<std::vector<int>> keypoints_status_;
418  /** \brief number of points to detect */
419  std::size_t keypoints_nbr_;
420  /** \brief tracking width */
422  /** \brief half of tracking window width */
424  /** \brief tracking height */
426  /** \brief half of tracking window height */
428  /** \brief maximum number of iterations */
429  unsigned int max_iterations_;
430  /** \brief accuracy criterion to stop iterating */
431  float accuracy_;
433  /** \brief epsilon for subpixel computation */
434  float epsilon_;
436  /** \brief number of hardware threads */
437  unsigned int threads_;
438  /** \brief intensity accessor */
439  IntensityT intensity_;
440  /** \brief is the tracker initialized ? */
442  /** \brief compute transformation from successfully tracked points */
444  /** \brief computed transformation between tracked points */
445  Eigen::Affine3f motion_;
446  /** \brief smoothing kernel */
447  Eigen::Array<float, 5, 1> kernel_;
448  /** \brief smoothing kernel half size */
450  /** \brief index of last element in kernel */
452 
453 public:
455 };
456 } // namespace tracking
457 } // namespace pcl
458 
459 #include <pcl/tracking/impl/pyramidal_klt.hpp>
PointCloudConstPtr input_
The input point cloud dataset.
Definition: pcl_base.h:147
IndicesPtr indices_
A pointer to the vector of point indices to use.
Definition: pcl_base.h:150
shared_ptr< PointCloud< float > > Ptr
Definition: point_cloud.h:413
shared_ptr< const PointCloud< float > > ConstPtr
Definition: point_cloud.h:414
Calculates a transformation based on corresponding 3D points.
Pyramidal Kanade Lucas Tomasi tracker.
Definition: pyramidal_klt.h:64
pcl::shared_ptr< std::vector< int > > keypoints_status_
status of keypoints of t-1 at t
void setMaxIterationsNumber(unsigned int max)
Set the maximum number of iterations in the Lucas Kanade loop.
unsigned int getMaxIterationsNumber() const
pcl::shared_ptr< const std::vector< int > > getStatusOfPointsToTrack() const
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
int kernel_last_
index of last element in kernel
void derivatives(const FloatImage &src, FloatImage &grad_x, FloatImage &grad_y) const
compute Scharr derivatives of a source cloud.
void setNumberOfPyramidLevels(int levels)
Set the number of pyramid levels.
std::size_t keypoints_nbr_
number of points to detect
int track_width_2_
half of tracking window width
int track_height_2_
half of tracking window height
Eigen::Affine3f getResult() const override
Return the computed transformation from tracked points.
int kernel_size_2_
smoothing kernel half size
int nb_levels_
number of pyramid levels
~PyramidalKLTTracker() override=default
Destructor.
bool initCompute() override
This method should get called before starting the actual computation.
std::vector< FloatImageConstPtr > ref_pyramid_
input pyranid at t-1
void setNumberOfThreads(unsigned int nr_threads=0)
Initialize the scheduler and set the number of threads to use.
void setAccuracy(float accuracy)
Set accuracy.
pcl::PointCloud< pcl::PointUV >::ConstPtr getTrackedPoints() const
void setEpsilon(float epsilon)
Set epsilon.
Eigen::Affine3f motion_
computed transformation between tracked points
typename PointCloudIn::ConstPtr PointCloudInConstPtr
Definition: pyramidal_klt.h:69
PyramidalKLTTracker(int nb_levels=5, int tracking_window_width=7, int tracking_window_height=7)
Constructor.
Definition: pyramidal_klt.h:81
typename PointCloudIn::Ptr PointCloudInPtr
Definition: pyramidal_klt.h:68
float epsilon_
epsilon for subpixel computation
IntensityT intensity_
intensity accessor
void setPointsToTrack(const pcl::PointIndicesConstPtr &points)
Provide a pointer to points to track.
bool initialized_
is the tracker initialized ?
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.
shared_ptr< PyramidalKLTTracker< PointInT, IntensityT > > Ptr
Definition: pyramidal_klt.h:73
void downsample(const FloatImageConstPtr &input, FloatImageConstPtr &output) const
downsample input
void setTrackingWindowHeight(int height)
Set tracking window height.
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 alocation while...
Eigen::Array< float, 5, 1 > kernel_
smoothing kernel
unsigned int max_iterations_
maximum number of iterations
typename TrackerBase::PointCloudIn PointCloudIn
Definition: pyramidal_klt.h:67
float accuracy_
accuracy criterion to stop iterating
void setTrackingWindowWidth(int width)
Set tracking window width.
FloatImage::ConstPtr FloatImageConstPtr
Definition: pyramidal_klt.h:72
pcl::PointIndicesConstPtr getPointsToTrackStatus() const
unsigned int threads_
number of hardware threads
PointCloudInConstPtr getReferenceCloud() const
Get a pointer of the cloud at t-1.
void convolveRows(const FloatImageConstPtr &input, FloatImage &output) const
Convolve image rows.
pcl::TransformationFromCorrespondences transformation_computer_
compute transformation from successfully tracked points
void setNumberOfKeypoints(std::size_t number)
Set the maximum number of points to track after sorting detected keypoints according to their respons...
pcl::PointCloud< float > FloatImage
Definition: pyramidal_klt.h:70
void convolveCols(const FloatImageConstPtr &input, FloatImage &output) const
Convolve image columns.
shared_ptr< const PyramidalKLTTracker< PointInT, IntensityT > > ConstPtr
Definition: pyramidal_klt.h:74
void convolve(const FloatImageConstPtr &input, FloatImage &output) const
Separately convolve image with decomposable convolution kernel.
PointCloudInConstPtr ref_
point cloud at t-1
pcl::PointCloud< pcl::PointUV >::ConstPtr keypoints_
detected keypoints 2D coordinates
Tracker represents the base tracker class.
Definition: tracker.h:55
std::string tracker_name_
The tracker name.
Definition: tracker.h:90
pcl::PointCloud< PointInT > PointCloudIn
Definition: tracker.h:70
Defines all the PCL implemented PointT point type structures.
#define PCL_MAKE_ALIGNED_OPERATOR_NEW
Macro to signal a class requires a custom allocator.
Definition: memory.h:63
Defines functions, macros and traits for allocating and using memory.
InterpolationType
Definition: io.h:247
PointIndices::Ptr PointIndicesPtr
Definition: PointIndices.h:23
PointIndices::ConstPtr PointIndicesConstPtr
Definition: PointIndices.h:24
Defines all the PCL and non-PCL macros used.
#define PCL_DEPRECATED(Major, Minor, Message)
macro for compatibility across compilers and help remove old deprecated items for the Major....
Definition: pcl_macros.h:156