Point Cloud Library (PCL)  1.14.0-dev
cvfh.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 #include <pcl/features/feature.h>
44 #include <pcl/features/vfh.h>
45 #include <pcl/search/search.h> // for Search
46 
47 namespace pcl
48 {
49  /** \brief CVFHEstimation estimates the Clustered Viewpoint Feature Histogram (CVFH) descriptor for a given
50  * point cloud dataset containing XYZ data and normals, as presented in:
51  * - CAD-Model Recognition and 6 DOF Pose Estimation
52  * A. Aldoma, N. Blodow, D. Gossow, S. Gedikli, R.B. Rusu, M. Vincze and G. Bradski
53  * ICCV 2011, 3D Representation and Recognition (3dRR11) workshop
54  * Barcelona, Spain, (2011)
55  *
56  * The suggested PointOutT is pcl::VFHSignature308.
57  *
58  * \author Aitor Aldoma
59  * \ingroup features
60  */
61  template<typename PointInT, typename PointNT, typename PointOutT = pcl::VFHSignature308>
62  class CVFHEstimation : public FeatureFromNormals<PointInT, PointNT, PointOutT>
63  {
64  public:
65  using Ptr = shared_ptr<CVFHEstimation<PointInT, PointNT, PointOutT> >;
66  using ConstPtr = shared_ptr<const CVFHEstimation<PointInT, PointNT, PointOutT> >;
67 
75 
79 
80  /** \brief Empty constructor. */
82 
83  cluster_tolerance_ (leaf_size_ * 3),
84 
85  radius_normals_ (leaf_size_ * 3)
86  {
87  search_radius_ = 0;
88  k_ = 1;
89  feature_name_ = "CVFHEstimation";
90  }
91  ;
92 
93  /** \brief Removes normals with high curvature caused by real edges or noisy data
94  * \param[in] cloud pointcloud to be filtered
95  * \param[in] indices_to_use the indices to use
96  * \param[out] indices_out the indices of the points with higher curvature than threshold
97  * \param[out] indices_in the indices of the remaining points after filtering
98  * \param[in] threshold threshold value for curvature
99  */
100  void
101  filterNormalsWithHighCurvature (const pcl::PointCloud<PointNT> & cloud, pcl::Indices & indices_to_use, pcl::Indices &indices_out,
102  pcl::Indices &indices_in, float threshold);
103 
104  /** \brief Set the viewpoint.
105  * \param[in] vpx the X coordinate of the viewpoint
106  * \param[in] vpy the Y coordinate of the viewpoint
107  * \param[in] vpz the Z coordinate of the viewpoint
108  */
109  inline void
110  setViewPoint (float vpx, float vpy, float vpz)
111  {
112  vpx_ = vpx;
113  vpy_ = vpy;
114  vpz_ = vpz;
115  }
116 
117  /** \brief Set the radius used to compute normals
118  * \param[in] radius_normals the radius
119  */
120  inline void
121  setRadiusNormals (float radius_normals)
122  {
123  radius_normals_ = radius_normals;
124  }
125 
126  /** \brief Get the viewpoint.
127  * \param[out] vpx the X coordinate of the viewpoint
128  * \param[out] vpy the Y coordinate of the viewpoint
129  * \param[out] vpz the Z coordinate of the viewpoint
130  */
131  inline void
132  getViewPoint (float &vpx, float &vpy, float &vpz)
133  {
134  vpx = vpx_;
135  vpy = vpy_;
136  vpz = vpz_;
137  }
138 
139  /** \brief Get the centroids used to compute different CVFH descriptors
140  * \param[out] centroids vector to hold the centroids
141  */
142  inline void
143  getCentroidClusters (std::vector<Eigen::Vector3f, Eigen::aligned_allocator<Eigen::Vector3f> > & centroids)
144  {
145  centroids.insert (centroids.cend (), centroids_dominant_orientations_.cbegin (),
147  }
148 
149  /** \brief Get the normal centroids used to compute different CVFH descriptors
150  * \param[out] centroids vector to hold the normal centroids
151  */
152  inline void
153  getCentroidNormalClusters (std::vector<Eigen::Vector3f, Eigen::aligned_allocator<Eigen::Vector3f> > & centroids)
154  {
155  for (const auto& normal: dominant_normals_)
156  centroids.push_back (normal);
157  }
158 
159  /** \brief Sets max. Euclidean distance between points to be added to the cluster
160  * \param[in] d the maximum Euclidean distance
161  */
162 
163  inline void
165  {
166  cluster_tolerance_ = d;
167  }
168 
169  /** \brief Sets max. deviation of the normals between two points so they can be clustered together
170  * \param[in] d the maximum deviation
171  */
172  inline void
174  {
175  eps_angle_threshold_ = d;
176  }
177 
178  /** \brief Sets curvature threshold for removing normals
179  * \param[in] d the curvature threshold
180  */
181  inline void
183  {
184  curv_threshold_ = d;
185  }
186 
187  /** \brief Set minimum amount of points for a cluster to be considered
188  * \param[in] min the minimum amount of points to be set
189  */
190  inline void
191  setMinPoints (std::size_t min)
192  {
193  min_points_ = min;
194  }
195 
196  /** \brief Sets whether if the CVFH signatures should be normalized or not
197  * \param[in] normalize true if normalization is required, false otherwise
198  */
199  inline void
200  setNormalizeBins (bool normalize)
201  {
202  normalize_bins_ = normalize;
203  }
204 
205  /** \brief Overloaded computed method from pcl::Feature.
206  * \param[out] output the resultant point cloud model dataset containing the estimated features
207  */
208  void
209  compute (PointCloudOut &output);
210 
211  private:
212  /** \brief Values describing the viewpoint ("pinhole" camera model assumed).
213  * By default, the viewpoint is set to 0,0,0.
214  */
215  float vpx_{0.0f}, vpy_{0.0f}, vpz_{0.0f};
216 
217  /** \brief Size of the voxels after voxel gridding. IMPORTANT: Must match the voxel
218  * size of the training data or the normalize_bins_ flag must be set to true.
219  */
220  float leaf_size_{0.005f};
221 
222  /** \brief Whether to normalize the signatures or not. Default: false. */
223  bool normalize_bins_{false};
224 
225  /** \brief Curvature threshold for removing normals. */
226  float curv_threshold_{0.03f};
227 
228  /** \brief allowed Euclidean distance between points to be added to the cluster. */
229  float cluster_tolerance_;
230 
231  /** \brief deviation of the normals between two points so they can be clustered together. */
232  float eps_angle_threshold_{0.125f};
233 
234  /** \brief Minimum amount of points in a clustered region to be considered stable for CVFH
235  * computation.
236  */
237  std::size_t min_points_{50};
238 
239  /** \brief Radius for the normals computation. */
240  float radius_normals_;
241 
242  /** \brief Estimate the Clustered Viewpoint Feature Histograms (CVFH) descriptors at
243  * a set of points given by <setInputCloud (), setIndices ()> using the surface in
244  * setSearchSurface ()
245  *
246  * \param[out] output the resultant point cloud model dataset that contains the CVFH
247  * feature estimates
248  */
249  void
250  computeFeature (PointCloudOut &output) override;
251 
252  /** \brief Region growing method using Euclidean distances and neighbors normals to
253  * add points to a region.
254  * \param[in] cloud point cloud to split into regions
255  * \param[in] normals are the normals of cloud
256  * \param[in] tolerance is the allowed Euclidean distance between points to be added to
257  * the cluster
258  * \param[in] tree is the spatial search structure for nearest neighbour search
259  * \param[out] clusters vector of indices representing the clustered regions
260  * \param[in] eps_angle deviation of the normals between two points so they can be
261  * clustered together
262  * \param[in] min_pts_per_cluster minimum cluster size. (default: 1 point)
263  * \param[in] max_pts_per_cluster maximum cluster size. (default: all the points)
264  */
265  void
266  extractEuclideanClustersSmooth (const pcl::PointCloud<pcl::PointNormal> &cloud,
267  const pcl::PointCloud<pcl::PointNormal> &normals, float tolerance,
269  std::vector<pcl::PointIndices> &clusters, double eps_angle,
270  unsigned int min_pts_per_cluster = 1,
271  unsigned int max_pts_per_cluster = (std::numeric_limits<int>::max) ());
272 
273  protected:
274  /** \brief Centroids that were used to compute different CVFH descriptors */
275  std::vector<Eigen::Vector3f, Eigen::aligned_allocator<Eigen::Vector3f> > centroids_dominant_orientations_;
276  /** \brief Normal centroids that were used to compute different CVFH descriptors */
277  std::vector<Eigen::Vector3f, Eigen::aligned_allocator<Eigen::Vector3f> > dominant_normals_;
278  };
279 }
280 
281 #ifdef PCL_NO_PRECOMPILE
282 #include <pcl/features/impl/cvfh.hpp>
283 #endif
CVFHEstimation estimates the Clustered Viewpoint Feature Histogram (CVFH) descriptor for a given poin...
Definition: cvfh.h:63
std::vector< Eigen::Vector3f, Eigen::aligned_allocator< Eigen::Vector3f > > centroids_dominant_orientations_
Centroids that were used to compute different CVFH descriptors.
Definition: cvfh.h:275
void setEPSAngleThreshold(float d)
Sets max.
Definition: cvfh.h:173
shared_ptr< CVFHEstimation< PointInT, PointNT, PointOutT > > Ptr
Definition: cvfh.h:65
void setMinPoints(std::size_t min)
Set minimum amount of points for a cluster to be considered.
Definition: cvfh.h:191
typename Feature< PointInT, PointOutT >::PointCloudOut PointCloudOut
Definition: cvfh.h:76
shared_ptr< const CVFHEstimation< PointInT, PointNT, PointOutT > > ConstPtr
Definition: cvfh.h:66
std::vector< Eigen::Vector3f, Eigen::aligned_allocator< Eigen::Vector3f > > dominant_normals_
Normal centroids that were used to compute different CVFH descriptors.
Definition: cvfh.h:277
void getCentroidNormalClusters(std::vector< Eigen::Vector3f, Eigen::aligned_allocator< Eigen::Vector3f > > &centroids)
Get the normal centroids used to compute different CVFH descriptors.
Definition: cvfh.h:153
void setNormalizeBins(bool normalize)
Sets whether if the CVFH signatures should be normalized or not.
Definition: cvfh.h:200
void setCurvatureThreshold(float d)
Sets curvature threshold for removing normals.
Definition: cvfh.h:182
void setRadiusNormals(float radius_normals)
Set the radius used to compute normals.
Definition: cvfh.h:121
void filterNormalsWithHighCurvature(const pcl::PointCloud< PointNT > &cloud, pcl::Indices &indices_to_use, pcl::Indices &indices_out, pcl::Indices &indices_in, float threshold)
Removes normals with high curvature caused by real edges or noisy data.
Definition: cvfh.hpp:160
void setViewPoint(float vpx, float vpy, float vpz)
Set the viewpoint.
Definition: cvfh.h:110
void getCentroidClusters(std::vector< Eigen::Vector3f, Eigen::aligned_allocator< Eigen::Vector3f > > &centroids)
Get the centroids used to compute different CVFH descriptors.
Definition: cvfh.h:143
typename pcl::search::Search< PointNormal >::Ptr KdTreePtr
Definition: cvfh.h:77
void setClusterTolerance(float d)
Sets max.
Definition: cvfh.h:164
void getViewPoint(float &vpx, float &vpy, float &vpz)
Get the viewpoint.
Definition: cvfh.h:132
CVFHEstimation()
Empty constructor.
Definition: cvfh.h:81
void compute(PointCloudOut &output)
Overloaded computed method from pcl::Feature.
Definition: cvfh.hpp:50
Feature represents the base feature class.
Definition: feature.h:107
double search_radius_
The nearest neighbors search radius for each point.
Definition: feature.h:237
int k_
The number of K nearest neighbors to use for each point.
Definition: feature.h:240
std::string feature_name_
The feature name.
Definition: feature.h:220
VFHEstimation estimates the Viewpoint Feature Histogram (VFH) descriptor for a given point cloud data...
Definition: vfh.h:73
shared_ptr< pcl::search::Search< PointT > > Ptr
Definition: search.h:81
IndicesAllocator<> Indices
Type used for indices in PCL.
Definition: types.h:133