Point Cloud Library (PCL)  1.3.1
vfh.hpp
Go to the documentation of this file.
00001 /*
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2009, Willow Garage, Inc.
00005  *  All rights reserved.
00006  *
00007  *  Redistribution and use in source and binary forms, with or without
00008  *  modification, are permitted provided that the following conditions
00009  *  are met:
00010  *
00011  *   * Redistributions of source code must retain the above copyright
00012  *     notice, this list of conditions and the following disclaimer.
00013  *   * Redistributions in binary form must reproduce the above
00014  *     copyright notice, this list of conditions and the following
00015  *     disclaimer in the documentation and/or other materials provided
00016  *     with the distribution.
00017  *   * Neither the name of Willow Garage, Inc. nor the names of its
00018  *     contributors may be used to endorse or promote products derived
00019  *     from this software without specific prior written permission.
00020  *
00021  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032  *  POSSIBILITY OF SUCH DAMAGE.
00033  *
00034  * $Id: vfh.hpp 3022 2011-11-01 03:42:22Z rusu $
00035  *
00036  */
00037 
00038 #ifndef PCL_FEATURES_IMPL_VFH_H_
00039 #define PCL_FEATURES_IMPL_VFH_H_
00040 
00041 #include "pcl/features/vfh.h"
00042 #include "pcl/features/pfh.h"
00043 #include <pcl/common/common.h>
00044 
00046 template<typename PointInT, typename PointNT, typename PointOutT> void
00047 pcl::VFHEstimation<PointInT, PointNT, PointOutT>::computePointSPFHSignature (const Eigen::Vector4f &centroid_p,
00048                                                                              const Eigen::Vector4f &centroid_n,
00049                                                                              const pcl::PointCloud<PointInT> &cloud,
00050                                                                              const pcl::PointCloud<PointNT> &normals,
00051                                                                              const std::vector<int> &indices)
00052 {
00053   Eigen::Vector4f pfh_tuple;
00054   // Reset the whole thing
00055   hist_f1_.setZero (nr_bins_f1_);
00056   hist_f2_.setZero (nr_bins_f2_);
00057   hist_f3_.setZero (nr_bins_f3_);
00058   hist_f4_.setZero (nr_bins_f4_);
00059 
00060   // Get the bounding box of the current cluster
00061   //Eigen::Vector4f min_pt, max_pt;
00062   //pcl::getMinMax3D (cloud, indices, min_pt, max_pt);
00063   //double distance_normalization_factor = (std::max)((centroid_p - min_pt).norm (), (centroid_p - max_pt).norm ());
00064 
00065   //Instead of using the bounding box to normalize the VFH distance component, it is better to use the max_distance
00066   //from any point to centroid. VFH is invariant to rotation about the roll axis but the bounding box is not,
00067   //resulting in different normalization factors for point clouds that are just rotated about that axis.
00068 
00069   double distance_normalization_factor = 1.0;
00070   if ( normalize_distances_ ) {
00071     Eigen::Vector4f max_pt;
00072     pcl::getMaxDistance (cloud, indices, centroid_p, max_pt);
00073     max_pt[3] = 0;
00074     distance_normalization_factor = (centroid_p - max_pt).norm ();
00075   }
00076 
00077   // Factorization constant
00078   float hist_incr;
00079   if (normalize_bins_) {
00080     hist_incr = 100.0 / (float)(indices.size () - 1);
00081   } else {
00082     hist_incr = 1.0;
00083   }
00084 
00085   float hist_incr_size_component;
00086   if (size_component_)
00087     hist_incr_size_component = hist_incr;
00088   else
00089     hist_incr_size_component = 0.0;
00090 
00091   // Iterate over all the points in the neighborhood
00092   for (size_t idx = 0; idx < indices.size (); ++idx)
00093   {
00094     // Compute the pair P to NNi
00095     if (!computePairFeatures (centroid_p, centroid_n, cloud.points[indices[idx]].getVector4fMap (),
00096                               normals.points[indices[idx]].getNormalVector4fMap (), pfh_tuple[0], pfh_tuple[1],
00097                               pfh_tuple[2], pfh_tuple[3]))
00098       continue;
00099 
00100     // Normalize the f1, f2, f3, f4 features and push them in the histogram
00101     int h_index = floor (nr_bins_f1_ * ((pfh_tuple[0] + M_PI) * d_pi_));
00102     if (h_index < 0)
00103       h_index = 0;
00104     if (h_index >= nr_bins_f1_)
00105       h_index = nr_bins_f1_ - 1;
00106     hist_f1_ (h_index) += hist_incr;
00107 
00108     h_index = floor (nr_bins_f2_ * ((pfh_tuple[1] + 1.0) * 0.5));
00109     if (h_index < 0)
00110       h_index = 0;
00111     if (h_index >= nr_bins_f2_)
00112       h_index = nr_bins_f2_ - 1;
00113     hist_f2_ (h_index) += hist_incr;
00114 
00115     h_index = floor (nr_bins_f3_ * ((pfh_tuple[2] + 1.0) * 0.5));
00116     if (h_index < 0)
00117       h_index = 0;
00118     if (h_index >= nr_bins_f3_)
00119       h_index = nr_bins_f3_ - 1;
00120     hist_f3_ (h_index) += hist_incr;
00121 
00122     if (normalize_distances_)
00123       h_index = floor (nr_bins_f4_ * (pfh_tuple[3] / distance_normalization_factor));
00124     else
00125       h_index = pcl_round (pfh_tuple[3] * 100);
00126 
00127     if (h_index < 0)
00128       h_index = 0;
00129     if (h_index >= nr_bins_f4_)
00130       h_index = nr_bins_f4_ - 1;
00131 
00132     hist_f4_ (h_index) += hist_incr_size_component;
00133   }
00134 }
00136 template <typename PointInT, typename PointNT, typename PointOutT> void
00137 pcl::VFHEstimation<PointInT, PointNT, PointOutT>::computeFeature (PointCloudOut &output)
00138 {
00139   // ---[ Step 1a : compute the centroid in XYZ space
00140   Eigen::Vector4f xyz_centroid;
00141 
00142   if (use_given_centroid_) 
00143     xyz_centroid = centroid_to_use_;
00144   else
00145     compute3DCentroid (*surface_, *indices_, xyz_centroid);          // Estimate the XYZ centroid
00146 
00147   // ---[ Step 1b : compute the centroid in normal space
00148   Eigen::Vector4f normal_centroid = Eigen::Vector4f::Zero ();
00149   int cp = 0;
00150 
00151   // If the data is dense, we don't need to check for NaN
00152 
00153   if (use_given_normal_)
00154     normal_centroid = normal_to_use_;
00155   else
00156   {
00157     if (normals_->is_dense)
00158     {
00159       for (size_t i = 0; i < indices_->size (); ++i)
00160       {
00161         normal_centroid += normals_->points[(*indices_)[i]].getNormalVector4fMap ();
00162         cp++;
00163       }
00164     }
00165     // NaN or Inf values could exist => check for them
00166     else
00167     {
00168       for (size_t i = 0; i < indices_->size (); ++i)
00169       {
00170         if (!pcl_isfinite (normals_->points[(*indices_)[i]].normal[0])
00171             ||
00172             !pcl_isfinite (normals_->points[(*indices_)[i]].normal[1])
00173             ||
00174             !pcl_isfinite (normals_->points[(*indices_)[i]].normal[2]))
00175           continue;
00176         normal_centroid += normals_->points[(*indices_)[i]].getNormalVector4fMap ();
00177         cp++;
00178       }
00179     }
00180     normal_centroid /= cp;
00181   }
00182 
00183   // Compute the direction of view from the viewpoint to the centroid
00184   Eigen::Vector4f viewpoint (vpx_, vpy_, vpz_, 0);
00185   Eigen::Vector4f d_vp_p = viewpoint - xyz_centroid;
00186   d_vp_p.normalize ();
00187 
00188   // Estimate the SPFH at nn_indices[0] using the entire cloud
00189   computePointSPFHSignature (xyz_centroid, normal_centroid, *surface_, *normals_, *indices_);
00190 
00191   // We only output _1_ signature
00192   output.points.resize (1);
00193   output.width = 1;
00194   output.height = 1;
00195 
00196   // Estimate the FPFH at nn_indices[0] using the entire cloud and copy the resultant signature
00197   for (int d = 0; d < hist_f1_.size (); ++d)
00198     output.points[0].histogram[d + 0] = hist_f1_[d];
00199 
00200   size_t data_size = hist_f1_.size ();
00201   for (int d = 0; d < hist_f2_.size (); ++d)
00202     output.points[0].histogram[d + data_size] = hist_f2_[d];
00203 
00204   data_size += hist_f2_.size ();
00205   for (int d = 0; d < hist_f3_.size (); ++d)
00206     output.points[0].histogram[d + data_size] = hist_f3_[d];
00207 
00208   data_size += hist_f3_.size ();
00209   for (int d = 0; d < hist_f4_.size (); ++d)
00210     output.points[0].histogram[d + data_size] = hist_f4_[d];
00211 
00212   // ---[ Step 2 : obtain the viewpoint component
00213   hist_vp_.setZero (nr_bins_vp_);
00214 
00215   double hist_incr;
00216   if (normalize_bins_)
00217     hist_incr = 100.0 / (double)(indices_->size ());
00218   else
00219     hist_incr = 1.0;
00220 
00221   for (size_t i = 0; i < indices_->size (); ++i)
00222   {
00223     Eigen::Vector4f normal (normals_->points[(*indices_)[i]].normal[0],
00224                             normals_->points[(*indices_)[i]].normal[1],
00225                             normals_->points[(*indices_)[i]].normal[2], 0);
00226     // Normalize
00227     double alpha = (normal.dot (d_vp_p) + 1.0) * 0.5;
00228     int fi = floor (alpha * hist_vp_.size ());
00229     if (fi < 0)
00230       fi = 0;
00231     if (fi > ((int)hist_vp_.size () - 1))
00232       fi = hist_vp_.size () - 1;
00233     // Bin into the histogram
00234     hist_vp_ [fi] += hist_incr;
00235   }
00236   data_size += hist_f4_.size ();
00237   // Copy the resultant signature
00238   for (int d = 0; d < hist_vp_.size (); ++d)
00239     output.points[0].histogram[d + data_size] = hist_vp_[d];
00240 }
00241 
00242 #define PCL_INSTANTIATE_VFHEstimation(T,NT,OutT) template class PCL_EXPORTS pcl::VFHEstimation<T,NT,OutT>;
00243 
00244 #endif    // PCL_FEATURES_IMPL_VFH_H_
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Defines