vfh.hpp
Go to the documentation of this file.
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: vfh.hpp 6089 2012-07-02 18:38:08Z rusu $
00038  *
00039  */
00040 
00041 #ifndef PCL_FEATURES_IMPL_VFH_H_
00042 #define PCL_FEATURES_IMPL_VFH_H_
00043 
00044 #include <pcl/features/vfh.h>
00045 #include <pcl/features/pfh.h>
00046 #include <pcl/common/common.h>
00047 
00049 template<typename PointInT, typename PointNT, typename PointOutT> bool
00050 pcl::VFHEstimation<PointInT, PointNT, PointOutT>::initCompute ()
00051 {
00052   if (input_->points.size () < 2 || (surface_ && surface_->points.size () < 2))
00053   {
00054     PCL_ERROR ("[pcl::VFHEstimation::initCompute] Input dataset must have at least 2 points!\n");
00055     return (false);
00056   }
00057   if (search_radius_ == 0 && k_ == 0)
00058     k_ = 1;
00059   return (Feature<PointInT, PointOutT>::initCompute ());
00060 }
00061 
00063 template<typename PointInT, typename PointNT, typename PointOutT> void
00064 pcl::VFHEstimation<PointInT, PointNT, PointOutT>::compute (PointCloudOut &output)
00065 {
00066   if (!initCompute ())
00067   {
00068     output.width = output.height = 0;
00069     output.points.clear ();
00070     return;
00071   }
00072   // Copy the header
00073   output.header = input_->header;
00074 
00075   // Resize the output dataset
00076   // Important! We should only allocate precisely how many elements we will need, otherwise
00077   // we risk at pre-allocating too much memory which could lead to bad_alloc 
00078   // (see http://dev.pointclouds.org/issues/657)
00079   output.width = output.height = 1;
00080   output.is_dense = input_->is_dense;
00081   output.points.resize (1);
00082 
00083   // Perform the actual feature computation
00084   computeFeature (output);
00085 
00086   Feature<PointInT, PointOutT>::deinitCompute ();
00087 }
00088 
00090 template<typename PointInT, typename PointNT, typename PointOutT> void
00091 pcl::VFHEstimation<PointInT, PointNT, PointOutT>::computePointSPFHSignature (const Eigen::Vector4f &centroid_p,
00092                                                                              const Eigen::Vector4f &centroid_n,
00093                                                                              const pcl::PointCloud<PointInT> &cloud,
00094                                                                              const pcl::PointCloud<PointNT> &normals,
00095                                                                              const std::vector<int> &indices)
00096 {
00097   Eigen::Vector4f pfh_tuple;
00098   // Reset the whole thing
00099   hist_f1_.setZero (nr_bins_f1_);
00100   hist_f2_.setZero (nr_bins_f2_);
00101   hist_f3_.setZero (nr_bins_f3_);
00102   hist_f4_.setZero (nr_bins_f4_);
00103 
00104   // Get the bounding box of the current cluster
00105   //Eigen::Vector4f min_pt, max_pt;
00106   //pcl::getMinMax3D (cloud, indices, min_pt, max_pt);
00107   //double distance_normalization_factor = (std::max)((centroid_p - min_pt).norm (), (centroid_p - max_pt).norm ());
00108 
00109   //Instead of using the bounding box to normalize the VFH distance component, it is better to use the max_distance
00110   //from any point to centroid. VFH is invariant to rotation about the roll axis but the bounding box is not,
00111   //resulting in different normalization factors for point clouds that are just rotated about that axis.
00112 
00113   double distance_normalization_factor = 1.0;
00114   if (normalize_distances_) 
00115   {
00116     Eigen::Vector4f max_pt;
00117     pcl::getMaxDistance (cloud, indices, centroid_p, max_pt);
00118     max_pt[3] = 0;
00119     distance_normalization_factor = (centroid_p - max_pt).norm ();
00120   }
00121 
00122   // Factorization constant
00123   float hist_incr;
00124   if (normalize_bins_)
00125     hist_incr = 100.0f / static_cast<float> (indices.size () - 1);
00126   else
00127     hist_incr = 1.0f;
00128 
00129   float hist_incr_size_component;
00130   if (size_component_)
00131     hist_incr_size_component = hist_incr;
00132   else
00133     hist_incr_size_component = 0.0;
00134 
00135   // Iterate over all the points in the neighborhood
00136   for (size_t idx = 0; idx < indices.size (); ++idx)
00137   {
00138     // Compute the pair P to NNi
00139     if (!computePairFeatures (centroid_p, centroid_n, cloud.points[indices[idx]].getVector4fMap (),
00140                               normals.points[indices[idx]].getNormalVector4fMap (), pfh_tuple[0], pfh_tuple[1],
00141                               pfh_tuple[2], pfh_tuple[3]))
00142       continue;
00143 
00144     // Normalize the f1, f2, f3, f4 features and push them in the histogram
00145     int h_index = static_cast<int> (floor (nr_bins_f1_ * ((pfh_tuple[0] + M_PI) * d_pi_)));
00146     if (h_index < 0)
00147       h_index = 0;
00148     if (h_index >= nr_bins_f1_)
00149       h_index = nr_bins_f1_ - 1;
00150     hist_f1_ (h_index) += hist_incr;
00151 
00152     h_index = static_cast<int> (floor (nr_bins_f2_ * ((pfh_tuple[1] + 1.0) * 0.5)));
00153     if (h_index < 0)
00154       h_index = 0;
00155     if (h_index >= nr_bins_f2_)
00156       h_index = nr_bins_f2_ - 1;
00157     hist_f2_ (h_index) += hist_incr;
00158 
00159     h_index = static_cast<int> (floor (nr_bins_f3_ * ((pfh_tuple[2] + 1.0) * 0.5)));
00160     if (h_index < 0)
00161       h_index = 0;
00162     if (h_index >= nr_bins_f3_)
00163       h_index = nr_bins_f3_ - 1;
00164     hist_f3_ (h_index) += hist_incr;
00165 
00166     if (normalize_distances_)
00167       h_index = static_cast<int> (floor (nr_bins_f4_ * (pfh_tuple[3] / distance_normalization_factor)));
00168     else
00169       h_index = static_cast<int> (pcl_round (pfh_tuple[3] * 100));
00170 
00171     if (h_index < 0)
00172       h_index = 0;
00173     if (h_index >= nr_bins_f4_)
00174       h_index = nr_bins_f4_ - 1;
00175 
00176     hist_f4_ (h_index) += hist_incr_size_component;
00177   }
00178 }
00180 template <typename PointInT, typename PointNT, typename PointOutT> void
00181 pcl::VFHEstimation<PointInT, PointNT, PointOutT>::computeFeature (PointCloudOut &output)
00182 {
00183   // ---[ Step 1a : compute the centroid in XYZ space
00184   Eigen::Vector4f xyz_centroid;
00185 
00186   if (use_given_centroid_) 
00187     xyz_centroid = centroid_to_use_;
00188   else
00189     compute3DCentroid (*surface_, *indices_, xyz_centroid);          // Estimate the XYZ centroid
00190 
00191   // ---[ Step 1b : compute the centroid in normal space
00192   Eigen::Vector4f normal_centroid = Eigen::Vector4f::Zero ();
00193   int cp = 0;
00194 
00195   // If the data is dense, we don't need to check for NaN
00196   if (use_given_normal_)
00197     normal_centroid = normal_to_use_;
00198   else
00199   {
00200     if (normals_->is_dense)
00201     {
00202       for (size_t i = 0; i < indices_->size (); ++i)
00203       {
00204         normal_centroid += normals_->points[(*indices_)[i]].getNormalVector4fMap ();
00205         cp++;
00206       }
00207     }
00208     // NaN or Inf values could exist => check for them
00209     else
00210     {
00211       for (size_t i = 0; i < indices_->size (); ++i)
00212       {
00213         if (!pcl_isfinite (normals_->points[(*indices_)[i]].normal[0])
00214             ||
00215             !pcl_isfinite (normals_->points[(*indices_)[i]].normal[1])
00216             ||
00217             !pcl_isfinite (normals_->points[(*indices_)[i]].normal[2]))
00218           continue;
00219         normal_centroid += normals_->points[(*indices_)[i]].getNormalVector4fMap ();
00220         cp++;
00221       }
00222     }
00223     normal_centroid /= static_cast<float> (cp);
00224   }
00225 
00226   // Compute the direction of view from the viewpoint to the centroid
00227   Eigen::Vector4f viewpoint (vpx_, vpy_, vpz_, 0);
00228   Eigen::Vector4f d_vp_p = viewpoint - xyz_centroid;
00229   d_vp_p.normalize ();
00230 
00231   // Estimate the SPFH at nn_indices[0] using the entire cloud
00232   computePointSPFHSignature (xyz_centroid, normal_centroid, *surface_, *normals_, *indices_);
00233 
00234   // We only output _1_ signature
00235   output.points.resize (1);
00236   output.width = 1;
00237   output.height = 1;
00238 
00239   // Estimate the FPFH at nn_indices[0] using the entire cloud and copy the resultant signature
00240   for (int d = 0; d < hist_f1_.size (); ++d)
00241     output.points[0].histogram[d + 0] = hist_f1_[d];
00242 
00243   size_t data_size = hist_f1_.size ();
00244   for (int d = 0; d < hist_f2_.size (); ++d)
00245     output.points[0].histogram[d + data_size] = hist_f2_[d];
00246 
00247   data_size += hist_f2_.size ();
00248   for (int d = 0; d < hist_f3_.size (); ++d)
00249     output.points[0].histogram[d + data_size] = hist_f3_[d];
00250 
00251   data_size += hist_f3_.size ();
00252   for (int d = 0; d < hist_f4_.size (); ++d)
00253     output.points[0].histogram[d + data_size] = hist_f4_[d];
00254 
00255   // ---[ Step 2 : obtain the viewpoint component
00256   hist_vp_.setZero (nr_bins_vp_);
00257 
00258   double hist_incr;
00259   if (normalize_bins_)
00260     hist_incr = 100.0 / static_cast<double> (indices_->size ());
00261   else
00262     hist_incr = 1.0;
00263 
00264   for (size_t i = 0; i < indices_->size (); ++i)
00265   {
00266     Eigen::Vector4f normal (normals_->points[(*indices_)[i]].normal[0],
00267                             normals_->points[(*indices_)[i]].normal[1],
00268                             normals_->points[(*indices_)[i]].normal[2], 0);
00269     // Normalize
00270     double alpha = (normal.dot (d_vp_p) + 1.0) * 0.5;
00271     int fi = static_cast<int> (floor (alpha * static_cast<double> (hist_vp_.size ())));
00272     if (fi < 0)
00273       fi = 0;
00274     if (fi > (static_cast<int> (hist_vp_.size ()) - 1))
00275       fi = static_cast<int> (hist_vp_.size ()) - 1;
00276     // Bin into the histogram
00277     hist_vp_ [fi] += static_cast<float> (hist_incr);
00278   }
00279   data_size += hist_f4_.size ();
00280   // Copy the resultant signature
00281   for (int d = 0; d < hist_vp_.size (); ++d)
00282     output.points[0].histogram[d + data_size] = hist_vp_[d];
00283 }
00284 
00285 #define PCL_INSTANTIATE_VFHEstimation(T,NT,OutT) template class PCL_EXPORTS pcl::VFHEstimation<T,NT,OutT>;
00286 
00287 #endif    // PCL_FEATURES_IMPL_VFH_H_


pcl
Author(s): Open Perception
autogenerated on Mon Oct 6 2014 03:18:55