Point Cloud Library (PCL)  1.12.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}), nr_bins_vp_ (128),
92  vpx_ (0), vpy_ (0), vpz_ (0),
93  use_given_normal_ (false), use_given_centroid_ (false),
94  normalize_bins_ (true), normalize_distances_ (false), size_component_ (false),
95  d_pi_ (1.0f / (2.0f * static_cast<float> (M_PI)))
96  {
97  for (int i = 0; i < 4; ++i)
98  {
99  hist_f_[i].setZero (nr_bins_f_[i]);
100  }
101  search_radius_ = 0;
102  k_ = 0;
103  feature_name_ = "VFHEstimation";
104  }
105 
106  /** \brief Estimate the SPFH (Simple Point Feature Histograms) signatures of the angular
107  * (f1, f2, f3) and distance (f4) features for a given point from its neighborhood
108  * \param[in] centroid_p the centroid point
109  * \param[in] centroid_n the centroid normal
110  * \param[in] cloud the dataset containing the XYZ Cartesian coordinates of the two points
111  * \param[in] normals the dataset containing the surface normals at each point in \a cloud
112  * \param[in] indices the k-neighborhood point indices in the dataset
113  */
114  void
115  computePointSPFHSignature (const Eigen::Vector4f &centroid_p, const Eigen::Vector4f &centroid_n,
116  const pcl::PointCloud<PointInT> &cloud, const pcl::PointCloud<PointNT> &normals,
117  const pcl::Indices &indices);
118 
119  /** \brief Set the viewpoint.
120  * \param[in] vpx the X coordinate of the viewpoint
121  * \param[in] vpy the Y coordinate of the viewpoint
122  * \param[in] vpz the Z coordinate of the viewpoint
123  */
124  inline void
125  setViewPoint (float vpx, float vpy, float vpz)
126  {
127  vpx_ = vpx;
128  vpy_ = vpy;
129  vpz_ = vpz;
130  }
131 
132  /** \brief Get the viewpoint. */
133  inline void
134  getViewPoint (float &vpx, float &vpy, float &vpz)
135  {
136  vpx = vpx_;
137  vpy = vpy_;
138  vpz = vpz_;
139  }
140 
141  /** \brief Set use_given_normal_
142  * \param[in] use Set to true if you want to use the normal passed to setNormalUse(normal)
143  */
144  inline void
145  setUseGivenNormal (bool use)
146  {
147  use_given_normal_ = use;
148  }
149 
150  /** \brief Set the normal to use
151  * \param[in] normal Sets the normal to be used in the VFH computation. It is is used
152  * to build the Darboux Coordinate system.
153  */
154  inline void
155  setNormalToUse (const Eigen::Vector3f &normal)
156  {
157  normal_to_use_ = Eigen::Vector4f (normal[0], normal[1], normal[2], 0);
158  }
159 
160  /** \brief Set use_given_centroid_
161  * \param[in] use Set to true if you want to use the centroid passed through setCentroidToUse(centroid)
162  */
163  inline void
165  {
166  use_given_centroid_ = use;
167  }
168 
169  /** \brief Set centroid_to_use_
170  * \param[in] centroid Centroid to be used in the VFH computation. It is used to compute the distances
171  * from all points to this centroid.
172  */
173  inline void
174  setCentroidToUse (const Eigen::Vector3f &centroid)
175  {
176  centroid_to_use_ = Eigen::Vector4f (centroid[0], centroid[1], centroid[2], 0);
177  }
178 
179  /** \brief set normalize_bins_
180  * \param[in] normalize If true, the VFH bins are normalized using the total number of points
181  */
182  inline void
183  setNormalizeBins (bool normalize)
184  {
185  normalize_bins_ = normalize;
186  }
187 
188  /** \brief set normalize_distances_
189  * \param[in] normalize If true, the 4th component of VFH (shape distribution component) get normalized
190  * by the maximum size between the centroid and the point cloud
191  */
192  inline void
193  setNormalizeDistance (bool normalize)
194  {
195  normalize_distances_ = normalize;
196  }
197 
198  /** \brief set size_component_
199  * \param[in] fill_size True if the 4th component of VFH (shape distribution component) needs to be filled.
200  * Otherwise, it is set to zero.
201  */
202  inline void
203  setFillSizeComponent (bool fill_size)
204  {
205  size_component_ = fill_size;
206  }
207 
208  /** \brief Overloaded computed method from pcl::Feature.
209  * \param[out] output the resultant point cloud model dataset containing the estimated features
210  */
211  void
212  compute (PointCloudOut &output);
213 
214  private:
215 
216  /** \brief The number of subdivisions for each feature interval. */
217  std::array<int, 4> nr_bins_f_;
218  int nr_bins_vp_;
219 
220  /** \brief Values describing the viewpoint ("pinhole" camera model assumed). For per point viewpoints, inherit
221  * from VFHEstimation and provide your own computeFeature (). By default, the viewpoint is set to 0,0,0.
222  */
223  float vpx_, vpy_, vpz_;
224 
225  /** \brief Estimate the Viewpoint Feature Histograms (VFH) descriptors at a set of points given by
226  * <setInputCloud (), setIndices ()> using the surface in setSearchSurface () and the spatial locator in
227  * setSearchMethod ()
228  * \param[out] output the resultant point cloud model dataset that contains the VFH feature estimates
229  */
230  void
231  computeFeature (PointCloudOut &output) override;
232 
233  protected:
234  /** \brief This method should get called before starting the actual computation. */
235  bool
236  initCompute () override;
237 
238  /** \brief Placeholder for the f1 histogram. */
239  std::array<Eigen::VectorXf, 4> hist_f_;
240  /** \brief Placeholder for the vp histogram. */
241  Eigen::VectorXf hist_vp_;
242 
243  /** \brief Normal to be used to computed VFH. Default, the average normal of the whole point cloud */
244  Eigen::Vector4f normal_to_use_;
245  /** \brief Centroid to be used to computed VFH. Default, the centroid of the whole point cloud */
246  Eigen::Vector4f centroid_to_use_;
247 
248  // VFH configuration parameters because CVFH instantiates it. See constructor for default values.
249 
250  /** \brief Use the normal_to_use_ */
252  /** \brief Use the centroid_to_use_ */
254  /** \brief Normalize bins by the number the total number of points. */
256  /** \brief Normalize the shape distribution component of VFH */
258  /** \brief Activate or deactivate the size component of VFH */
260 
261  private:
262  /** \brief Float constant = 1.0 / (2.0 * M_PI) */
263  float d_pi_;
264  };
265 }
266 
267 #ifdef PCL_NO_PRECOMPILE
268 #include <pcl/features/impl/vfh.hpp>
269 #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:240
int k_
The number of K nearest neighbors to use for each point.
Definition: feature.h:243
shared_ptr< Feature< PointInT, PointOutT > > Ptr
Definition: feature.h:114
std::string feature_name_
The feature name.
Definition: feature.h:223
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:255
void setFillSizeComponent(bool fill_size)
set size_component_
Definition: vfh.h:203
Eigen::Vector4f normal_to_use_
Normal to be used to computed VFH.
Definition: vfh.h:244
typename Feature< PointInT, PointOutT >::PointCloudOut PointCloudOut
Definition: vfh.h:84
bool use_given_normal_
Use the normal_to_use_.
Definition: vfh.h:251
VFHEstimation()
Empty constructor.
Definition: vfh.h:90
void setViewPoint(float vpx, float vpy, float vpz)
Set the viewpoint.
Definition: vfh.h:125
Eigen::VectorXf hist_vp_
Placeholder for the vp histogram.
Definition: vfh.h:241
void setNormalizeDistance(bool normalize)
set normalize_distances_
Definition: vfh.h:193
bool size_component_
Activate or deactivate the size component of VFH.
Definition: vfh.h:259
void setUseGivenNormal(bool use)
Set use_given_normal_.
Definition: vfh.h:145
void setCentroidToUse(const Eigen::Vector3f &centroid)
Set centroid_to_use_.
Definition: vfh.h:174
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:155
bool normalize_distances_
Normalize the shape distribution component of VFH.
Definition: vfh.h:257
Eigen::Vector4f centroid_to_use_
Centroid to be used to computed VFH.
Definition: vfh.h:246
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:183
bool use_given_centroid_
Use the centroid_to_use_.
Definition: vfh.h:253
std::array< Eigen::VectorXf, 4 > hist_f_
Placeholder for the f1 histogram.
Definition: vfh.h:239
void getViewPoint(float &vpx, float &vpy, float &vpz)
Get the viewpoint.
Definition: vfh.h:134
void setUseGivenCentroid(bool use)
Set use_given_centroid_.
Definition: vfh.h:164
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