Point Cloud Library (PCL)  1.9.1
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 #ifndef PCL_FEATURES_VFH_H_
42 #define PCL_FEATURES_VFH_H_
43 
44 #include <pcl/point_types.h>
45 #include <pcl/features/feature.h>
46 
47 namespace pcl
48 {
49  /** \brief VFHEstimation estimates the <b>Viewpoint Feature Histogram (VFH)</b> descriptor for a given point cloud
50  * dataset containing points and normals. The default VFH implementation uses 45 binning subdivisions for each of
51  * the three extended FPFH values, plus another 45 binning subdivisions for the distances between each point and
52  * the centroid and 128 binning subdivisions for the viewpoint component, which results in a
53  * 308-byte array of float values. These are stored in a pcl::VFHSignature308 point type.
54  * A major difference between the PFH/FPFH descriptors and VFH, is that for a given point cloud dataset, only a
55  * single VFH descriptor will be estimated (vfhs->points.size() should be 1), while the resultant PFH/FPFH data
56  * will have the same number of entries as the number of points in the cloud.
57  *
58  * \note If you use this code in any academic work, please cite:
59  *
60  * - R.B. Rusu, G. Bradski, R. Thibaux, J. Hsu.
61  * Fast 3D Recognition and Pose Using the Viewpoint Feature Histogram.
62  * In Proceedings of International Conference on Intelligent Robots and Systems (IROS)
63  * Taipei, Taiwan, October 18-22 2010.
64  *
65  * \note The code is stateful as we do not expect this class to be multicore parallelized. Please look at
66  * \ref FPFHEstimationOMP for an example of a parallel implementation of the FPFH (Fast Point Feature Histogram).
67  * \author Radu B. Rusu
68  * \ingroup features
69  */
70  template<typename PointInT, typename PointNT, typename PointOutT = pcl::VFHSignature308>
71  class VFHEstimation : public FeatureFromNormals<PointInT, PointNT, PointOutT>
72  {
73  public:
82 
84  typedef typename boost::shared_ptr<VFHEstimation<PointInT, PointNT, PointOutT> > Ptr;
85  typedef typename boost::shared_ptr<const VFHEstimation<PointInT, PointNT, PointOutT> > ConstPtr;
86 
87 
88  /** \brief Empty constructor. */
90  nr_bins_f1_ (45), nr_bins_f2_ (45), nr_bins_f3_ (45), nr_bins_f4_ (45), nr_bins_vp_ (128),
91  vpx_ (0), vpy_ (0), vpz_ (0),
92  hist_f1_ (), hist_f2_ (), hist_f3_ (), hist_f4_ (), hist_vp_ (),
94  normalize_bins_ (true), normalize_distances_ (false), size_component_ (false),
95  d_pi_ (1.0f / (2.0f * static_cast<float> (M_PI)))
96  {
97  hist_f1_.setZero (nr_bins_f1_);
98  hist_f2_.setZero (nr_bins_f2_);
99  hist_f3_.setZero (nr_bins_f3_);
100  hist_f4_.setZero (nr_bins_f4_);
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 std::vector<int> &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  int nr_bins_f1_, nr_bins_f2_, nr_bins_f3_, nr_bins_f4_, nr_bins_vp_;
218 
219  /** \brief Values describing the viewpoint ("pinhole" camera model assumed). For per point viewpoints, inherit
220  * from VFHEstimation and provide your own computeFeature (). By default, the viewpoint is set to 0,0,0.
221  */
222  float vpx_, vpy_, vpz_;
223 
224  /** \brief Estimate the Viewpoint Feature Histograms (VFH) descriptors at a set of points given by
225  * <setInputCloud (), setIndices ()> using the surface in setSearchSurface () and the spatial locator in
226  * setSearchMethod ()
227  * \param[out] output the resultant point cloud model dataset that contains the VFH feature estimates
228  */
229  void
230  computeFeature (PointCloudOut &output);
231 
232  protected:
233  /** \brief This method should get called before starting the actual computation. */
234  bool
235  initCompute ();
236 
237  /** \brief Placeholder for the f1 histogram. */
238  Eigen::VectorXf hist_f1_;
239  /** \brief Placeholder for the f2 histogram. */
240  Eigen::VectorXf hist_f2_;
241  /** \brief Placeholder for the f3 histogram. */
242  Eigen::VectorXf hist_f3_;
243  /** \brief Placeholder for the f4 histogram. */
244  Eigen::VectorXf hist_f4_;
245  /** \brief Placeholder for the vp histogram. */
246  Eigen::VectorXf hist_vp_;
247 
248  /** \brief Normal to be used to computed VFH. Default, the average normal of the whole point cloud */
249  Eigen::Vector4f normal_to_use_;
250  /** \brief Centroid to be used to computed VFH. Default, the centroid of the whole point cloud */
251  Eigen::Vector4f centroid_to_use_;
252 
253  // VFH configuration parameters because CVFH instantiates it. See constructor for default values.
254 
255  /** \brief Use the normal_to_use_ */
257  /** \brief Use the centroid_to_use_ */
259  /** \brief Normalize bins by the number the total number of points. */
261  /** \brief Normalize the shape distribution component of VFH */
263  /** \brief Activate or deactivate the size component of VFH */
265 
266  private:
267  /** \brief Float constant = 1.0 / (2.0 * M_PI) */
268  float d_pi_;
269  };
270 }
271 
272 #ifdef PCL_NO_PRECOMPILE
273 #include <pcl/features/impl/vfh.hpp>
274 #endif
275 
276 #endif //#ifndef PCL_FEATURES_VFH_H_
void getViewPoint(float &vpx, float &vpy, float &vpz)
Get the viewpoint.
Definition: vfh.h:134
Eigen::VectorXf hist_f1_
Placeholder for the f1 histogram.
Definition: vfh.h:238
void setUseGivenCentroid(bool use)
Set use_given_centroid_.
Definition: vfh.h:164
Eigen::VectorXf hist_f4_
Placeholder for the f4 histogram.
Definition: vfh.h:244
Eigen::Vector4f normal_to_use_
Normal to be used to computed VFH.
Definition: vfh.h:249
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
Eigen::VectorXf hist_f2_
Placeholder for the f2 histogram.
Definition: vfh.h:240
Eigen::VectorXf hist_vp_
Placeholder for the vp histogram.
Definition: vfh.h:246
Eigen::VectorXf hist_f3_
Placeholder for the f3 histogram.
Definition: vfh.h:242
boost::shared_ptr< VFHEstimation< PointInT, PointNT, PointOutT > > Ptr
Definition: vfh.h:84
void setUseGivenNormal(bool use)
Set use_given_normal_.
Definition: vfh.h:145
boost::shared_ptr< const VFHEstimation< PointInT, PointNT, PointOutT > > ConstPtr
Definition: vfh.h:85
Eigen::Vector4f centroid_to_use_
Centroid to be used to computed VFH.
Definition: vfh.h:251
VFHEstimation()
Empty constructor.
Definition: vfh.h:89
bool normalize_distances_
Normalize the shape distribution component of VFH.
Definition: vfh.h:262
void setNormalToUse(const Eigen::Vector3f &normal)
Set the normal to use.
Definition: vfh.h:155
void setCentroidToUse(const Eigen::Vector3f &centroid)
Set centroid_to_use_.
Definition: vfh.h:174
Defines all the PCL implemented PointT point type structures.
void computePointSPFHSignature(const Eigen::Vector4f &centroid_p, const Eigen::Vector4f &centroid_n, const pcl::PointCloud< PointInT > &cloud, const pcl::PointCloud< PointNT > &normals, const std::vector< int > &indices)
Estimate the SPFH (Simple Point Feature Histograms) signatures of the angular (f1, f2, f3) and distance (f4) features for a given point from its neighborhood.
Definition: vfh.hpp:92
bool normalize_bins_
Normalize bins by the number the total number of points.
Definition: vfh.h:260
VFHEstimation estimates the Viewpoint Feature Histogram (VFH) descriptor for a given point cloud data...
Definition: vfh.h:71
void setNormalizeBins(bool normalize)
set normalize_bins_
Definition: vfh.h:183
PointCloud represents the base class in PCL for storing collections of 3D points. ...
void setViewPoint(float vpx, float vpy, float vpz)
Set the viewpoint.
Definition: vfh.h:125
void compute(PointCloudOut &output)
Overloaded computed method from pcl::Feature.
Definition: vfh.hpp:65
Feature< PointInT, PointOutT >::PointCloudOut PointCloudOut
Definition: vfh.h:83
Feature represents the base feature class.
Definition: feature.h:105
bool use_given_normal_
Use the normal_to_use_.
Definition: vfh.h:256
bool size_component_
Activate or deactivate the size component of VFH.
Definition: vfh.h:264
void setFillSizeComponent(bool fill_size)
set size_component_
Definition: vfh.h:203
bool initCompute()
This method should get called before starting the actual computation.
Definition: vfh.hpp:51
double search_radius_
The nearest neighbors search radius for each point.
Definition: feature.h:239
bool use_given_centroid_
Use the centroid_to_use_.
Definition: vfh.h:258
void setNormalizeDistance(bool normalize)
set normalize_distances_
Definition: vfh.h:193