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