Point Cloud Library (PCL) 1.14.0
Loading...
Searching...
No Matches
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
47namespace pcl {
48namespace 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 */
62template <typename PointInT,
64class PyramidalKLTTracker : public Tracker<PointInT, Eigen::Affine3f> {
65public:
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
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
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
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. */
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")
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
299protected:
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 allocation 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 */
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
453public:
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
shared_ptr< const PointCloud< float > > ConstPtr
Calculates a transformation based on corresponding 3D points.
Pyramidal Kanade Lucas Tomasi tracker.
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
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
pcl::PointCloud< pcl::PointUV >::ConstPtr getTrackedPoints() const
~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.
void setEpsilon(float epsilon)
Set epsilon.
Eigen::Affine3f motion_
computed transformation between tracked points
typename PointCloudIn::ConstPtr PointCloudInConstPtr
PyramidalKLTTracker(int nb_levels=5, int tracking_window_width=7, int tracking_window_height=7)
Constructor.
typename PointCloudIn::Ptr PointCloudInPtr
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
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 allocation whil...
Eigen::Array< float, 5, 1 > kernel_
smoothing kernel
unsigned int max_iterations_
maximum number of iterations
typename TrackerBase::PointCloudIn PointCloudIn
float accuracy_
accuracy criterion to stop iterating
void setTrackingWindowWidth(int width)
Set tracking window width.
FloatImage::ConstPtr FloatImageConstPtr
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::shared_ptr< const std::vector< int > > getStatusOfPointsToTrack() const
pcl::PointCloud< float > FloatImage
void convolveCols(const FloatImageConstPtr &input, FloatImage &output) const
Convolve image columns.
shared_ptr< const PyramidalKLTTracker< PointInT, IntensityT > > ConstPtr
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
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:255
PointIndices::Ptr PointIndicesPtr
PointIndices::ConstPtr PointIndicesConstPtr
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