Point Cloud Library (PCL)  1.10.1-dev
feature.h
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Point Cloud Library (PCL) - www.pointclouds.org
5  * Copyright (c) 2010-2011, Willow Garage, Inc.
6  * Copyright (c) 2012-, Open Perception, Inc.
7  *
8  * All rights reserved.
9  *
10  * Redistribution and use in source and binary forms, with or without
11  * modification, are permitted provided that the following conditions
12  * are met:
13  *
14  * * Redistributions of source code must retain the above copyright
15  * notice, this list of conditions and the following disclaimer.
16  * * Redistributions in binary form must reproduce the above
17  * copyright notice, this list of conditions and the following
18  * disclaimer in the documentation and/or other materials provided
19  * with the distribution.
20  * * Neither the name of the copyright holder(s) nor the names of its
21  * contributors may be used to endorse or promote products derived
22  * from this software without specific prior written permission.
23  *
24  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
25  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
26  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
27  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
28  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
29  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
30  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
31  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
32  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
33  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
34  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
35  * POSSIBILITY OF SUCH DAMAGE.
36  *
37  * $Id$
38  *
39  */
40 
41 #pragma once
42 
43 #if defined __GNUC__
44 # pragma GCC system_header
45 #endif
46 
47 // PCL includes
48 #include <pcl/memory.h>
49 #include <pcl/pcl_base.h>
50 #include <pcl/pcl_macros.h>
51 #include <pcl/search/search.h>
52 
53 #include <functional>
54 
55 namespace pcl
56 {
57  /** \brief Solve the eigenvalues and eigenvectors of a given 3x3 covariance matrix, and estimate the least-squares
58  * plane normal and surface curvature.
59  * \param covariance_matrix the 3x3 covariance matrix
60  * \param point a point lying on the least-squares plane (SSE aligned)
61  * \param plane_parameters the resultant plane parameters as: a, b, c, d (ax + by + cz + d = 0)
62  * \param curvature the estimated surface curvature as a measure of
63  * \f[
64  * \lambda_0 / (\lambda_0 + \lambda_1 + \lambda_2)
65  * \f]
66  * \ingroup features
67  */
68  inline void
69  solvePlaneParameters (const Eigen::Matrix3f &covariance_matrix,
70  const Eigen::Vector4f &point,
71  Eigen::Vector4f &plane_parameters, float &curvature);
72 
73  /** \brief Solve the eigenvalues and eigenvectors of a given 3x3 covariance matrix, and estimate the least-squares
74  * plane normal and surface curvature.
75  * \param covariance_matrix the 3x3 covariance matrix
76  * \param nx the resultant X component of the plane normal
77  * \param ny the resultant Y component of the plane normal
78  * \param nz the resultant Z component of the plane normal
79  * \param curvature the estimated surface curvature as a measure of
80  * \f[
81  * \lambda_0 / (\lambda_0 + \lambda_1 + \lambda_2)
82  * \f]
83  * \ingroup features
84  */
85  inline void
86  solvePlaneParameters (const Eigen::Matrix3f &covariance_matrix,
87  float &nx, float &ny, float &nz, float &curvature);
88 
89  ////////////////////////////////////////////////////////////////////////////////////////////
90  ////////////////////////////////////////////////////////////////////////////////////////////
91  ////////////////////////////////////////////////////////////////////////////////////////////
92  /** \brief Feature represents the base feature class. Some generic 3D operations that
93  * are applicable to all features are defined here as static methods.
94  *
95  * \attention
96  * The convention for a feature descriptor is:
97  * - if the nearest neighbors for the query point at which the descriptor is to be computed cannot be
98  * determined, the descriptor values will be set to NaN (not a number)
99  * - it is impossible to estimate a feature descriptor for a point that doesn't have finite 3D coordinates.
100  * Therefore, any point that has NaN data on x, y, or z, will most likely have its descriptor set to NaN.
101  *
102  * \author Radu B. Rusu
103  * \ingroup features
104  */
105  template <typename PointInT, typename PointOutT>
106  class Feature : public PCLBase<PointInT>
107  {
108  public:
111 
113 
116 
118  using KdTreePtr = typename KdTree::Ptr;
119 
123 
125 
126  using SearchMethod = std::function<int (std::size_t, double, std::vector<int> &, std::vector<float> &)>;
127  using SearchMethodSurface = std::function<int (const PointCloudIn &cloud, std::size_t index, double, std::vector<int> &, std::vector<float> &)>;
128 
129  public:
130  /** \brief Empty constructor. */
131  Feature () :
133  surface_(), tree_(),
135  fake_surface_(false)
136  {}
137 
138  /** \brief Empty destructor */
139  virtual ~Feature () {}
140 
141  /** \brief Provide a pointer to a dataset to add additional information
142  * to estimate the features for every point in the input dataset. This
143  * is optional, if this is not set, it will only use the data in the
144  * input cloud to estimate the features. This is useful when you only
145  * need to compute the features for a downsampled cloud.
146  * \param[in] cloud a pointer to a PointCloud message
147  */
148  inline void
150  {
151  surface_ = cloud;
152  fake_surface_ = false;
153  //use_surface_ = true;
154  }
155 
156  /** \brief Get a pointer to the surface point cloud dataset. */
157  inline PointCloudInConstPtr
159  {
160  return (surface_);
161  }
162 
163  /** \brief Provide a pointer to the search object.
164  * \param[in] tree a pointer to the spatial search object.
165  */
166  inline void
167  setSearchMethod (const KdTreePtr &tree) { tree_ = tree; }
168 
169  /** \brief Get a pointer to the search method used. */
170  inline KdTreePtr
172  {
173  return (tree_);
174  }
175 
176  /** \brief Get the internal search parameter. */
177  inline double
179  {
180  return (search_parameter_);
181  }
182 
183  /** \brief Set the number of k nearest neighbors to use for the feature estimation.
184  * \param[in] k the number of k-nearest neighbors
185  */
186  inline void
187  setKSearch (int k) { k_ = k; }
188 
189  /** \brief get the number of k nearest neighbors used for the feature estimation. */
190  inline int
191  getKSearch () const
192  {
193  return (k_);
194  }
195 
196  /** \brief Set the sphere radius that is to be used for determining the nearest neighbors used for the feature
197  * estimation.
198  * \param[in] radius the sphere radius used as the maximum distance to consider a point a neighbor
199  */
200  inline void
201  setRadiusSearch (double radius)
202  {
203  search_radius_ = radius;
204  }
205 
206  /** \brief Get the sphere radius used for determining the neighbors. */
207  inline double
209  {
210  return (search_radius_);
211  }
212 
213  /** \brief Base method for feature estimation for all points given in
214  * <setInputCloud (), setIndices ()> using the surface in setSearchSurface ()
215  * and the spatial locator in setSearchMethod ()
216  * \param[out] output the resultant point cloud model dataset containing the estimated features
217  */
218  void
219  compute (PointCloudOut &output);
220 
221  protected:
222  /** \brief The feature name. */
223  std::string feature_name_;
224 
225  /** \brief The search method template for points. */
227 
228  /** \brief An input point cloud describing the surface that is to be used
229  * for nearest neighbors estimation.
230  */
232 
233  /** \brief A pointer to the spatial search object. */
235 
236  /** \brief The actual search parameter (from either \a search_radius_ or \a k_). */
238 
239  /** \brief The nearest neighbors search radius for each point. */
241 
242  /** \brief The number of K nearest neighbors to use for each point. */
243  int k_;
244 
245  /** \brief Get a string representation of the name of this class. */
246  inline const std::string&
247  getClassName () const { return (feature_name_); }
248 
249  /** \brief This method should get called before starting the actual computation. */
250  virtual bool
251  initCompute ();
252 
253  /** \brief This method should get called after ending the actual computation. */
254  virtual bool
255  deinitCompute ();
256 
257  /** \brief If no surface is given, we use the input PointCloud as the surface. */
259 
260  /** \brief Search for k-nearest neighbors using the spatial locator from
261  * \a setSearchmethod, and the given surface from \a setSearchSurface.
262  * \param[in] index the index of the query point
263  * \param[in] parameter the search parameter (either k or radius)
264  * \param[out] indices the resultant vector of indices representing the k-nearest neighbors
265  * \param[out] distances the resultant vector of distances representing the distances from the query point to the
266  * k-nearest neighbors
267  *
268  * \return the number of neighbors found. If no neighbors are found or an error occurred, return 0.
269  */
270  inline int
271  searchForNeighbors (std::size_t index, double parameter,
272  std::vector<int> &indices, std::vector<float> &distances) const
273  {
274  return (search_method_surface_ (*input_, index, parameter, indices, distances));
275  }
276 
277  /** \brief Search for k-nearest neighbors using the spatial locator from
278  * \a setSearchmethod, and the given surface from \a setSearchSurface.
279  * \param[in] cloud the query point cloud
280  * \param[in] index the index of the query point in \a cloud
281  * \param[in] parameter the search parameter (either k or radius)
282  * \param[out] indices the resultant vector of indices representing the k-nearest neighbors
283  * \param[out] distances the resultant vector of distances representing the distances from the query point to the
284  * k-nearest neighbors
285  *
286  * \return the number of neighbors found. If no neighbors are found or an error occurred, return 0.
287  */
288  inline int
289  searchForNeighbors (const PointCloudIn &cloud, std::size_t index, double parameter,
290  std::vector<int> &indices, std::vector<float> &distances) const
291  {
292  return (search_method_surface_ (cloud, index, parameter, indices, distances));
293  }
294 
295  private:
296  /** \brief Abstract feature estimation method.
297  * \param[out] output the resultant features
298  */
299  virtual void
300  computeFeature (PointCloudOut &output) = 0;
301 
302  public:
304  };
305 
306 
307  ////////////////////////////////////////////////////////////////////////////////////////////
308  ////////////////////////////////////////////////////////////////////////////////////////////
309  ////////////////////////////////////////////////////////////////////////////////////////////
310  template <typename PointInT, typename PointNT, typename PointOutT>
311  class FeatureFromNormals : public Feature<PointInT, PointOutT>
312  {
314  using PointCloudInPtr = typename PointCloudIn::Ptr;
317 
318  public:
321  using PointCloudNConstPtr = typename PointCloudN::ConstPtr;
322 
325 
326  // Members derived from the base class
330 
331  /** \brief Empty constructor. */
332  FeatureFromNormals () : normals_ () {}
333 
334  /** \brief Empty destructor */
335  virtual ~FeatureFromNormals () {}
336 
337  /** \brief Provide a pointer to the input dataset that contains the point normals of
338  * the XYZ dataset.
339  * In case of search surface is set to be different from the input cloud,
340  * normals should correspond to the search surface, not the input cloud!
341  * \param[in] normals the const boost shared pointer to a PointCloud of normals.
342  * By convention, L2 norm of each normal should be 1.
343  */
344  inline void
345  setInputNormals (const PointCloudNConstPtr &normals) { normals_ = normals; }
346 
347  /** \brief Get a pointer to the normals of the input XYZ point cloud dataset. */
348  inline PointCloudNConstPtr
349  getInputNormals () const { return (normals_); }
350 
351  protected:
352  /** \brief A pointer to the input dataset that contains the point normals of the XYZ
353  * dataset.
354  */
356 
357  /** \brief This method should get called before starting the actual computation. */
358  virtual bool
359  initCompute ();
360 
361  public:
363  };
364 
365  ////////////////////////////////////////////////////////////////////////////////////////////
366  ////////////////////////////////////////////////////////////////////////////////////////////
367  ////////////////////////////////////////////////////////////////////////////////////////////
368  template <typename PointInT, typename PointLT, typename PointOutT>
369  class FeatureFromLabels : public Feature<PointInT, PointOutT>
370  {
372  using PointCloudInPtr = typename PointCloudIn::Ptr;
374 
376  using PointCloudNPtr = typename PointCloudL::Ptr;
377  using PointCloudLConstPtr = typename PointCloudL::ConstPtr;
378 
380 
381  public:
384 
385  // Members derived from the base class
390 
391  /** \brief Empty constructor. */
392  FeatureFromLabels () : labels_ ()
393  {
394  k_ = 1; // Search tree is not always used here.
395  }
396 
397  /** \brief Empty destructor */
398  virtual ~FeatureFromLabels () {}
399 
400  /** \brief Provide a pointer to the input dataset that contains the point labels of
401  * the XYZ dataset.
402  * In case of search surface is set to be different from the input cloud,
403  * labels should correspond to the search surface, not the input cloud!
404  * \param[in] labels the const boost shared pointer to a PointCloud of labels.
405  */
406  inline void
407  setInputLabels (const PointCloudLConstPtr &labels)
408  {
409  labels_ = labels;
410  }
411 
412  /** \brief Get a pointer to the labels of the input XYZ point cloud dataset. */
413  inline PointCloudLConstPtr
414  getInputLabels () const
415  {
416  return (labels_);
417  }
418 
419  protected:
420  /** \brief A pointer to the input dataset that contains the point labels of the XYZ
421  * dataset.
422  */
423  PointCloudLConstPtr labels_;
424 
425  /** \brief This method should get called before starting the actual computation. */
426  virtual bool
427  initCompute ();
428 
429  public:
431  };
432 
433  ////////////////////////////////////////////////////////////////////////////////////////////
434  ////////////////////////////////////////////////////////////////////////////////////////////
435  ////////////////////////////////////////////////////////////////////////////////////////////
436  /** \brief FeatureWithLocalReferenceFrames provides a public interface for descriptor
437  * extractor classes which need a local reference frame at each input keypoint.
438  *
439  * \attention
440  * This interface is for backward compatibility with existing code and in the future it could be
441  * merged with pcl::Feature. Subclasses should call the protected method initLocalReferenceFrames ()
442  * to correctly initialize the frames_ member.
443  *
444  * \author Nicola Fioraio
445  * \ingroup features
446  */
447  template <typename PointInT, typename PointRFT>
449  {
450  public:
454 
455  /** \brief Empty constructor. */
456  FeatureWithLocalReferenceFrames () : frames_ (), frames_never_defined_ (true) {}
457 
458  /** \brief Empty destructor. */
460 
461  /** \brief Provide a pointer to the input dataset that contains the local
462  * reference frames of the XYZ dataset.
463  * In case of search surface is set to be different from the input cloud,
464  * local reference frames should correspond to the input cloud, not the search surface!
465  * \param[in] frames the const boost shared pointer to a PointCloud of reference frames.
466  */
467  inline void
469  {
470  frames_ = frames;
471  frames_never_defined_ = false;
472  }
473 
474  /** \brief Get a pointer to the local reference frames. */
475  inline PointCloudLRFConstPtr
477  {
478  return (frames_);
479  }
480 
481  protected:
482  /** \brief A boost shared pointer to the local reference frames. */
484  /** \brief The user has never set the frames. */
486 
487  /** \brief Check if frames_ has been correctly initialized and compute it if needed.
488  * \param input the subclass' input cloud dataset.
489  * \param lrf_estimation a pointer to a local reference frame estimation class to be used as default.
490  * \return true if frames_ has been correctly initialized.
491  */
493  virtual bool
494  initLocalReferenceFrames (const std::size_t& indices_size,
495  const LRFEstimationPtr& lrf_estimation = LRFEstimationPtr());
496  };
497 }
498 
499 #include <pcl/features/impl/feature.hpp>
void setSearchSurface(const PointCloudInConstPtr &cloud)
Provide a pointer to a dataset to add additional information to estimate the features for every point...
Definition: feature.h:149
typename Feature< PointInT, PointRFT >::Ptr LRFEstimationPtr
Check if frames_ has been correctly initialized and compute it if needed.
Definition: feature.h:492
shared_ptr< PointCloud< PointInT > > Ptr
Definition: point_cloud.h:414
Defines functions, macros and traits for allocating and using memory.
PointCloudLRFConstPtr frames_
A boost shared pointer to the local reference frames.
Definition: feature.h:483
typename PointCloudLRF::ConstPtr PointCloudLRFConstPtr
Definition: feature.h:453
FeatureFromNormals()
Empty constructor.
Definition: feature.h:332
virtual ~FeatureFromLabels()
Empty destructor.
Definition: feature.h:398
typename KdTree::Ptr KdTreePtr
Definition: feature.h:118
std::string feature_name_
The feature name.
Definition: feature.h:223
int k_
The number of K nearest neighbors to use for each point.
Definition: feature.h:243
double getRadiusSearch() const
Get the sphere radius used for determining the neighbors.
Definition: feature.h:208
int searchForNeighbors(std::size_t index, double parameter, std::vector< int > &indices, std::vector< float > &distances) const
Search for k-nearest neighbors using the spatial locator from setSearchmethod, and the given surface ...
Definition: feature.h:271
FeatureWithLocalReferenceFrames()
Empty constructor.
Definition: feature.h:456
void solvePlaneParameters(const Eigen::Matrix3f &covariance_matrix, const Eigen::Vector4f &point, Eigen::Vector4f &plane_parameters, float &curvature)
Solve the eigenvalues and eigenvectors of a given 3x3 covariance matrix, and estimate the least-squar...
Definition: feature.hpp:48
virtual bool deinitCompute()
This method should get called after ending the actual computation.
Definition: feature.hpp:179
typename PointCloudIn::ConstPtr PointCloudInConstPtr
Definition: feature.h:122
void setRadiusSearch(double radius)
Set the sphere radius that is to be used for determining the nearest neighbors used for the feature e...
Definition: feature.h:201
#define PCL_MAKE_ALIGNED_OPERATOR_NEW
Macro to signal a class requires a custom allocator.
Definition: memory.h:65
std::function< int(std::size_t, double, std::vector< int > &, std::vector< float > &)> SearchMethod
Definition: feature.h:126
KdTreePtr tree_
A pointer to the spatial search object.
Definition: feature.h:234
shared_ptr< const Feature< PointWithRange, Narf36 > > ConstPtr
Definition: feature.h:115
const std::string & getClassName() const
Get a string representation of the name of this class.
Definition: feature.h:247
double getSearchParameter() const
Get the internal search parameter.
Definition: feature.h:178
PointCloudInConstPtr surface_
An input point cloud describing the surface that is to be used for nearest neighbors estimation...
Definition: feature.h:231
KdTreePtr getSearchMethod() const
Get a pointer to the search method used.
Definition: feature.h:171
PCL base class.
Definition: pcl_base.h:70
typename PointCloudLRF::Ptr PointCloudLRFPtr
Definition: feature.h:452
bool fake_surface_
If no surface is given, we use the input PointCloud as the surface.
Definition: feature.h:258
void setInputReferenceFrames(const PointCloudLRFConstPtr &frames)
Provide a pointer to the input dataset that contains the local reference frames of the XYZ dataset...
Definition: feature.h:468
int getKSearch() const
get the number of k nearest neighbors used for the feature estimation.
Definition: feature.h:191
Feature()
Empty constructor.
Definition: feature.h:131
PointCloudInConstPtr getSearchSurface() const
Get a pointer to the surface point cloud dataset.
Definition: feature.h:158
bool frames_never_defined_
The user has never set the frames.
Definition: feature.h:485
virtual ~FeatureFromNormals()
Empty destructor.
Definition: feature.h:335
virtual bool initCompute()
This method should get called before starting the actual computation.
Definition: feature.hpp:93
void setSearchMethod(const KdTreePtr &tree)
Provide a pointer to the search object.
Definition: feature.h:167
virtual ~Feature()
Empty destructor.
Definition: feature.h:139
PointCloudNConstPtr normals_
A pointer to the input dataset that contains the point normals of the XYZ dataset.
Definition: feature.h:355
PointCloudLConstPtr labels_
A pointer to the input dataset that contains the point labels of the XYZ dataset. ...
Definition: feature.h:423
PointCloudLConstPtr getInputLabels() const
Get a pointer to the labels of the input XYZ point cloud dataset.
Definition: feature.h:414
std::function< int(const PointCloudIn &cloud, std::size_t index, double, std::vector< int > &, std::vector< float > &)> SearchMethodSurface
Definition: feature.h:127
void setInputLabels(const PointCloudLConstPtr &labels)
Provide a pointer to the input dataset that contains the point labels of the XYZ dataset.
Definition: feature.h:407
double search_parameter_
The actual search parameter (from either search_radius_ or k_).
Definition: feature.h:237
shared_ptr< const PointCloud< PointInT > > ConstPtr
Definition: point_cloud.h:415
PointCloudNConstPtr getInputNormals() const
Get a pointer to the normals of the input XYZ point cloud dataset.
Definition: feature.h:349
int searchForNeighbors(const PointCloudIn &cloud, std::size_t index, double parameter, std::vector< int > &indices, std::vector< float > &distances) const
Search for k-nearest neighbors using the spatial locator from setSearchmethod, and the given surface ...
Definition: feature.h:289
PointCloudConstPtr input_
The input point cloud dataset.
Definition: pcl_base.h:152
shared_ptr< pcl::search::Search< PointInT > > Ptr
Definition: search.h:80
Feature represents the base feature class.
Definition: feature.h:106
FeatureWithLocalReferenceFrames provides a public interface for descriptor extractor classes which ne...
Definition: feature.h:448
void compute(PointCloudOut &output)
Base method for feature estimation for all points given in <setInputCloud (), setIndices ()> using th...
Definition: feature.hpp:192
typename PointCloudN::ConstPtr PointCloudNConstPtr
Definition: feature.h:321
void setKSearch(int k)
Set the number of k nearest neighbors to use for the feature estimation.
Definition: feature.h:187
void setInputNormals(const PointCloudNConstPtr &normals)
Provide a pointer to the input dataset that contains the point normals of the XYZ dataset...
Definition: feature.h:345
virtual ~FeatureWithLocalReferenceFrames()
Empty destructor.
Definition: feature.h:459
boost::shared_ptr< T > shared_ptr
Alias for boost::shared_ptr.
Definition: memory.h:81
PointCloudLRFConstPtr getInputReferenceFrames() const
Get a pointer to the local reference frames.
Definition: feature.h:476
double search_radius_
The nearest neighbors search radius for each point.
Definition: feature.h:240
SearchMethodSurface search_method_surface_
The search method template for points.
Definition: feature.h:226
Defines all the PCL and non-PCL macros used.
shared_ptr< Feature< PointWithRange, Narf36 > > Ptr
Definition: feature.h:114
typename PointCloudIn::Ptr PointCloudInPtr
Definition: feature.h:121
FeatureFromLabels()
Empty constructor.
Definition: feature.h:392
Generic search class.
Definition: search.h:73