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