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