Point Cloud Library (PCL)  1.14.1-dev
our_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: cvfh.h 4936 2012-03-07 11:12:45Z aaldoma $
38  *
39  */
40 
41 #pragma once
42 
43 #include <pcl/features/feature.h>
44 
45 namespace pcl
46 {
47  /** \brief OURCVFHEstimation estimates the Oriented, Unique and Repetable Clustered Viewpoint Feature Histogram (CVFH) descriptor for a given
48  * point cloud dataset given XYZ data and normals, as presented in:
49  * - OUR-CVFH – Oriented, Unique and Repeatable Clustered Viewpoint Feature Histogram for Object Recognition and 6DOF Pose Estimation
50  * A. Aldoma, F. Tombari, R.B. Rusu and M. Vincze
51  * DAGM-OAGM 2012
52  * Graz, Austria
53  * The suggested PointOutT is pcl::VFHSignature308.
54  *
55  * \author Aitor Aldoma
56  * \ingroup features
57  */
58  template<typename PointInT, typename PointNT, typename PointOutT = pcl::VFHSignature308>
59  class OURCVFHEstimation : public FeatureFromNormals<PointInT, PointNT, PointOutT>
60  {
61  public:
62  using Ptr = shared_ptr<OURCVFHEstimation<PointInT, PointNT, PointOutT> >;
63  using ConstPtr = shared_ptr<const OURCVFHEstimation<PointInT, PointNT, PointOutT> >;
71 
75  /** \brief Empty constructor. */
77  cluster_tolerance_ (leaf_size_ * 3),
78  radius_normals_ (leaf_size_ * 3)
79  {
80  search_radius_ = 0;
81  k_ = 1;
82  feature_name_ = "OURCVFHEstimation";
83  refine_clusters_ = 1.f;
84  min_axis_value_ = 0.925f;
85  axis_ratio_ = 0.8f;
86  }
87  ;
88 
89  /** \brief Creates an affine transformation from the RF axes
90  * \param[in] evx the x-axis
91  * \param[in] evy the y-axis
92  * \param[in] evz the z-axis
93  * \param[out] transformPC the resulting transformation
94  * \param[in] center_mat 4x4 matrix concatenated to the resulting transformation
95  */
96  inline Eigen::Matrix4f
97  createTransFromAxes (Eigen::Vector3f & evx, Eigen::Vector3f & evy, Eigen::Vector3f & evz, Eigen::Affine3f & transformPC,
98  Eigen::Matrix4f & center_mat)
99  {
100  Eigen::Matrix4f trans;
101  trans.setIdentity (4, 4);
102  trans (0, 0) = evx (0, 0);
103  trans (1, 0) = evx (1, 0);
104  trans (2, 0) = evx (2, 0);
105  trans (0, 1) = evy (0, 0);
106  trans (1, 1) = evy (1, 0);
107  trans (2, 1) = evy (2, 0);
108  trans (0, 2) = evz (0, 0);
109  trans (1, 2) = evz (1, 0);
110  trans (2, 2) = evz (2, 0);
111 
112  Eigen::Matrix4f homMatrix = Eigen::Matrix4f ();
113  homMatrix.setIdentity (4, 4);
114  homMatrix = transformPC.matrix ();
115 
116  Eigen::Matrix4f trans_copy = trans.inverse ();
117  trans = trans_copy * center_mat * homMatrix;
118  return trans;
119  }
120 
121  /** \brief Computes SGURF and the shape distribution based on the selected SGURF
122  * \param[in] processed the input cloud
123  * \param[out] output the resulting signature
124  * \param[in] cluster_indices the indices of the stable cluster
125  */
126  void
127  computeRFAndShapeDistribution (PointInTPtr & processed, PointCloudOut &output, std::vector<pcl::PointIndices> & cluster_indices);
128 
129  /** \brief Computes SGURF
130  * \param[in] centroid the centroid of the cluster
131  * \param[in] normal_centroid the average of the normals
132  * \param[in] processed the input cloud
133  * \param[out] transformations the transformations aligning the cloud to the SGURF axes
134  * \param[out] grid the cloud transformed internally
135  * \param[in] indices the indices of the stable cluster
136  */
137  bool
138  sgurf (Eigen::Vector3f & centroid, Eigen::Vector3f & normal_centroid, PointInTPtr & processed, std::vector<Eigen::Matrix4f, Eigen::aligned_allocator<Eigen::Matrix4f> > & transformations,
139  PointInTPtr & grid, pcl::PointIndices & indices);
140 
141  /** \brief Removes normals with high curvature caused by real edges or noisy data
142  * \param[in] cloud pointcloud to be filtered
143  * \param[in] indices_to_use
144  * \param[out] indices_out the indices of the points with higher curvature than threshold
145  * \param[out] indices_in the indices of the remaining points after filtering
146  * \param[in] threshold threshold value for curvature
147  */
148  void
149  filterNormalsWithHighCurvature (const pcl::PointCloud<PointNT> & cloud, pcl::Indices & indices_to_use, pcl::Indices &indices_out,
150  pcl::Indices &indices_in, float threshold);
151 
152  /** \brief Set the viewpoint.
153  * \param[in] vpx the X coordinate of the viewpoint
154  * \param[in] vpy the Y coordinate of the viewpoint
155  * \param[in] vpz the Z coordinate of the viewpoint
156  */
157  inline void
158  setViewPoint (float vpx, float vpy, float vpz)
159  {
160  vpx_ = vpx;
161  vpy_ = vpy;
162  vpz_ = vpz;
163  }
164 
165  /** \brief Set the radius used to compute normals
166  * \param[in] radius_normals the radius
167  */
168  inline void
169  setRadiusNormals (float radius_normals)
170  {
171  radius_normals_ = radius_normals;
172  }
173 
174  /** \brief Get the viewpoint.
175  * \param[out] vpx the X coordinate of the viewpoint
176  * \param[out] vpy the Y coordinate of the viewpoint
177  * \param[out] vpz the Z coordinate of the viewpoint
178  */
179  inline void
180  getViewPoint (float &vpx, float &vpy, float &vpz)
181  {
182  vpx = vpx_;
183  vpy = vpy_;
184  vpz = vpz_;
185  }
186 
187  /** \brief Get the centroids used to compute different CVFH descriptors
188  * \param[out] centroids vector to hold the centroids
189  */
190  inline void
191  getCentroidClusters (std::vector<Eigen::Vector3f, Eigen::aligned_allocator<Eigen::Vector3f> > & centroids)
192  {
193  for (const auto & centroids_dominant_orientation : centroids_dominant_orientations_)
194  centroids.push_back (centroids_dominant_orientation);
195  }
196 
197  /** \brief Get the normal centroids used to compute different CVFH descriptors
198  * \param[out] centroids vector to hold the normal centroids
199  */
200  inline void
201  getCentroidNormalClusters (std::vector<Eigen::Vector3f, Eigen::aligned_allocator<Eigen::Vector3f> > & centroids)
202  {
203  for (const auto & dominant_normal : dominant_normals_)
204  centroids.push_back (dominant_normal);
205  }
206 
207  /** \brief Sets max. Euclidean distance between points to be added to the cluster
208  * \param[in] d the maximum Euclidean distance
209  */
210 
211  inline void
213  {
214  cluster_tolerance_ = d;
215  }
216 
217  /** \brief Sets max. deviation of the normals between two points so they can be clustered together
218  * \param[in] d the maximum deviation
219  */
220  inline void
222  {
223  eps_angle_threshold_ = d;
224  }
225 
226  /** \brief Sets curvature threshold for removing normals
227  * \param[in] d the curvature threshold
228  */
229  inline void
231  {
232  curv_threshold_ = d;
233  }
234 
235  /** \brief Set minimum amount of points for a cluster to be considered
236  * \param[in] min the minimum amount of points to be set
237  */
238  inline void
239  setMinPoints (std::size_t min)
240  {
241  min_points_ = min;
242  }
243 
244  /** \brief Sets whether the signatures should be normalized or not
245  * \param[in] normalize true if normalization is required, false otherwise
246  */
247  inline void
248  setNormalizeBins (bool normalize)
249  {
250  normalize_bins_ = normalize;
251  }
252 
253  /** \brief Gets the indices of the original point cloud used to compute the signatures
254  * \param[out] indices vector of point indices
255  */
256  inline void
257  getClusterIndices (std::vector<pcl::PointIndices> & indices)
258  {
259  indices = clusters_;
260  }
261 
262  /** \brief Gets the number of non-disambiguable axes that correspond to each centroid
263  * \param[out] cluster_axes vector mapping each centroid to the number of signatures
264  */
265  inline void
266  getClusterAxes (std::vector<short> & cluster_axes)
267  {
268  cluster_axes = cluster_axes_;
269  }
270 
271  /** \brief Sets the refinement factor for the clusters
272  * \param[in] rc the factor used to decide if a point is used to estimate a stable cluster
273  */
274  void
275  setRefineClusters (float rc)
276  {
277  refine_clusters_ = rc;
278  }
279 
280  /** \brief Returns the transformations aligning the point cloud to the corresponding SGURF
281  * \param[out] trans vector of transformations
282  */
283  void
284  getTransforms (std::vector<Eigen::Matrix4f, Eigen::aligned_allocator<Eigen::Matrix4f> > & trans)
285  {
286  trans = transforms_;
287  }
288 
289  /** \brief Returns a boolean vector indicating of the transformation obtained by getTransforms() represents
290  * a valid SGURF
291  * \param[out] valid vector of booleans
292  */
293  void
294  getValidTransformsVec (std::vector<bool> & valid)
295  {
296  valid = valid_transforms_;
297  }
298 
299  /** \brief Sets the min axis ratio between the SGURF axes to decide if disambiguition is feasible
300  * \param[in] f the ratio between axes
301  */
302  void
303  setAxisRatio (float f)
304  {
305  axis_ratio_ = f;
306  }
307 
308  /** \brief Sets the min disambiguition axis value to generate several SGURFs for the cluster when disambiguition is difficult
309  * \param[in] f the min axis value
310  */
311  void
312  setMinAxisValue (float f)
313  {
314  min_axis_value_ = f;
315  }
316 
317  /** \brief Overloaded computed method from pcl::Feature.
318  * \param[out] output the resultant point cloud model dataset containing the estimated features
319  */
320  void
321  compute (PointCloudOut &output);
322 
323  private:
324  /** \brief Values describing the viewpoint ("pinhole" camera model assumed).
325  * By default, the viewpoint is set to 0,0,0.
326  */
327  float vpx_{0.0f}, vpy_{0.0f}, vpz_{0.0f};
328 
329  /** \brief Size of the voxels after voxel gridding. IMPORTANT: Must match the voxel
330  * size of the training data or the normalize_bins_ flag must be set to true.
331  */
332  float leaf_size_{0.005f};
333 
334  /** \brief Whether to normalize the signatures or not. Default: false. */
335  bool normalize_bins_{false};
336 
337  /** \brief Curvature threshold for removing normals. */
338  float curv_threshold_{0.03f};
339 
340  /** \brief allowed Euclidean distance between points to be added to the cluster. */
341  float cluster_tolerance_;
342 
343  /** \brief deviation of the normals between two points so they can be clustered together. */
344  float eps_angle_threshold_{0.125f};
345 
346  /** \brief Minimum amount of points in a clustered region to be considered stable for CVFH
347  * computation.
348  */
349  std::size_t min_points_{50};
350 
351  /** \brief Radius for the normals computation. */
352  float radius_normals_;
353 
354  /** \brief Factor for the cluster refinement */
355  float refine_clusters_;
356 
357  std::vector<Eigen::Matrix4f, Eigen::aligned_allocator<Eigen::Matrix4f> > transforms_;
358  std::vector<bool> valid_transforms_;
359 
360  float axis_ratio_;
361  float min_axis_value_;
362 
363  /** \brief Estimate the OUR-CVFH descriptors at
364  * a set of points given by <setInputCloud (), setIndices ()> using the surface in
365  * setSearchSurface ()
366  *
367  * \param[out] output the resultant point cloud model dataset that contains the OUR-CVFH
368  * feature estimates
369  */
370  void
371  computeFeature (PointCloudOut &output) override;
372 
373  /** \brief Region growing method using Euclidean distances and neighbors normals to
374  * add points to a region.
375  * \param[in] cloud point cloud to split into regions
376  * \param[in] normals are the normals of cloud
377  * \param[in] tolerance is the allowed Euclidean distance between points to be added to
378  * the cluster
379  * \param[in] tree is the spatial search structure for nearest neighbour search
380  * \param[out] clusters vector of indices representing the clustered regions
381  * \param[in] eps_angle deviation of the normals between two points so they can be
382  * clustered together
383  * \param[in] min_pts_per_cluster minimum cluster size. (default: 1 point)
384  * \param[in] max_pts_per_cluster maximum cluster size. (default: all the points)
385  */
386  void
387  extractEuclideanClustersSmooth (const pcl::PointCloud<pcl::PointNormal> &cloud, const pcl::PointCloud<pcl::PointNormal> &normals,
388  float tolerance, const pcl::search::Search<pcl::PointNormal>::Ptr &tree,
389  std::vector<pcl::PointIndices> &clusters, double eps_angle, unsigned int min_pts_per_cluster = 1,
390  unsigned int max_pts_per_cluster = (std::numeric_limits<int>::max) ());
391 
392  protected:
393  /** \brief Centroids that were used to compute different OUR-CVFH descriptors */
394  std::vector<Eigen::Vector3f, Eigen::aligned_allocator<Eigen::Vector3f> > centroids_dominant_orientations_;
395  /** \brief Normal centroids that were used to compute different OUR-CVFH descriptors */
396  std::vector<Eigen::Vector3f, Eigen::aligned_allocator<Eigen::Vector3f> > dominant_normals_;
397  /** \brief Indices to the points representing the stable clusters */
398  std::vector<pcl::PointIndices> clusters_;
399  /** \brief Mapping from clusters to OUR-CVFH descriptors */
400  std::vector<short> cluster_axes_;
401  };
402 }
403 
404 #ifdef PCL_NO_PRECOMPILE
405 #include <pcl/features/impl/our_cvfh.hpp>
406 #endif
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
shared_ptr< Feature< PointInT, PointOutT > > Ptr
Definition: feature.h:114
std::string feature_name_
The feature name.
Definition: feature.h:220
shared_ptr< const Feature< PointInT, PointOutT > > ConstPtr
Definition: feature.h:115
typename KdTree::Ptr KdTreePtr
Definition: feature.h:118
OURCVFHEstimation estimates the Oriented, Unique and Repetable Clustered Viewpoint Feature Histogram ...
Definition: our_cvfh.h:60
void setClusterTolerance(float d)
Sets max.
Definition: our_cvfh.h:212
void setNormalizeBins(bool normalize)
Sets whether the signatures should be normalized or not.
Definition: our_cvfh.h:248
void getValidTransformsVec(std::vector< bool > &valid)
Returns a boolean vector indicating of the transformation obtained by getTransforms() represents a va...
Definition: our_cvfh.h:294
bool sgurf(Eigen::Vector3f &centroid, Eigen::Vector3f &normal_centroid, PointInTPtr &processed, std::vector< Eigen::Matrix4f, Eigen::aligned_allocator< Eigen::Matrix4f > > &transformations, PointInTPtr &grid, pcl::PointIndices &indices)
Computes SGURF.
Definition: our_cvfh.hpp:200
typename Feature< PointInT, PointOutT >::PointCloudOut PointCloudOut
Definition: our_cvfh.h:72
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: our_cvfh.hpp:170
void getCentroidClusters(std::vector< Eigen::Vector3f, Eigen::aligned_allocator< Eigen::Vector3f > > &centroids)
Get the centroids used to compute different CVFH descriptors.
Definition: our_cvfh.h:191
void getViewPoint(float &vpx, float &vpy, float &vpz)
Get the viewpoint.
Definition: our_cvfh.h:180
void setRadiusNormals(float radius_normals)
Set the radius used to compute normals.
Definition: our_cvfh.h:169
void getTransforms(std::vector< Eigen::Matrix4f, Eigen::aligned_allocator< Eigen::Matrix4f > > &trans)
Returns the transformations aligning the point cloud to the corresponding SGURF.
Definition: our_cvfh.h:284
void getCentroidNormalClusters(std::vector< Eigen::Vector3f, Eigen::aligned_allocator< Eigen::Vector3f > > &centroids)
Get the normal centroids used to compute different CVFH descriptors.
Definition: our_cvfh.h:201
void setMinAxisValue(float f)
Sets the min disambiguition axis value to generate several SGURFs for the cluster when disambiguition...
Definition: our_cvfh.h:312
void compute(PointCloudOut &output)
Overloaded computed method from pcl::Feature.
Definition: our_cvfh.hpp:53
Eigen::Matrix4f createTransFromAxes(Eigen::Vector3f &evx, Eigen::Vector3f &evy, Eigen::Vector3f &evz, Eigen::Affine3f &transformPC, Eigen::Matrix4f &center_mat)
Creates an affine transformation from the RF axes.
Definition: our_cvfh.h:97
void setViewPoint(float vpx, float vpy, float vpz)
Set the viewpoint.
Definition: our_cvfh.h:158
void setRefineClusters(float rc)
Sets the refinement factor for the clusters.
Definition: our_cvfh.h:275
OURCVFHEstimation()
Empty constructor.
Definition: our_cvfh.h:76
std::vector< pcl::PointIndices > clusters_
Indices to the points representing the stable clusters.
Definition: our_cvfh.h:398
std::vector< Eigen::Vector3f, Eigen::aligned_allocator< Eigen::Vector3f > > dominant_normals_
Normal centroids that were used to compute different OUR-CVFH descriptors.
Definition: our_cvfh.h:396
std::vector< short > cluster_axes_
Mapping from clusters to OUR-CVFH descriptors.
Definition: our_cvfh.h:400
std::vector< Eigen::Vector3f, Eigen::aligned_allocator< Eigen::Vector3f > > centroids_dominant_orientations_
Centroids that were used to compute different OUR-CVFH descriptors.
Definition: our_cvfh.h:394
void setAxisRatio(float f)
Sets the min axis ratio between the SGURF axes to decide if disambiguition is feasible.
Definition: our_cvfh.h:303
typename pcl::PointCloud< PointInT >::Ptr PointInTPtr
Definition: our_cvfh.h:74
void setCurvatureThreshold(float d)
Sets curvature threshold for removing normals.
Definition: our_cvfh.h:230
void getClusterIndices(std::vector< pcl::PointIndices > &indices)
Gets the indices of the original point cloud used to compute the signatures.
Definition: our_cvfh.h:257
void getClusterAxes(std::vector< short > &cluster_axes)
Gets the number of non-disambiguable axes that correspond to each centroid.
Definition: our_cvfh.h:266
void setMinPoints(std::size_t min)
Set minimum amount of points for a cluster to be considered.
Definition: our_cvfh.h:239
void setEPSAngleThreshold(float d)
Sets max.
Definition: our_cvfh.h:221
void computeRFAndShapeDistribution(PointInTPtr &processed, PointCloudOut &output, std::vector< pcl::PointIndices > &cluster_indices)
Computes SGURF and the shape distribution based on the selected SGURF.
Definition: our_cvfh.hpp:383
shared_ptr< PointCloud< PointT > > Ptr
Definition: point_cloud.h:413
shared_ptr< pcl::search::Search< PointT > > Ptr
Definition: search.h:81
IndicesAllocator<> Indices
Type used for indices in PCL.
Definition: types.h:133