Point Cloud Library (PCL)  1.9.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/pcl_macros.h>
41 #include <pcl/point_types.h>
42 #include <pcl/tracking/tracker.h>
43 #include <pcl/common/intensity.h>
44 #include <pcl/common/transformation_from_correspondences.h>
45 
46 namespace pcl
47 {
48  namespace tracking
49  {
50  /** Pyramidal Kanade Lucas Tomasi tracker.
51  * This is an implementation of the Pyramidal Kanade Lucas Tomasi tracker that operates on
52  * organized 3D keypoints with color/intensity information (this is the default behaviour but you
53  * can alterate it by providing another operator as second template argument). It is an affine
54  * tracker that iteratively computes the optical flow to find the best guess for a point p at t
55  * given its location at t-1.
56  * User is advised to respect the Tomasi condition: the response computed is the maximum eigenvalue
57  * of the second moment matrix but no restrictin are applied to points to track so you can use a
58  * detector of your choice to indicate points to track.
59  *
60  * \author Nizar Sallem
61  */
62  template<typename PointInT, typename IntensityT = pcl::common::IntensityFieldAccessor<PointInT> >
63  class PyramidalKLTTracker : public Tracker<PointInT, Eigen::Affine3f>
64  {
65  public:
68  using PointCloudInPtr = typename PointCloudIn::Ptr;
69  using PointCloudInConstPtr = typename PointCloudIn::ConstPtr;
73  using Ptr = boost::shared_ptr<PyramidalKLTTracker<PointInT, IntensityT> >;
74  using ConstPtr = boost::shared_ptr<const PyramidalKLTTracker<PointInT, IntensityT> >;
75 
77  using TrackerBase::input_;
79 
80  /// Constructor
81  PyramidalKLTTracker (int nb_levels = 5, int tracking_window_width = 7, int tracking_window_height = 7)
82  : ref_ ()
83  , nb_levels_ (nb_levels)
84  , track_width_ (tracking_window_width)
85  , track_height_ (tracking_window_height)
86  , threads_ (0)
87  , initialized_ (false)
88  {
89  tracker_name_ = "PyramidalKLTTracker";
90  accuracy_ = 0.1;
91  epsilon_ = 1e-3;
92  max_iterations_ = 10;
93  keypoints_nbr_ = 100;
95  kernel_ << 1.f/16 ,1.f/4 ,3.f/8 ,1.f/4 ,1.f/16;
96  kernel_size_2_ = kernel_.size () / 2;
97  kernel_last_ = kernel_.size () -1;
98  }
99 
100  /// Destructor
102 
103  /** \brief Set the number of pyramid levels
104  * \param levels desired number of pyramid levels
105  */
106  inline void
107  setNumberOfPyramidLevels (int levels) { nb_levels_ = levels; }
108 
109  /// \brief \return the number of pyramid levels
110  inline int
111  getNumberOfPyramidLevels () const { return (nb_levels_); }
112 
113  /** Set accuracy
114  * \param[in] accuracy desired accuracy.
115  */
116  inline void
117  setAccuracy (float accuracy) { accuracy_ = accuracy; }
118 
119  /// \return the accuracy
120  inline float
121  getAccuracy () const { return (accuracy_); }
122 
123  /** Set epsilon
124  * \param[in] epsilon desired epsilon.
125  */
126  inline void
127  setEpsilon (float epsilon) { epsilon_ = epsilon; }
128 
129  /// \return the epsilon
130  inline float
131  getEpsilon () const { return (epsilon_); }
132 
133  /** \brief Set the maximum number of points to track. Only the first keypoints_nbr_
134  * are used as points to track after sorting detected keypoints according to their
135  * response measure.
136  * \param[in] number the desired number of points to detect.
137  */
138  inline void
139  setNumberOfKeypoints (std::size_t number) { keypoints_nbr_ = number; }
140 
141  /// \return the maximum number of keypoints to keep
142  inline std::size_t
144 
145  /** \brief set the tracking window size
146  * \param[in] width the tracking window width
147  * \param[in] height the tracking window height
148  */
149  inline void
150  setTrackingWindowSize (int width, int height);
151 
152  /// \brief Set tracking window width
153  inline void
154  setTrackingWindowWidth (int width) {track_width_ = width; };
155 
156  /// \brief \return the tracking window size
157  inline int
159 
160  /// \brief Set tracking window height
161  inline void
162  setTrackingWindowHeight (int height) {track_height_ = height; };
163 
164  /// \brief \return the tracking window size
165  inline int
167 
168  /** \brief Initialize the scheduler and set the number of threads to use.
169  * \param nr_threads the number of hardware threads to use (0 sets the value back to
170  * automatic).
171  */
172  inline void
173  setNumberOfThreads (unsigned int nr_threads = 0) { threads_ = nr_threads; }
174 
175  /** \brief Get a pointer of the cloud at t-1. */
176  inline PointCloudInConstPtr
177  getReferenceCloud () const { return (ref_); }
178 
179  /** \brief Set the maximum number of iterations in the Lucas Kanade loop.
180  * \param[in] max the desired maximum number of iterations
181  */
182  inline void
183  setMaxIterationsNumber (unsigned int max) { max_iterations_ = max; }
184 
185  /// \brief \return the maximum iterations number
186  inline unsigned int
188 
189  /** \brief Provide a pointer to points to track.
190  * \param points the const boost shared pointer to a PointIndices message
191  */
192  inline void
194 
195  /** \brief Provide a pointer to points to track.
196  * \param points the const boost shared pointer to a PointIndices message
197  */
198  inline void
200 
201  /// \brief \return a pointer to the points successfully tracked.
203  getTrackedPoints () const { return (keypoints_); };
204 
205  /** \brief \return the status of points to track.
206  * Status == 0 --> points successfully tracked;
207  * Status < 0 --> point is lost;
208  * Status == -1 --> point is out of bond;
209  * Status == -2 --> optical flow can not be computed for this point.
210  */
213 
214  /** \brief Return the computed transformation from tracked points. */
215  Eigen::Affine3f
216  getResult () const override { return (motion_); }
217 
218  /// \brief \return initialization state
219  bool
220  getInitialized () const { return (initialized_); }
221 
222  protected:
223  bool
224  initCompute () override;
225 
226  /** \brief compute Scharr derivatives of a source cloud.
227  * \param[in] src the image for which gradients are to be computed
228  * \param[out] grad_x image gradient along X direction
229  * \param[out] grad_y image gradient along Y direction
230  */
231  void
232  derivatives (const FloatImage& src, FloatImage& grad_x, FloatImage& grad_y) const;
233 
234  /** \brief downsample input
235  * \param[in] input the image to downsample
236  * \param[out] output the downsampled image
237  */
238  void
239  downsample (const FloatImageConstPtr& input, FloatImageConstPtr& output) const;
240 
241  /** \brief downsample input and compute output gradients.
242  * \param[in] input the image to downsample
243  * \param[out] output the downsampled image
244  * \param[out] output_grad_x downsampled image gradient along X direction
245  * \param[out] output_grad_y downsampled image gradient along Y direction
246  */
247  void
248  downsample (const FloatImageConstPtr& input, FloatImageConstPtr& output,
249  FloatImageConstPtr& output_grad_x, FloatImageConstPtr& output_grad_y) const;
250 
251  /** \brief Separately convolve image with decomposable convolution kernel.
252  * \param[in] input input the image to convolve
253  * \param[out] output output the convolved image
254  */
255  void
256  convolve (const FloatImageConstPtr& input, FloatImage& output) const;
257 
258  /** \brief Convolve image columns.
259  * \param[in] input input the image to convolve
260  * \param[out] output output the convolved image
261  */
262  void
263  convolveCols (const FloatImageConstPtr& input, FloatImage& output) const;
264 
265  /** \brief Convolve image rows.
266  * \param[in] input input the image to convolve
267  * \param[out] output output the convolved image
268  */
269  void
270  convolveRows (const FloatImageConstPtr& input, FloatImage& output) const;
271 
272  /** \brief extract the patch from the previous image, previous image gradients surrounding
273  * pixel alocation while interpolating image and gradients data and compute covariation
274  * matrix of derivatives.
275  * \param[in] img original image
276  * \param[in] grad_x original image gradient along X direction
277  * \param[in] grad_y original image gradient along Y direction
278  * \param[in] location pixel at the center of the patch
279  * \param[in] weights bilinear interpolation weights at this location computed from subpixel
280  * location
281  * \param[out] win patch with interpolated intensity values
282  * \param[out] grad_x_win patch with interpolated gradient along X values
283  * \param[out] grad_y_win patch with interpolated gradient along Y values
284  * \param[out] covariance covariance matrix coefficients
285  */
286  virtual void
287  spatialGradient (const FloatImage& img,
288  const FloatImage& grad_x,
289  const FloatImage& grad_y,
290  const Eigen::Array2i& location,
291  const Eigen::Array4f& weights,
292  Eigen::ArrayXXf& win,
293  Eigen::ArrayXXf& grad_x_win,
294  Eigen::ArrayXXf& grad_y_win,
295  Eigen::Array3f & covariance) const;
296  void
297  mismatchVector (const Eigen::ArrayXXf& prev,
298  const Eigen::ArrayXXf& prev_grad_x,
299  const Eigen::ArrayXXf& prev_grad_y,
300  const FloatImage& next,
301  const Eigen::Array2i& location,
302  const Eigen::Array4f& weights,
303  Eigen::Array2f &b) const;
304 
305  /** \brief Compute the pyramidal representation of an image.
306  * \param[in] input the input cloud
307  * \param[out] pyramid computed pyramid levels along with their respective gradients
308  * \param[in] border_type
309  */
310  virtual void
312  std::vector<FloatImageConstPtr>& pyramid,
313  pcl::InterpolationType border_type) const;
314 
315  virtual void
316  track (const PointCloudInConstPtr& previous_input,
317  const PointCloudInConstPtr& current_input,
318  const std::vector<FloatImageConstPtr>& previous_pyramid,
319  const std::vector<FloatImageConstPtr>& current_pyramid,
320  const pcl::PointCloud<pcl::PointUV>::ConstPtr& previous_keypoints,
321  pcl::PointCloud<pcl::PointUV>::Ptr& current_keypoints,
322  std::vector<int>& status,
323  Eigen::Affine3f& motion) const;
324 
325  void
326  computeTracking () override;
327 
328  /// \brief input pyranid at t-1
329  std::vector<FloatImageConstPtr> ref_pyramid_;
330  /// \brief point cloud at t-1
332  /// \brief number of pyramid levels
334  /// \brief detected keypoints 2D coordinates
336  /// \brief status of keypoints of t-1 at t
338  /// \brief number of points to detect
339  std::size_t keypoints_nbr_;
340  /// \brief tracking width
342  /// \brief half of tracking window width
344  /// \brief tracking height
346  /// \brief half of tracking window height
348  /// \brief maximum number of iterations
349  unsigned int max_iterations_;
350  /// \brief accuracy criterion to stop iterating
351  float accuracy_;
353  /// \brief epsilon for subpixel computation
354  float epsilon_;
356  /// \brief number of hardware threads
357  unsigned int threads_;
358  /// \brief intensity accessor
359  IntensityT intensity_;
360  /// \brief is the tracker initialized ?
362  /// \brief compute transformation from successfully tracked points
364  /// \brief computed transformation between tracked points
365  Eigen::Affine3f motion_;
366  /// \brief smoothing kernel
367  Eigen::Array<float, 5, 1> kernel_;
368  /// \brief smoothing kernel half size
370  /// \brief index of last element in kernel
372  public:
374  };
375  }
376 }
377 
378 #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
void setTrackingWindowHeight(int height)
Set tracking window height.
void setNumberOfKeypoints(std::size_t number)
Set the maximum number of points to track.
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
boost::shared_ptr< const PyramidalKLTTracker< PointInT, IntensityT > > ConstPtr
Definition: pyramidal_klt.h:74
FloatImage::ConstPtr FloatImageConstPtr
Definition: pyramidal_klt.h:72
This file defines compatibility wrappers for low level I/O functions.
Definition: convolution.h:45
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:154
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:72
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: pcl_macros.h:344
typename TrackerBase::PointCloudIn PointCloudIn
Definition: pyramidal_klt.h:67
bool initialized_
is the tracker initialized ?
boost::shared_ptr< ::pcl::PointIndices > PointIndicesPtr
Definition: PointIndices.h:26
Pyramidal Kanade Lucas Tomasi tracker.
Definition: pyramidal_klt.h:63
pcl::PointIndicesConstPtr getPointsToTrackStatus() const
boost::shared_ptr< const ::pcl::PointIndices > PointIndicesConstPtr
Definition: PointIndices.h:27
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.
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
boost::shared_ptr< PointCloud< float > > Ptr
Definition: point_cloud.h:441
float accuracy_
accuracy criterion to stop iterating
IntensityT intensity_
intensity accessor
InterpolationType
Definition: io.h:229
float epsilon_
epsilon for subpixel computation
pcl::PointCloud< pcl::PointUV >::ConstPtr keypoints_
detected keypoints 2D coordinates
void setTrackingWindowWidth(int width)
Set tracking window width.
boost::shared_ptr< PyramidalKLTTracker< PointInT, IntensityT > > Ptr
Definition: pyramidal_klt.h:73
Calculates a transformation based on corresponding 3D points.
void setNumberOfPyramidLevels(int levels)
Set the number of pyramid levels.
void computeTracking() override
Abstract tracking method.
boost::shared_ptr< const PointCloud< float > > ConstPtr
Definition: point_cloud.h:442
Tracker represents the base tracker class.
Definition: tracker.h:56
PointCloudConstPtr input_
The input point cloud dataset.
Definition: pcl_base.h:151
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:92
unsigned int max_iterations_
maximum number of iterations