Point Cloud Library (PCL)  1.11.1
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 */
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  */
265  {
266  return (keypoints_status_);
267  }
268 
269  /** \brief Return the computed transformation from tracked points. */
270  Eigen::Affine3f
271  getResult() const override
272  {
273  return (motion_);
274  }
275 
276  /** \return initialization state */
277  bool
279  {
280  return (initialized_);
281  }
282 
283 protected:
284  bool
285  initCompute() override;
286 
287  /** \brief compute Scharr derivatives of a source cloud.
288  * \param[in] src the image for which gradients are to be computed
289  * \param[out] grad_x image gradient along X direction
290  * \param[out] grad_y image gradient along Y direction
291  */
292  void
293  derivatives(const FloatImage& src, FloatImage& grad_x, FloatImage& grad_y) const;
294 
295  /** \brief downsample input
296  * \param[in] input the image to downsample
297  * \param[out] output the downsampled image
298  */
299  void
300  downsample(const FloatImageConstPtr& input, FloatImageConstPtr& output) const;
301 
302  /** \brief downsample input and compute output gradients.
303  * \param[in] input the image to downsample
304  * \param[out] output the downsampled image
305  * \param[out] output_grad_x downsampled image gradient along X direction
306  * \param[out] output_grad_y downsampled image gradient along Y direction
307  */
308  void
309  downsample(const FloatImageConstPtr& input,
310  FloatImageConstPtr& output,
311  FloatImageConstPtr& output_grad_x,
312  FloatImageConstPtr& output_grad_y) const;
313 
314  /** \brief Separately convolve image with decomposable convolution kernel.
315  * \param[in] input input the image to convolve
316  * \param[out] output output the convolved image
317  */
318  void
319  convolve(const FloatImageConstPtr& input, FloatImage& output) const;
320 
321  /** \brief Convolve image columns.
322  * \param[in] input input the image to convolve
323  * \param[out] output output the convolved image
324  */
325  void
326  convolveCols(const FloatImageConstPtr& input, FloatImage& output) const;
327 
328  /** \brief Convolve image rows.
329  * \param[in] input input the image to convolve
330  * \param[out] output output the convolved image
331  */
332  void
333  convolveRows(const FloatImageConstPtr& input, FloatImage& output) const;
334 
335  /** \brief extract the patch from the previous image, previous image gradients
336  * surrounding pixel alocation while interpolating image and gradients data
337  * and compute covariation matrix of derivatives.
338  * \param[in] img original image
339  * \param[in] grad_x original image gradient along X direction
340  * \param[in] grad_y original image gradient along Y direction
341  * \param[in] location pixel at the center of the patch
342  * \param[in] weights bilinear interpolation weights at this location computed from
343  * subpixel location
344  * \param[out] win patch with interpolated intensity values
345  * \param[out] grad_x_win patch with interpolated gradient along X values
346  * \param[out] grad_y_win patch with interpolated gradient along Y values
347  * \param[out] covariance covariance matrix coefficients
348  */
349  virtual void
350  spatialGradient(const FloatImage& img,
351  const FloatImage& grad_x,
352  const FloatImage& grad_y,
353  const Eigen::Array2i& location,
354  const Eigen::Array4f& weights,
355  Eigen::ArrayXXf& win,
356  Eigen::ArrayXXf& grad_x_win,
357  Eigen::ArrayXXf& grad_y_win,
358  Eigen::Array3f& covariance) const;
359  void
360  mismatchVector(const Eigen::ArrayXXf& prev,
361  const Eigen::ArrayXXf& prev_grad_x,
362  const Eigen::ArrayXXf& prev_grad_y,
363  const FloatImage& next,
364  const Eigen::Array2i& location,
365  const Eigen::Array4f& weights,
366  Eigen::Array2f& b) const;
367 
368  /** \brief Compute the pyramidal representation of an image.
369  * \param[in] input the input cloud
370  * \param[out] pyramid computed pyramid levels along with their respective
371  * gradients
372  * \param[in] border_type
373  */
374  virtual void
376  std::vector<FloatImageConstPtr>& pyramid,
377  pcl::InterpolationType border_type) const;
378 
379  virtual void
380  track(const PointCloudInConstPtr& previous_input,
381  const PointCloudInConstPtr& current_input,
382  const std::vector<FloatImageConstPtr>& previous_pyramid,
383  const std::vector<FloatImageConstPtr>& current_pyramid,
384  const pcl::PointCloud<pcl::PointUV>::ConstPtr& previous_keypoints,
385  pcl::PointCloud<pcl::PointUV>::Ptr& current_keypoints,
386  std::vector<int>& status,
387  Eigen::Affine3f& motion) const;
388 
389  void
390  computeTracking() override;
391 
392  /** \brief input pyranid at t-1 */
393  std::vector<FloatImageConstPtr> ref_pyramid_;
394  /** \brief point cloud at t-1 */
396  /** \brief number of pyramid levels */
398  /** \brief detected keypoints 2D coordinates */
400  /** \brief status of keypoints of t-1 at t */
402  /** \brief number of points to detect */
403  std::size_t keypoints_nbr_;
404  /** \brief tracking width */
406  /** \brief half of tracking window width */
408  /** \brief tracking height */
410  /** \brief half of tracking window height */
412  /** \brief maximum number of iterations */
413  unsigned int max_iterations_;
414  /** \brief accuracy criterion to stop iterating */
415  float accuracy_;
417  /** \brief epsilon for subpixel computation */
418  float epsilon_;
420  /** \brief number of hardware threads */
421  unsigned int threads_;
422  /** \brief intensity accessor */
423  IntensityT intensity_;
424  /** \brief is the tracker initialized ? */
426  /** \brief compute transformation from successfully tracked points */
428  /** \brief computed transformation between tracked points */
429  Eigen::Affine3f motion_;
430  /** \brief smoothing kernel */
431  Eigen::Array<float, 5, 1> kernel_;
432  /** \brief smoothing kernel half size */
434  /** \brief index of last element in kernel */
436 
437 public:
439 };
440 } // namespace tracking
441 } // namespace pcl
442 
443 #include <pcl/tracking/impl/pyramidal_klt.hpp>
int track_height_2_
half of tracking window height
typename PointCloudIn::ConstPtr PointCloudInConstPtr
Definition: pyramidal_klt.h:69
void setEpsilon(float epsilon)
Set epsilon.
typename PointCloudIn::Ptr PointCloudInPtr
Definition: pyramidal_klt.h:68
shared_ptr< const PyramidalKLTTracker< PointInT, IntensityT > > ConstPtr
Definition: pyramidal_klt.h:74
void setTrackingWindowHeight(int height)
Set tracking window height.
shared_ptr< PointCloud< float > > Ptr
Definition: point_cloud.h:429
void setNumberOfKeypoints(std::size_t number)
Set the maximum number of points to track after sorting detected keypoints according to their respons...
Defines functions, macros and traits for allocating and using memory.
void derivatives(const FloatImage &src, FloatImage &grad_x, FloatImage &grad_y) const
compute Scharr derivatives of a source cloud.
void setAccuracy(float accuracy)
Set accuracy.
pcl::PointCloud< pcl::PointUV >::ConstPtr getTrackedPoints() const
int track_width_2_
half of tracking window width
void convolve(const FloatImageConstPtr &input, FloatImage &output) const
Separately convolve image with decomposable convolution kernel.
int nb_levels_
number of pyramid levels
FloatImage::ConstPtr FloatImageConstPtr
Definition: pyramidal_klt.h:72
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...
IndicesPtr indices_
A pointer to the vector of point indices to use.
Definition: pcl_base.h:153
PyramidalKLTTracker(int nb_levels=5, int tracking_window_width=7, int tracking_window_height=7)
Constructor.
Definition: pyramidal_klt.h:81
pcl::PointCloud< PointInT > PointCloudIn
Definition: tracker.h:70
Eigen::Affine3f getResult() const override
Return the computed transformation from tracked points.
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
virtual void computePyramids(const PointCloudInConstPtr &input, std::vector< FloatImageConstPtr > &pyramid, pcl::InterpolationType border_type) const
Compute the pyramidal representation of an image.
std::vector< FloatImageConstPtr > ref_pyramid_
input pyranid at t-1
void convolveCols(const FloatImageConstPtr &input, FloatImage &output) const
Convolve image columns.
pcl::TransformationFromCorrespondences transformation_computer_
compute transformation from successfully tracked points
bool initCompute() override
This method should get called before starting the actual computation.
#define PCL_MAKE_ALIGNED_OPERATOR_NEW
Macro to signal a class requires a custom allocator.
Definition: memory.h:63
typename TrackerBase::PointCloudIn PointCloudIn
Definition: pyramidal_klt.h:67
bool initialized_
is the tracker initialized ?
pcl::PointCloud< float > FloatImage
Definition: pyramidal_klt.h:70
Pyramidal Kanade Lucas Tomasi tracker.
Definition: pyramidal_klt.h:64
pcl::PointIndicesConstPtr getPointsToTrackStatus() const
shared_ptr< PyramidalKLTTracker< PointInT, IntensityT > > Ptr
Definition: pyramidal_klt.h:73
void convolveRows(const FloatImageConstPtr &input, FloatImage &output) const
Convolve image rows.
int kernel_last_
index of last element in kernel
void setTrackingWindowSize(int width, int height)
set the tracking window size
void setPointsToTrack(const pcl::PointIndicesConstPtr &points)
Provide a pointer to points to track.
std::size_t keypoints_nbr_
number of points to detect
Eigen::Affine3f motion_
computed transformation between tracked points
void downsample(const FloatImageConstPtr &input, FloatImageConstPtr &output) const
downsample input
Defines all the PCL implemented PointT point type structures.
PointIndices::ConstPtr PointIndicesConstPtr
Definition: PointIndices.h:27
PointCloudInConstPtr ref_
point cloud at t-1
void setMaxIterationsNumber(unsigned int max)
Set the maximum number of iterations in the Lucas Kanade loop.
void setNumberOfThreads(unsigned int nr_threads=0)
Initialize the scheduler and set the number of threads to use.
Eigen::Array< float, 5, 1 > kernel_
smoothing kernel
float accuracy_
accuracy criterion to stop iterating
IntensityT intensity_
intensity accessor
InterpolationType
Definition: io.h:255
float epsilon_
epsilon for subpixel computation
pcl::PointCloud< pcl::PointUV >::ConstPtr keypoints_
detected keypoints 2D coordinates
void setTrackingWindowWidth(int width)
Set tracking window width.
Calculates a transformation based on corresponding 3D points.
void setNumberOfPyramidLevels(int levels)
Set the number of pyramid levels.
void computeTracking() override
Abstract tracking method.
shared_ptr< const PointCloud< float > > ConstPtr
Definition: point_cloud.h:430
Tracker represents the base tracker class.
Definition: tracker.h:55
PointCloudConstPtr input_
The input point cloud dataset.
Definition: pcl_base.h:150
PointCloudInConstPtr getReferenceCloud() const
Get a pointer of the cloud at t-1.
pcl::PointIndicesPtr keypoints_status_
status of keypoints of t-1 at t
unsigned int getMaxIterationsNumber() const
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
unsigned int threads_
number of hardware threads
Defines all the PCL and non-PCL macros used.
int kernel_size_2_
smoothing kernel half size
std::string tracker_name_
The tracker name.
Definition: tracker.h:90
unsigned int max_iterations_
maximum number of iterations
PointIndices::Ptr PointIndicesPtr
Definition: PointIndices.h:26