Point Cloud Library (PCL)  1.14.1-dev
vfh.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 <array>
44 
45 #include <pcl/point_types.h>
46 #include <pcl/features/feature.h>
47 
48 namespace pcl
49 {
50  /** \brief VFHEstimation estimates the <b>Viewpoint Feature Histogram (VFH)</b> descriptor for a given point cloud
51  * dataset containing points and normals. The default VFH implementation uses 45 binning subdivisions for each of
52  * the three extended FPFH values, plus another 45 binning subdivisions for the distances between each point and
53  * the centroid and 128 binning subdivisions for the viewpoint component, which results in a
54  * 308-byte array of float values. These are stored in a pcl::VFHSignature308 point type.
55  * A major difference between the PFH/FPFH descriptors and VFH, is that for a given point cloud dataset, only a
56  * single VFH descriptor will be estimated (vfhs->size() should be 1), while the resultant PFH/FPFH data
57  * will have the same number of entries as the number of points in the cloud.
58  *
59  * \note If you use this code in any academic work, please cite:
60  *
61  * - R.B. Rusu, G. Bradski, R. Thibaux, J. Hsu.
62  * Fast 3D Recognition and Pose Using the Viewpoint Feature Histogram.
63  * In Proceedings of International Conference on Intelligent Robots and Systems (IROS)
64  * Taipei, Taiwan, October 18-22 2010.
65  *
66  * \note The code is stateful as we do not expect this class to be multicore parallelized. Please look at
67  * \ref FPFHEstimationOMP for an example of a parallel implementation of the FPFH (Fast Point Feature Histogram).
68  * \author Radu B. Rusu
69  * \ingroup features
70  */
71  template<typename PointInT, typename PointNT, typename PointOutT = pcl::VFHSignature308>
72  class VFHEstimation : public FeatureFromNormals<PointInT, PointNT, PointOutT>
73  {
74  public:
83 
85  using Ptr = shared_ptr<VFHEstimation<PointInT, PointNT, PointOutT> >;
86  using ConstPtr = shared_ptr<const VFHEstimation<PointInT, PointNT, PointOutT> >;
87 
88 
89  /** \brief Empty constructor. */
91  nr_bins_f_ ({45, 45, 45, 45}),
92  d_pi_ (1.0f / (2.0f * static_cast<float> (M_PI)))
93  {
94  for (int i = 0; i < 4; ++i)
95  {
96  hist_f_[i].setZero (nr_bins_f_[i]);
97  }
98  search_radius_ = 0;
99  k_ = 0;
100  feature_name_ = "VFHEstimation";
101  }
102 
103  /** \brief Estimate the SPFH (Simple Point Feature Histograms) signatures of the angular
104  * (f1, f2, f3) and distance (f4) features for a given point from its neighborhood
105  * \param[in] centroid_p the centroid point
106  * \param[in] centroid_n the centroid normal
107  * \param[in] cloud the dataset containing the XYZ Cartesian coordinates of the two points
108  * \param[in] normals the dataset containing the surface normals at each point in \a cloud
109  * \param[in] indices the k-neighborhood point indices in the dataset
110  */
111  void
112  computePointSPFHSignature (const Eigen::Vector4f &centroid_p, const Eigen::Vector4f &centroid_n,
113  const pcl::PointCloud<PointInT> &cloud, const pcl::PointCloud<PointNT> &normals,
114  const pcl::Indices &indices);
115 
116  /** \brief Set the viewpoint.
117  * \param[in] vpx the X coordinate of the viewpoint
118  * \param[in] vpy the Y coordinate of the viewpoint
119  * \param[in] vpz the Z coordinate of the viewpoint
120  */
121  inline void
122  setViewPoint (float vpx, float vpy, float vpz)
123  {
124  vpx_ = vpx;
125  vpy_ = vpy;
126  vpz_ = vpz;
127  }
128 
129  /** \brief Get the viewpoint. */
130  inline void
131  getViewPoint (float &vpx, float &vpy, float &vpz)
132  {
133  vpx = vpx_;
134  vpy = vpy_;
135  vpz = vpz_;
136  }
137 
138  /** \brief Set use_given_normal_
139  * \param[in] use Set to true if you want to use the normal passed to setNormalUse(normal)
140  */
141  inline void
142  setUseGivenNormal (bool use)
143  {
144  use_given_normal_ = use;
145  }
146 
147  /** \brief Set the normal to use
148  * \param[in] normal Sets the normal to be used in the VFH computation. It is is used
149  * to build the Darboux Coordinate system.
150  */
151  inline void
152  setNormalToUse (const Eigen::Vector3f &normal)
153  {
154  normal_to_use_ = Eigen::Vector4f (normal[0], normal[1], normal[2], 0);
155  }
156 
157  /** \brief Set use_given_centroid_
158  * \param[in] use Set to true if you want to use the centroid passed through setCentroidToUse(centroid)
159  */
160  inline void
162  {
163  use_given_centroid_ = use;
164  }
165 
166  /** \brief Set centroid_to_use_
167  * \param[in] centroid Centroid to be used in the VFH computation. It is used to compute the distances
168  * from all points to this centroid.
169  */
170  inline void
171  setCentroidToUse (const Eigen::Vector3f &centroid)
172  {
173  centroid_to_use_ = Eigen::Vector4f (centroid[0], centroid[1], centroid[2], 0);
174  }
175 
176  /** \brief set normalize_bins_
177  * \param[in] normalize If true, the VFH bins are normalized using the total number of points
178  */
179  inline void
180  setNormalizeBins (bool normalize)
181  {
182  normalize_bins_ = normalize;
183  }
184 
185  /** \brief set normalize_distances_
186  * \param[in] normalize If true, the 4th component of VFH (shape distribution component) get normalized
187  * by the maximum size between the centroid and the point cloud
188  */
189  inline void
190  setNormalizeDistance (bool normalize)
191  {
192  normalize_distances_ = normalize;
193  }
194 
195  /** \brief set size_component_
196  * \param[in] fill_size True if the 4th component of VFH (shape distribution component) needs to be filled.
197  * Otherwise, it is set to zero.
198  */
199  inline void
200  setFillSizeComponent (bool fill_size)
201  {
202  size_component_ = fill_size;
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 
213  /** \brief The number of subdivisions for each feature interval. */
214  std::array<int, 4> nr_bins_f_;
215  int nr_bins_vp_{128};
216 
217  /** \brief Values describing the viewpoint ("pinhole" camera model assumed). For per point viewpoints, inherit
218  * from VFHEstimation and provide your own computeFeature (). By default, the viewpoint is set to 0,0,0.
219  */
220  float vpx_{0.0f}, vpy_{0.0f}, vpz_{0.0f};
221 
222  /** \brief Estimate the Viewpoint Feature Histograms (VFH) descriptors at a set of points given by
223  * <setInputCloud (), setIndices ()> using the surface in setSearchSurface () and the spatial locator in
224  * setSearchMethod ()
225  * \param[out] output the resultant point cloud model dataset that contains the VFH feature estimates
226  */
227  void
228  computeFeature (PointCloudOut &output) override;
229 
230  protected:
231  /** \brief This method should get called before starting the actual computation. */
232  bool
233  initCompute () override;
234 
235  /** \brief Placeholder for the f1 histogram. */
236  std::array<Eigen::VectorXf, 4> hist_f_;
237  /** \brief Placeholder for the vp histogram. */
238  Eigen::VectorXf hist_vp_;
239 
240  /** \brief Normal to be used to computed VFH. Default, the average normal of the whole point cloud */
241  Eigen::Vector4f normal_to_use_;
242  /** \brief Centroid to be used to computed VFH. Default, the centroid of the whole point cloud */
243  Eigen::Vector4f centroid_to_use_;
244 
245  // VFH configuration parameters because CVFH instantiates it. See constructor for default values.
246 
247  /** \brief Use the normal_to_use_ */
248  bool use_given_normal_{false};
249  /** \brief Use the centroid_to_use_ */
250  bool use_given_centroid_{false};
251  /** \brief Normalize bins by the number the total number of points. */
252  bool normalize_bins_{true};
253  /** \brief Normalize the shape distribution component of VFH */
254  bool normalize_distances_{false};
255  /** \brief Activate or deactivate the size component of VFH */
256  bool size_component_{false};
257 
258  private:
259  /** \brief Float constant = 1.0 / (2.0 * M_PI) */
260  float d_pi_;
261  };
262 }
263 
264 #ifdef PCL_NO_PRECOMPILE
265 #include <pcl/features/impl/vfh.hpp>
266 #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
VFHEstimation estimates the Viewpoint Feature Histogram (VFH) descriptor for a given point cloud data...
Definition: vfh.h:73
bool normalize_bins_
Normalize bins by the number the total number of points.
Definition: vfh.h:252
void setFillSizeComponent(bool fill_size)
set size_component_
Definition: vfh.h:200
Eigen::Vector4f normal_to_use_
Normal to be used to computed VFH.
Definition: vfh.h:241
typename Feature< PointInT, PointOutT >::PointCloudOut PointCloudOut
Definition: vfh.h:84
bool use_given_normal_
Use the normal_to_use_.
Definition: vfh.h:248
VFHEstimation()
Empty constructor.
Definition: vfh.h:90
void setViewPoint(float vpx, float vpy, float vpz)
Set the viewpoint.
Definition: vfh.h:122
Eigen::VectorXf hist_vp_
Placeholder for the vp histogram.
Definition: vfh.h:238
void setNormalizeDistance(bool normalize)
set normalize_distances_
Definition: vfh.h:190
bool size_component_
Activate or deactivate the size component of VFH.
Definition: vfh.h:256
void setUseGivenNormal(bool use)
Set use_given_normal_.
Definition: vfh.h:142
void setCentroidToUse(const Eigen::Vector3f &centroid)
Set centroid_to_use_.
Definition: vfh.h:171
void compute(PointCloudOut &output)
Overloaded computed method from pcl::Feature.
Definition: vfh.hpp:65
bool initCompute() override
This method should get called before starting the actual computation.
Definition: vfh.hpp:51
void setNormalToUse(const Eigen::Vector3f &normal)
Set the normal to use.
Definition: vfh.h:152
bool normalize_distances_
Normalize the shape distribution component of VFH.
Definition: vfh.h:254
Eigen::Vector4f centroid_to_use_
Centroid to be used to computed VFH.
Definition: vfh.h:243
void computePointSPFHSignature(const Eigen::Vector4f &centroid_p, const Eigen::Vector4f &centroid_n, const pcl::PointCloud< PointInT > &cloud, const pcl::PointCloud< PointNT > &normals, const pcl::Indices &indices)
Estimate the SPFH (Simple Point Feature Histograms) signatures of the angular (f1,...
Definition: vfh.hpp:92
void setNormalizeBins(bool normalize)
set normalize_bins_
Definition: vfh.h:180
bool use_given_centroid_
Use the centroid_to_use_.
Definition: vfh.h:250
std::array< Eigen::VectorXf, 4 > hist_f_
Placeholder for the f1 histogram.
Definition: vfh.h:236
void getViewPoint(float &vpx, float &vpy, float &vpz)
Get the viewpoint.
Definition: vfh.h:131
void setUseGivenCentroid(bool use)
Set use_given_centroid_.
Definition: vfh.h:161
Defines all the PCL implemented PointT point type structures.
IndicesAllocator<> Indices
Type used for indices in PCL.
Definition: types.h:133
#define M_PI
Definition: pcl_macros.h:201