Point Cloud Library (PCL)  1.3.1
pfh.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: pfh.hpp 1778 2011-07-15 00:02:55Z rusu $
00035  *
00036  */
00037 
00038 #ifndef PCL_FEATURES_IMPL_PFH_H_
00039 #define PCL_FEATURES_IMPL_PFH_H_
00040 
00041 #include "pcl/features/pfh.h"
00042 
00044 template <typename PointInT, typename PointNT, typename PointOutT> bool
00045 pcl::PFHEstimation<PointInT, PointNT, PointOutT>::computePairFeatures (
00046       const pcl::PointCloud<PointInT> &cloud, const pcl::PointCloud<PointNT> &normals,
00047       int p_idx, int q_idx, float &f1, float &f2, float &f3, float &f4)
00048 {
00049   pcl::computePairFeatures (cloud.points[p_idx].getVector4fMap (), normals.points[p_idx].getNormalVector4fMap (),
00050                             cloud.points[q_idx].getVector4fMap (), normals.points[q_idx].getNormalVector4fMap (),
00051                             f1, f2, f3, f4);
00052   return (true);
00053 }
00054 
00056 template <typename PointInT, typename PointNT, typename PointOutT> void
00057 pcl::PFHEstimation<PointInT, PointNT, PointOutT>::computePointPFHSignature (
00058       const pcl::PointCloud<PointInT> &cloud, const pcl::PointCloud<PointNT> &normals,
00059       const std::vector<int> &indices, int nr_split, Eigen::VectorXf &pfh_histogram)
00060 {
00061   int h_index, h_p;
00062 
00063   // Clear the resultant point histogram
00064   pfh_histogram.setZero ();
00065 
00066   // Factorization constant
00067   float hist_incr = 100.0 / (indices.size () * indices.size () - 1);
00068 
00069   // Iterate over all the points in the neighborhood
00070   for (size_t i_idx = 0; i_idx < indices.size (); ++i_idx)
00071   {
00072     for (size_t j_idx = 0; j_idx < indices.size (); ++j_idx)
00073     {
00074       // Avoid unnecessary returns
00075       if (i_idx == j_idx)
00076         continue;
00077 
00078       // Compute the pair NNi to NNj
00079       if (!computePairFeatures (cloud, normals, indices[i_idx], indices[j_idx],
00080                                 pfh_tuple_[0], pfh_tuple_[1], pfh_tuple_[2], pfh_tuple_[3]))
00081         continue;
00082 
00083       // Normalize the f1, f2, f3 features and push them in the histogram
00084       f_index_[0] = floor (nr_split * ((pfh_tuple_[0] + M_PI) * d_pi_));
00085       if (f_index_[0] < 0)         f_index_[0] = 0;
00086       if (f_index_[0] >= nr_split) f_index_[0] = nr_split - 1;
00087 
00088       f_index_[1] = floor (nr_split * ((pfh_tuple_[1] + 1.0) * 0.5));
00089       if (f_index_[1] < 0)         f_index_[1] = 0;
00090       if (f_index_[1] >= nr_split) f_index_[1] = nr_split - 1;
00091 
00092       f_index_[2] = floor (nr_split * ((pfh_tuple_[2] + 1.0) * 0.5));
00093       if (f_index_[2] < 0)         f_index_[2] = 0;
00094       if (f_index_[2] >= nr_split) f_index_[2] = nr_split - 1;
00095 
00096       // Copy into the histogram
00097       h_index = 0;
00098       h_p     = 1;
00099       for (int d = 0; d < 3; ++d)
00100       {
00101         h_index += h_p * f_index_[d];
00102         h_p     *= nr_split;
00103       }
00104       pfh_histogram[h_index] += hist_incr;
00105     }
00106   }
00107 }
00108 
00110 template <typename PointInT, typename PointNT, typename PointOutT> void
00111 pcl::PFHEstimation<PointInT, PointNT, PointOutT>::computeFeature (PointCloudOut &output)
00112 {
00113   pfh_histogram_.setZero (nr_subdiv_ * nr_subdiv_ * nr_subdiv_);
00114 
00115   // Allocate enough space to hold the results
00116   // \note This resize is irrelevant for a radiusSearch ().
00117   std::vector<int> nn_indices (k_);
00118   std::vector<float> nn_dists (k_);
00119 
00120   // Iterating over the entire index vector
00121   for (size_t idx = 0; idx < indices_->size (); ++idx)
00122   {
00123     this->searchForNeighbors ((*indices_)[idx], search_parameter_, nn_indices, nn_dists);
00124 
00125     // Estimate the PFH signature at each patch
00126     computePointPFHSignature (*surface_, *normals_, nn_indices, nr_subdiv_, pfh_histogram_);
00127 
00128     // Copy into the resultant cloud
00129     for (int d = 0; d < pfh_histogram_.size (); ++d)
00130       output.points[idx].histogram[d] = pfh_histogram_[d];
00131   }
00132 }
00133 
00134 #define PCL_INSTANTIATE_PFHEstimation(T,NT,OutT) template class PCL_EXPORTS pcl::PFHEstimation<T,NT,OutT>;
00135 
00136 #endif    // PCL_FEATURES_IMPL_PFH_H_ 
00137 
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Defines