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