Point Cloud Library (PCL)  1.12.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 
114  using Ptr = shared_ptr< Feature<PointInT, PointOutT> >;
115  using ConstPtr = shared_ptr< const Feature<PointInT, PointOutT> >;
116 
118  using KdTreePtr = typename KdTree::Ptr;
119 
123 
125 
126  using SearchMethod = std::function<int (std::size_t, double, pcl::Indices &, std::vector<float> &)>;
127  using SearchMethodSurface = std::function<int (const PointCloudIn &cloud, std::size_t index, double, pcl::Indices &, std::vector<float> &)>;
128 
129  public:
130  /** \brief Empty constructor. */
131  Feature () :
133  surface_(), tree_(),
135  fake_surface_(false)
136  {}
137 
138  /** \brief Provide a pointer to a dataset to add additional information
139  * to estimate the features for every point in the input dataset. This
140  * is optional, if this is not set, it will only use the data in the
141  * input cloud to estimate the features. This is useful when you only
142  * need to compute the features for a downsampled cloud.
143  * \param[in] cloud a pointer to a PointCloud message
144  */
145  inline void
147  {
148  surface_ = cloud;
149  fake_surface_ = false;
150  //use_surface_ = true;
151  }
152 
153  /** \brief Get a pointer to the surface point cloud dataset. */
154  inline PointCloudInConstPtr
156  {
157  return (surface_);
158  }
159 
160  /** \brief Provide a pointer to the search object.
161  * \param[in] tree a pointer to the spatial search object.
162  */
163  inline void
164  setSearchMethod (const KdTreePtr &tree) { tree_ = tree; }
165 
166  /** \brief Get a pointer to the search method used. */
167  inline KdTreePtr
169  {
170  return (tree_);
171  }
172 
173  /** \brief Get the internal search parameter. */
174  inline double
176  {
177  return (search_parameter_);
178  }
179 
180  /** \brief Set the number of k nearest neighbors to use for the feature estimation.
181  * \param[in] k the number of k-nearest neighbors
182  */
183  inline void
184  setKSearch (int k) { k_ = k; }
185 
186  /** \brief get the number of k nearest neighbors used for the feature estimation. */
187  inline int
188  getKSearch () const
189  {
190  return (k_);
191  }
192 
193  /** \brief Set the sphere radius that is to be used for determining the nearest neighbors used for the feature
194  * estimation.
195  * \param[in] radius the sphere radius used as the maximum distance to consider a point a neighbor
196  */
197  inline void
198  setRadiusSearch (double radius)
199  {
200  search_radius_ = radius;
201  }
202 
203  /** \brief Get the sphere radius used for determining the neighbors. */
204  inline double
206  {
207  return (search_radius_);
208  }
209 
210  /** \brief Base method for feature estimation for all points given in
211  * <setInputCloud (), setIndices ()> using the surface in setSearchSurface ()
212  * and the spatial locator in setSearchMethod ()
213  * \param[out] output the resultant point cloud model dataset containing the estimated features
214  */
215  void
216  compute (PointCloudOut &output);
217 
218  protected:
219  /** \brief The feature name. */
220  std::string feature_name_;
221 
222  /** \brief The search method template for points. */
224 
225  /** \brief An input point cloud describing the surface that is to be used
226  * for nearest neighbors estimation.
227  */
229 
230  /** \brief A pointer to the spatial search object. */
232 
233  /** \brief The actual search parameter (from either \a search_radius_ or \a k_). */
235 
236  /** \brief The nearest neighbors search radius for each point. */
238 
239  /** \brief The number of K nearest neighbors to use for each point. */
240  int k_;
241 
242  /** \brief Get a string representation of the name of this class. */
243  inline const std::string&
244  getClassName () const { return (feature_name_); }
245 
246  /** \brief This method should get called before starting the actual computation. */
247  virtual bool
248  initCompute ();
249 
250  /** \brief This method should get called after ending the actual computation. */
251  virtual bool
252  deinitCompute ();
253 
254  /** \brief If no surface is given, we use the input PointCloud as the surface. */
256 
257  /** \brief Search for k-nearest neighbors using the spatial locator from
258  * \a setSearchmethod, and the given surface from \a setSearchSurface.
259  * \param[in] index the index of the query point
260  * \param[in] parameter the search parameter (either k or radius)
261  * \param[out] indices the resultant vector of indices representing the k-nearest neighbors
262  * \param[out] distances the resultant vector of distances representing the distances from the query point to the
263  * k-nearest neighbors
264  *
265  * \return the number of neighbors found. If no neighbors are found or an error occurred, return 0.
266  */
267  inline int
268  searchForNeighbors (std::size_t index, double parameter,
269  pcl::Indices &indices, std::vector<float> &distances) const
270  {
271  return (search_method_surface_ (*input_, index, parameter, indices, distances));
272  }
273 
274  /** \brief Search for k-nearest neighbors using the spatial locator from
275  * \a setSearchmethod, and the given surface from \a setSearchSurface.
276  * \param[in] cloud the query point cloud
277  * \param[in] index the index of the query point in \a cloud
278  * \param[in] parameter the search parameter (either k or radius)
279  * \param[out] indices the resultant vector of indices representing the k-nearest neighbors
280  * \param[out] distances the resultant vector of distances representing the distances from the query point to the
281  * k-nearest neighbors
282  *
283  * \return the number of neighbors found. If no neighbors are found or an error occurred, return 0.
284  */
285  inline int
286  searchForNeighbors (const PointCloudIn &cloud, std::size_t index, double parameter,
287  pcl::Indices &indices, std::vector<float> &distances) const
288  {
289  return (search_method_surface_ (cloud, index, parameter, indices, distances));
290  }
291 
292  private:
293  /** \brief Abstract feature estimation method.
294  * \param[out] output the resultant features
295  */
296  virtual void
297  computeFeature (PointCloudOut &output) = 0;
298 
299  public:
301  };
302 
303 
304  ////////////////////////////////////////////////////////////////////////////////////////////
305  ////////////////////////////////////////////////////////////////////////////////////////////
306  ////////////////////////////////////////////////////////////////////////////////////////////
307  template <typename PointInT, typename PointNT, typename PointOutT>
308  class FeatureFromNormals : public Feature<PointInT, PointOutT>
309  {
311  using PointCloudInPtr = typename PointCloudIn::Ptr;
314 
315  public:
319 
320  using Ptr = shared_ptr< FeatureFromNormals<PointInT, PointNT, PointOutT> >;
321  using ConstPtr = shared_ptr< const FeatureFromNormals<PointInT, PointNT, PointOutT> >;
322 
323  // Members derived from the base class
327 
328  /** \brief Empty constructor. */
330 
331  /** \brief Provide a pointer to the input dataset that contains the point normals of
332  * the XYZ dataset.
333  * In case of search surface is set to be different from the input cloud,
334  * normals should correspond to the search surface, not the input cloud!
335  * \param[in] normals the const boost shared pointer to a PointCloud of normals.
336  * By convention, L2 norm of each normal should be 1.
337  */
338  inline void
339  setInputNormals (const PointCloudNConstPtr &normals) { normals_ = normals; }
340 
341  /** \brief Get a pointer to the normals of the input XYZ point cloud dataset. */
342  inline PointCloudNConstPtr
343  getInputNormals () const { return (normals_); }
344 
345  protected:
346  /** \brief A pointer to the input dataset that contains the point normals of the XYZ
347  * dataset.
348  */
350 
351  /** \brief This method should get called before starting the actual computation. */
352  virtual bool
353  initCompute ();
354 
355  public:
357  };
358 
359  ////////////////////////////////////////////////////////////////////////////////////////////
360  ////////////////////////////////////////////////////////////////////////////////////////////
361  ////////////////////////////////////////////////////////////////////////////////////////////
362  template <typename PointInT, typename PointLT, typename PointOutT>
363  class FeatureFromLabels : public Feature<PointInT, PointOutT>
364  {
366  using PointCloudInPtr = typename PointCloudIn::Ptr;
368 
370  using PointCloudNPtr = typename PointCloudL::Ptr;
371  using PointCloudLConstPtr = typename PointCloudL::ConstPtr;
372 
374 
375  public:
376  using Ptr = shared_ptr< FeatureFromLabels<PointInT, PointLT, PointOutT> >;
377  using ConstPtr = shared_ptr< const FeatureFromLabels<PointInT, PointLT, PointOutT> >;
378 
379  // Members derived from the base class
384 
385  /** \brief Empty constructor. */
387  {
388  k_ = 1; // Search tree is not always used here.
389  }
390 
391  /** \brief Provide a pointer to the input dataset that contains the point labels of
392  * the XYZ dataset.
393  * In case of search surface is set to be different from the input cloud,
394  * labels should correspond to the search surface, not the input cloud!
395  * \param[in] labels the const boost shared pointer to a PointCloud of labels.
396  */
397  inline void
398  setInputLabels (const PointCloudLConstPtr &labels)
399  {
400  labels_ = labels;
401  }
402 
403  /** \brief Get a pointer to the labels of the input XYZ point cloud dataset. */
404  inline PointCloudLConstPtr
405  getInputLabels () const
406  {
407  return (labels_);
408  }
409 
410  protected:
411  /** \brief A pointer to the input dataset that contains the point labels of the XYZ
412  * dataset.
413  */
414  PointCloudLConstPtr labels_;
415 
416  /** \brief This method should get called before starting the actual computation. */
417  virtual bool
418  initCompute ();
419 
420  public:
422  };
423 
424  ////////////////////////////////////////////////////////////////////////////////////////////
425  ////////////////////////////////////////////////////////////////////////////////////////////
426  ////////////////////////////////////////////////////////////////////////////////////////////
427  /** \brief FeatureWithLocalReferenceFrames provides a public interface for descriptor
428  * extractor classes which need a local reference frame at each input keypoint.
429  *
430  * \attention
431  * This interface is for backward compatibility with existing code and in the future it could be
432  * merged with pcl::Feature. Subclasses should call the protected method initLocalReferenceFrames ()
433  * to correctly initialize the frames_ member.
434  *
435  * \author Nicola Fioraio
436  * \ingroup features
437  */
438  template <typename PointInT, typename PointRFT>
440  {
441  public:
445 
446  /** \brief Empty constructor. */
448 
449  /** \brief Default virtual destructor. */
450  virtual
452 
453  /** \brief Provide a pointer to the input dataset that contains the local
454  * reference frames of the XYZ dataset.
455  * In case of search surface is set to be different from the input cloud,
456  * local reference frames should correspond to the input cloud, not the search surface!
457  * \param[in] frames the const boost shared pointer to a PointCloud of reference frames.
458  */
459  inline void
461  {
462  frames_ = frames;
463  frames_never_defined_ = false;
464  }
465 
466  /** \brief Get a pointer to the local reference frames. */
467  inline PointCloudLRFConstPtr
469  {
470  return (frames_);
471  }
472 
473  protected:
474  /** \brief A boost shared pointer to the local reference frames. */
476  /** \brief The user has never set the frames. */
478 
479  /** \brief Check if frames_ has been correctly initialized and compute it if needed.
480  * \param input the subclass' input cloud dataset.
481  * \param lrf_estimation a pointer to a local reference frame estimation class to be used as default.
482  * \return true if frames_ has been correctly initialized.
483  */
485  virtual bool
486  initLocalReferenceFrames (const std::size_t& indices_size,
487  const LRFEstimationPtr& lrf_estimation = LRFEstimationPtr());
488  };
489 }
490 
491 #include <pcl/features/impl/feature.hpp>
void setInputLabels(const PointCloudLConstPtr &labels)
Provide a pointer to the input dataset that contains the point labels of the XYZ dataset.
Definition: feature.h:398
FeatureFromLabels()
Empty constructor.
Definition: feature.h:386
PointCloudLConstPtr labels_
A pointer to the input dataset that contains the point labels of the XYZ dataset.
Definition: feature.h:414
virtual bool initCompute()
This method should get called before starting the actual computation.
Definition: feature.hpp:265
PointCloudLConstPtr getInputLabels() const
Get a pointer to the labels of the input XYZ point cloud dataset.
Definition: feature.h:405
FeatureFromNormals()
Empty constructor.
Definition: feature.h:329
virtual bool initCompute()
This method should get called before starting the actual computation.
Definition: feature.hpp:232
PointCloudNConstPtr getInputNormals() const
Get a pointer to the normals of the input XYZ point cloud dataset.
Definition: feature.h:343
void setInputNormals(const PointCloudNConstPtr &normals)
Provide a pointer to the input dataset that contains the point normals of the XYZ dataset.
Definition: feature.h:339
typename PointCloudN::Ptr PointCloudNPtr
Definition: feature.h:317
typename PointCloudN::ConstPtr PointCloudNConstPtr
Definition: feature.h:318
PointCloudNConstPtr normals_
A pointer to the input dataset that contains the point normals of the XYZ dataset.
Definition: feature.h:349
Feature represents the base feature class.
Definition: feature.h:107
typename PointCloudIn::Ptr PointCloudInPtr
Definition: feature.h:121
PointCloudInConstPtr getSearchSurface() const
Get a pointer to the surface point cloud dataset.
Definition: feature.h:155
double search_parameter_
The actual search parameter (from either search_radius_ or k_).
Definition: feature.h:234
int searchForNeighbors(std::size_t index, double parameter, pcl::Indices &indices, std::vector< float > &distances) const
Search for k-nearest neighbors using the spatial locator from setSearchmethod, and the given surface ...
Definition: feature.h:268
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:146
pcl::PointCloud< PointOutT > PointCloudOut
Definition: feature.h:124
double search_radius_
The nearest neighbors search radius for each point.
Definition: feature.h:237
virtual bool initCompute()
This method should get called before starting the actual computation.
Definition: feature.hpp:95
std::function< int(std::size_t, double, pcl::Indices &, std::vector< float > &)> SearchMethod
Definition: feature.h:126
int k_
The number of K nearest neighbors to use for each point.
Definition: feature.h:240
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:198
shared_ptr< Feature< PointInT, PointOutT > > Ptr
Definition: feature.h:114
void setKSearch(int k)
Set the number of k nearest neighbors to use for the feature estimation.
Definition: feature.h:184
std::string feature_name_
The feature name.
Definition: feature.h:220
shared_ptr< const Feature< PointInT, PointOutT > > ConstPtr
Definition: feature.h:115
KdTreePtr tree_
A pointer to the spatial search object.
Definition: feature.h:231
typename KdTree::Ptr KdTreePtr
Definition: feature.h:118
double getSearchParameter() const
Get the internal search parameter.
Definition: feature.h:175
int getKSearch() const
get the number of k nearest neighbors used for the feature estimation.
Definition: feature.h:188
std::function< int(const PointCloudIn &cloud, std::size_t index, double, pcl::Indices &, std::vector< float > &)> SearchMethodSurface
Definition: feature.h:127
Feature()
Empty constructor.
Definition: feature.h:131
PointCloudInConstPtr surface_
An input point cloud describing the surface that is to be used for nearest neighbors estimation.
Definition: feature.h:228
typename PointCloudIn::ConstPtr PointCloudInConstPtr
Definition: feature.h:122
bool fake_surface_
If no surface is given, we use the input PointCloud as the surface.
Definition: feature.h:255
int searchForNeighbors(const PointCloudIn &cloud, std::size_t index, double parameter, pcl::Indices &indices, std::vector< float > &distances) const
Search for k-nearest neighbors using the spatial locator from setSearchmethod, and the given surface ...
Definition: feature.h:286
virtual bool deinitCompute()
This method should get called after ending the actual computation.
Definition: feature.hpp:181
double getRadiusSearch() const
Get the sphere radius used for determining the neighbors.
Definition: feature.h:205
KdTreePtr getSearchMethod() const
Get a pointer to the search method used.
Definition: feature.h:168
void setSearchMethod(const KdTreePtr &tree)
Provide a pointer to the search object.
Definition: feature.h:164
void compute(PointCloudOut &output)
Base method for feature estimation for all points given in <setInputCloud (), setIndices ()> using th...
Definition: feature.hpp:194
const std::string & getClassName() const
Get a string representation of the name of this class.
Definition: feature.h:244
SearchMethodSurface search_method_surface_
The search method template for points.
Definition: feature.h:223
FeatureWithLocalReferenceFrames provides a public interface for descriptor extractor classes which ne...
Definition: feature.h:440
PointCloudLRFConstPtr frames_
A boost shared pointer to the local reference frames.
Definition: feature.h:475
typename PointCloudLRF::Ptr PointCloudLRFPtr
Definition: feature.h:443
PointCloudLRFConstPtr getInputReferenceFrames() const
Get a pointer to the local reference frames.
Definition: feature.h:468
typename Feature< PointInT, PointRFT >::Ptr LRFEstimationPtr
Check if frames_ has been correctly initialized and compute it if needed.
Definition: feature.h:484
virtual bool initLocalReferenceFrames(const std::size_t &indices_size, const LRFEstimationPtr &lrf_estimation=LRFEstimationPtr())
Definition: feature.hpp:294
virtual ~FeatureWithLocalReferenceFrames()=default
Default virtual destructor.
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:460
typename PointCloudLRF::ConstPtr PointCloudLRFConstPtr
Definition: feature.h:444
FeatureWithLocalReferenceFrames()
Empty constructor.
Definition: feature.h:447
bool frames_never_defined_
The user has never set the frames.
Definition: feature.h:477
PCL base class.
Definition: pcl_base.h:70
PointCloudConstPtr input_
The input point cloud dataset.
Definition: pcl_base.h:147
shared_ptr< PointCloud< PointInT > > Ptr
Definition: point_cloud.h:413
shared_ptr< const PointCloud< PointInT > > ConstPtr
Definition: point_cloud.h:414
shared_ptr< pcl::search::Search< PointInT > > Ptr
Definition: search.h:81
#define PCL_MAKE_ALIGNED_OPERATOR_NEW
Macro to signal a class requires a custom allocator.
Definition: memory.h:63
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:52
Defines functions, macros and traits for allocating and using memory.
IndicesAllocator<> Indices
Type used for indices in PCL.
Definition: types.h:133
Defines all the PCL and non-PCL macros used.