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