Point Cloud Library (PCL)
1.3.1
|
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