Point Cloud Library (PCL)
1.7.0
|
00001 /* 00002 * Software License Agreement (BSD License) 00003 * 00004 * Point Cloud Library (PCL) - www.pointclouds.org 00005 * Copyright (c) 2010-2011, Willow Garage, Inc. 00006 * Copyright (c) 2012-, Open Perception, Inc. 00007 * 00008 * All rights reserved. 00009 * 00010 * Redistribution and use in source and binary forms, with or without 00011 * modification, are permitted provided that the following conditions 00012 * are met: 00013 * 00014 * * Redistributions of source code must retain the above copyright 00015 * notice, this list of conditions and the following disclaimer. 00016 * * Redistributions in binary form must reproduce the above 00017 * copyright notice, this list of conditions and the following 00018 * disclaimer in the documentation and/or other materials provided 00019 * with the distribution. 00020 * * Neither the name of the copyright holder(s) nor the names of its 00021 * contributors may be used to endorse or promote products derived 00022 * from this software without specific prior written permission. 00023 * 00024 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 00025 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 00026 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 00027 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 00028 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 00029 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 00030 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 00031 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 00032 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 00033 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 00034 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 00035 * POSSIBILITY OF SUCH DAMAGE. 00036 * 00037 * $Id$ 00038 * 00039 */ 00040 00041 #ifndef PCL_FEATURES_VFH_H_ 00042 #define PCL_FEATURES_VFH_H_ 00043 00044 #include <pcl/point_types.h> 00045 #include <pcl/features/feature.h> 00046 00047 namespace pcl 00048 { 00049 /** \brief VFHEstimation estimates the <b>Viewpoint Feature Histogram (VFH)</b> descriptor for a given point cloud 00050 * dataset containing points and normals. The default VFH implementation uses 45 binning subdivisions for each of 00051 * the three extended FPFH values, plus another 45 binning subdivisions for the distances between each point and 00052 * the centroid and 128 binning subdivisions for the viewpoint component, which results in a 00053 * 308-byte array of float values. These are stored in a pcl::VFHSignature308 point type. 00054 * A major difference between the PFH/FPFH descriptors and VFH, is that for a given point cloud dataset, only a 00055 * single VFH descriptor will be estimated (vfhs->points.size() should be 1), while the resultant PFH/FPFH data 00056 * will have the same number of entries as the number of points in the cloud. 00057 * 00058 * \note If you use this code in any academic work, please cite: 00059 * 00060 * - R.B. Rusu, G. Bradski, R. Thibaux, J. Hsu. 00061 * Fast 3D Recognition and Pose Using the Viewpoint Feature Histogram. 00062 * In Proceedings of International Conference on Intelligent Robots and Systems (IROS) 00063 * Taipei, Taiwan, October 18-22 2010. 00064 * 00065 * \note The code is stateful as we do not expect this class to be multicore parallelized. Please look at 00066 * \ref FPFHEstimationOMP for an example of a parallel implementation of the FPFH (Fast Point Feature Histogram). 00067 * \author Radu B. Rusu 00068 * \ingroup features 00069 */ 00070 template<typename PointInT, typename PointNT, typename PointOutT = pcl::VFHSignature308> 00071 class VFHEstimation : public FeatureFromNormals<PointInT, PointNT, PointOutT> 00072 { 00073 public: 00074 using Feature<PointInT, PointOutT>::feature_name_; 00075 using Feature<PointInT, PointOutT>::getClassName; 00076 using Feature<PointInT, PointOutT>::indices_; 00077 using Feature<PointInT, PointOutT>::k_; 00078 using Feature<PointInT, PointOutT>::search_radius_; 00079 using Feature<PointInT, PointOutT>::input_; 00080 using Feature<PointInT, PointOutT>::surface_; 00081 using FeatureFromNormals<PointInT, PointNT, PointOutT>::normals_; 00082 00083 typedef typename Feature<PointInT, PointOutT>::PointCloudOut PointCloudOut; 00084 typedef typename boost::shared_ptr<VFHEstimation<PointInT, PointNT, PointOutT> > Ptr; 00085 typedef typename boost::shared_ptr<const VFHEstimation<PointInT, PointNT, PointOutT> > ConstPtr; 00086 00087 00088 /** \brief Empty constructor. */ 00089 VFHEstimation () : 00090 nr_bins_f1_ (45), nr_bins_f2_ (45), nr_bins_f3_ (45), nr_bins_f4_ (45), nr_bins_vp_ (128), 00091 vpx_ (0), vpy_ (0), vpz_ (0), 00092 hist_f1_ (), hist_f2_ (), hist_f3_ (), hist_f4_ (), hist_vp_ (), 00093 normal_to_use_ (), centroid_to_use_ (), use_given_normal_ (false), use_given_centroid_ (false), 00094 normalize_bins_ (true), normalize_distances_ (false), size_component_ (false), 00095 d_pi_ (1.0f / (2.0f * static_cast<float> (M_PI))) 00096 { 00097 hist_f1_.setZero (nr_bins_f1_); 00098 hist_f2_.setZero (nr_bins_f2_); 00099 hist_f3_.setZero (nr_bins_f3_); 00100 hist_f4_.setZero (nr_bins_f4_); 00101 search_radius_ = 0; 00102 k_ = 0; 00103 feature_name_ = "VFHEstimation"; 00104 } 00105 00106 /** \brief Estimate the SPFH (Simple Point Feature Histograms) signatures of the angular 00107 * (f1, f2, f3) and distance (f4) features for a given point from its neighborhood 00108 * \param[in] centroid_p the centroid point 00109 * \param[in] centroid_n the centroid normal 00110 * \param[in] cloud the dataset containing the XYZ Cartesian coordinates of the two points 00111 * \param[in] normals the dataset containing the surface normals at each point in \a cloud 00112 * \param[in] indices the k-neighborhood point indices in the dataset 00113 */ 00114 void 00115 computePointSPFHSignature (const Eigen::Vector4f ¢roid_p, const Eigen::Vector4f ¢roid_n, 00116 const pcl::PointCloud<PointInT> &cloud, const pcl::PointCloud<PointNT> &normals, 00117 const std::vector<int> &indices); 00118 00119 /** \brief Set the viewpoint. 00120 * \param[in] vpx the X coordinate of the viewpoint 00121 * \param[in] vpy the Y coordinate of the viewpoint 00122 * \param[in] vpz the Z coordinate of the viewpoint 00123 */ 00124 inline void 00125 setViewPoint (float vpx, float vpy, float vpz) 00126 { 00127 vpx_ = vpx; 00128 vpy_ = vpy; 00129 vpz_ = vpz; 00130 } 00131 00132 /** \brief Get the viewpoint. */ 00133 inline void 00134 getViewPoint (float &vpx, float &vpy, float &vpz) 00135 { 00136 vpx = vpx_; 00137 vpy = vpy_; 00138 vpz = vpz_; 00139 } 00140 00141 /** \brief Set use_given_normal_ 00142 * \param[in] use Set to true if you want to use the normal passed to setNormalUse(normal) 00143 */ 00144 inline void 00145 setUseGivenNormal (bool use) 00146 { 00147 use_given_normal_ = use; 00148 } 00149 00150 /** \brief Set the normal to use 00151 * \param[in] normal Sets the normal to be used in the VFH computation. It is is used 00152 * to build the Darboux Coordinate system. 00153 */ 00154 inline void 00155 setNormalToUse (const Eigen::Vector3f &normal) 00156 { 00157 normal_to_use_ = Eigen::Vector4f (normal[0], normal[1], normal[2], 0); 00158 } 00159 00160 /** \brief Set use_given_centroid_ 00161 * \param[in] use Set to true if you want to use the centroid passed through setCentroidToUse(centroid) 00162 */ 00163 inline void 00164 setUseGivenCentroid (bool use) 00165 { 00166 use_given_centroid_ = use; 00167 } 00168 00169 /** \brief Set centroid_to_use_ 00170 * \param[in] centroid Centroid to be used in the VFH computation. It is used to compute the distances 00171 * from all points to this centroid. 00172 */ 00173 inline void 00174 setCentroidToUse (const Eigen::Vector3f ¢roid) 00175 { 00176 centroid_to_use_ = Eigen::Vector4f (centroid[0], centroid[1], centroid[2], 0); 00177 } 00178 00179 /** \brief set normalize_bins_ 00180 * \param[in] normalize If true, the VFH bins are normalized using the total number of points 00181 */ 00182 inline void 00183 setNormalizeBins (bool normalize) 00184 { 00185 normalize_bins_ = normalize; 00186 } 00187 00188 /** \brief set normalize_distances_ 00189 * \param[in] normalize If true, the 4th component of VFH (shape distribution component) get normalized 00190 * by the maximum size between the centroid and the point cloud 00191 */ 00192 inline void 00193 setNormalizeDistance (bool normalize) 00194 { 00195 normalize_distances_ = normalize; 00196 } 00197 00198 /** \brief set size_component_ 00199 * \param[in] fill_size True if the 4th component of VFH (shape distribution component) needs to be filled. 00200 * Otherwise, it is set to zero. 00201 */ 00202 inline void 00203 setFillSizeComponent (bool fill_size) 00204 { 00205 size_component_ = fill_size; 00206 } 00207 00208 /** \brief Overloaded computed method from pcl::Feature. 00209 * \param[out] output the resultant point cloud model dataset containing the estimated features 00210 */ 00211 void 00212 compute (PointCloudOut &output); 00213 00214 private: 00215 00216 /** \brief The number of subdivisions for each feature interval. */ 00217 int nr_bins_f1_, nr_bins_f2_, nr_bins_f3_, nr_bins_f4_, nr_bins_vp_; 00218 00219 /** \brief Values describing the viewpoint ("pinhole" camera model assumed). For per point viewpoints, inherit 00220 * from VFHEstimation and provide your own computeFeature (). By default, the viewpoint is set to 0,0,0. 00221 */ 00222 float vpx_, vpy_, vpz_; 00223 00224 /** \brief Estimate the Viewpoint Feature Histograms (VFH) descriptors at a set of points given by 00225 * <setInputCloud (), setIndices ()> using the surface in setSearchSurface () and the spatial locator in 00226 * setSearchMethod () 00227 * \param[out] output the resultant point cloud model dataset that contains the VFH feature estimates 00228 */ 00229 void 00230 computeFeature (PointCloudOut &output); 00231 00232 protected: 00233 /** \brief This method should get called before starting the actual computation. */ 00234 bool 00235 initCompute (); 00236 00237 /** \brief Placeholder for the f1 histogram. */ 00238 Eigen::VectorXf hist_f1_; 00239 /** \brief Placeholder for the f2 histogram. */ 00240 Eigen::VectorXf hist_f2_; 00241 /** \brief Placeholder for the f3 histogram. */ 00242 Eigen::VectorXf hist_f3_; 00243 /** \brief Placeholder for the f4 histogram. */ 00244 Eigen::VectorXf hist_f4_; 00245 /** \brief Placeholder for the vp histogram. */ 00246 Eigen::VectorXf hist_vp_; 00247 00248 /** \brief Normal to be used to computed VFH. Default, the average normal of the whole point cloud */ 00249 Eigen::Vector4f normal_to_use_; 00250 /** \brief Centroid to be used to computed VFH. Default, the centroid of the whole point cloud */ 00251 Eigen::Vector4f centroid_to_use_; 00252 00253 // VFH configuration parameters because CVFH instantiates it. See constructor for default values. 00254 00255 /** \brief Use the normal_to_use_ */ 00256 bool use_given_normal_; 00257 /** \brief Use the centroid_to_use_ */ 00258 bool use_given_centroid_; 00259 /** \brief Normalize bins by the number the total number of points. */ 00260 bool normalize_bins_; 00261 /** \brief Normalize the shape distribution component of VFH */ 00262 bool normalize_distances_; 00263 /** \brief Activate or deactivate the size component of VFH */ 00264 bool size_component_; 00265 00266 private: 00267 /** \brief Float constant = 1.0 / (2.0 * M_PI) */ 00268 float d_pi_; 00269 }; 00270 } 00271 00272 #ifdef PCL_NO_PRECOMPILE 00273 #include <pcl/features/impl/vfh.hpp> 00274 #endif 00275 00276 #endif //#ifndef PCL_FEATURES_VFH_H_