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: fpfh.hpp 2713 2011-10-11 21:06:28Z rusu $ 00035 * 00036 */ 00037 00038 #ifndef PCL_FEATURES_IMPL_FPFH_H_ 00039 #define PCL_FEATURES_IMPL_FPFH_H_ 00040 00041 #include "pcl/features/fpfh.h" 00042 #include <pcl/features/pfh.h> 00043 00045 template <typename PointInT, typename PointNT, typename PointOutT> bool 00046 pcl::FPFHEstimation<PointInT, PointNT, PointOutT>::computePairFeatures ( 00047 const pcl::PointCloud<PointInT> &cloud, const pcl::PointCloud<PointNT> &normals, 00048 int p_idx, int q_idx, float &f1, float &f2, float &f3, float &f4) 00049 { 00050 pcl::computePairFeatures (cloud.points[p_idx].getVector4fMap (), normals.points[p_idx].getNormalVector4fMap (), 00051 cloud.points[q_idx].getVector4fMap (), normals.points[q_idx].getNormalVector4fMap (), 00052 f1, f2, f3, f4); 00053 return (true); 00054 } 00055 00057 template <typename PointInT, typename PointNT, typename PointOutT> 00058 void pcl::FPFHEstimation<PointInT, PointNT, PointOutT>:: 00059 computePointSPFHSignature (const pcl::PointCloud<PointInT> &cloud, const pcl::PointCloud<PointNT> &normals, 00060 int p_idx, int row, const std::vector<int> &indices, 00061 Eigen::MatrixXf &hist_f1, Eigen::MatrixXf &hist_f2, Eigen::MatrixXf &hist_f3) 00062 { 00063 Eigen::Vector4f pfh_tuple; 00064 // Get the number of bins from the histograms size 00065 int nr_bins_f1 = hist_f1.cols (); 00066 int nr_bins_f2 = hist_f2.cols (); 00067 int nr_bins_f3 = hist_f3.cols (); 00068 00069 // Factorization constant 00070 float hist_incr = 100.0 / (float)(indices.size () - 1); 00071 00072 // Iterate over all the points in the neighborhood 00073 for (size_t idx = 0; idx < indices.size (); ++idx) 00074 { 00075 // Avoid unnecessary returns 00076 if (p_idx == indices[idx]) 00077 continue; 00078 00079 // Compute the pair P to NNi 00080 if (!computePairFeatures (cloud, normals, p_idx, indices[idx], 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 int h_index = floor (nr_bins_f1 * ((pfh_tuple[0] + M_PI) * d_pi_)); 00085 if (h_index < 0) h_index = 0; 00086 if (h_index >= nr_bins_f1) h_index = nr_bins_f1 - 1; 00087 hist_f1 (row, h_index) += hist_incr; 00088 00089 h_index = floor (nr_bins_f2 * ((pfh_tuple[1] + 1.0) * 0.5)); 00090 if (h_index < 0) h_index = 0; 00091 if (h_index >= nr_bins_f2) h_index = nr_bins_f2 - 1; 00092 hist_f2 (row, h_index) += hist_incr; 00093 00094 h_index = floor (nr_bins_f3 * ((pfh_tuple[2] + 1.0) * 0.5)); 00095 if (h_index < 0) h_index = 0; 00096 if (h_index >= nr_bins_f3) h_index = nr_bins_f3 - 1; 00097 hist_f3 (row, h_index) += hist_incr; 00098 } 00099 } 00100 00102 template <typename PointInT, typename PointNT, typename PointOutT> void 00103 pcl::FPFHEstimation<PointInT, PointNT, PointOutT>::weightPointSPFHSignature ( 00104 const Eigen::MatrixXf &hist_f1, const Eigen::MatrixXf &hist_f2, const Eigen::MatrixXf &hist_f3, 00105 const std::vector<int> &indices, const std::vector<float> &dists, Eigen::VectorXf &fpfh_histogram) 00106 { 00107 assert (indices.size () == dists.size ()); 00108 double sum_f1 = 0.0, sum_f2 = 0.0, sum_f3 = 0.0; 00109 float weight = 0.0, val_f1, val_f2, val_f3; 00110 00111 // Get the number of bins from the histograms size 00112 int nr_bins_f1 = hist_f1.cols (); 00113 int nr_bins_f2 = hist_f2.cols (); 00114 int nr_bins_f3 = hist_f3.cols (); 00115 int nr_bins_f12 = nr_bins_f1 + nr_bins_f2; 00116 00117 // Use the entire patch 00118 for (size_t idx = 0, data_size = indices.size (); idx < data_size; ++idx) 00119 { 00120 // Minus the query point itself 00121 if (dists[idx] == 0) 00122 continue; 00123 00124 // Standard weighting function used 00125 weight = 1.0 / dists[idx]; 00126 00127 // Weight the SPFH of the query point with the SPFH of its neighbors 00128 for (int f1_i = 0; f1_i < nr_bins_f1; ++f1_i) 00129 { 00130 val_f1 = hist_f1 (indices[idx], f1_i) * weight; 00131 sum_f1 += val_f1; 00132 fpfh_histogram[f1_i] += val_f1; 00133 } 00134 00135 for (int f2_i = 0; f2_i < nr_bins_f2; ++f2_i) 00136 { 00137 val_f2 = hist_f2 (indices[idx], f2_i) * weight; 00138 sum_f2 += val_f2; 00139 fpfh_histogram[f2_i + nr_bins_f1] += val_f2; 00140 } 00141 00142 for (int f3_i = 0; f3_i < nr_bins_f3; ++f3_i) 00143 { 00144 val_f3 = hist_f3 (indices[idx], f3_i) * weight; 00145 sum_f3 += val_f3; 00146 fpfh_histogram[f3_i + nr_bins_f12] += val_f3; 00147 } 00148 } 00149 00150 if (sum_f1 != 0) 00151 sum_f1 = 100.0 / sum_f1; // histogram values sum up to 100 00152 if (sum_f2 != 0) 00153 sum_f2 = 100.0 / sum_f2; // histogram values sum up to 100 00154 if (sum_f3 != 0) 00155 sum_f3 = 100.0 / sum_f3; // histogram values sum up to 100 00156 00157 // Adjust final FPFH values 00158 for (int f1_i = 0; f1_i < nr_bins_f1; ++f1_i) 00159 fpfh_histogram[f1_i] *= sum_f1; 00160 for (int f2_i = 0; f2_i < nr_bins_f2; ++f2_i) 00161 fpfh_histogram[f2_i + nr_bins_f1] *= sum_f2; 00162 for (int f3_i = 0; f3_i < nr_bins_f3; ++f3_i) 00163 fpfh_histogram[f3_i + nr_bins_f12] *= sum_f3; 00164 } 00165 00167 template <typename PointInT, typename PointNT, typename PointOutT> void 00168 pcl::FPFHEstimation<PointInT, PointNT, PointOutT>::computeFeature (PointCloudOut &output) 00169 { 00170 // Allocate enough space to hold the NN search results 00171 // \note This resize is irrelevant for a radiusSearch (). 00172 std::vector<int> nn_indices (k_); 00173 std::vector<float> nn_dists (k_); 00174 00175 std::set<int> spfh_indices; 00176 std::vector<int> spfh_hist_lookup (surface_->points.size ()); 00177 00178 // Build a list of (unique) indices for which we will need to compute SPFH signatures 00179 // (We need an SPFH signature for every point that is a neighbor of any point in input_[indices_]) 00180 if (surface_ != input_ || 00181 indices_->size () != surface_->points.size ()) 00182 { 00183 for (size_t idx = 0; idx < indices_->size (); ++idx) 00184 { 00185 int p_idx = (*indices_)[idx]; 00186 this->searchForNeighbors (p_idx, search_parameter_, nn_indices, nn_dists); 00187 00188 spfh_indices.insert (nn_indices.begin (), nn_indices.end ()); 00189 } 00190 } 00191 else 00192 { 00193 // Special case: When a feature must be computed at every point, there is no need for a neighborhood search 00194 for (size_t idx = 0; idx < indices_->size (); ++idx) 00195 spfh_indices.insert ((int) idx); 00196 } 00197 00198 // Initialize the arrays that will store the SPFH signatures 00199 size_t data_size = spfh_indices.size (); 00200 hist_f1_.setZero (data_size, nr_bins_f1_); 00201 hist_f2_.setZero (data_size, nr_bins_f2_); 00202 hist_f3_.setZero (data_size, nr_bins_f3_); 00203 00204 // Compute SPFH signatures for every point that needs them 00205 std::set<int>::iterator spfh_indices_itr = spfh_indices.begin (); 00206 for (size_t i = 0; i < spfh_indices.size (); ++i) 00207 { 00208 // Get the next point index 00209 int p_idx = *spfh_indices_itr; 00210 ++spfh_indices_itr; 00211 00212 // Find the neighborhood around p_idx 00213 this->searchForNeighbors (*surface_, p_idx, search_parameter_, nn_indices, nn_dists); 00214 00215 // Estimate the SPFH signature around p_idx 00216 computePointSPFHSignature (*surface_, *normals_, p_idx, i, nn_indices, hist_f1_, hist_f2_, hist_f3_); 00217 00218 // Populate a lookup table for converting a point index to its corresponding row in the spfh_hist_* matrices 00219 spfh_hist_lookup[p_idx] = i; 00220 } 00221 00222 // Intialize the array that will store the FPFH signature 00223 int nr_bins = nr_bins_f1_ + nr_bins_f2_ + nr_bins_f3_; 00224 fpfh_histogram_.setZero (nr_bins); 00225 00226 // Iterate over the entire index vector 00227 for (size_t idx = 0; idx < indices_->size (); ++idx) 00228 { 00229 // Find the indices of point idx's neighbors... 00230 this->searchForNeighbors ((*indices_)[idx], search_parameter_, nn_indices, nn_dists); 00231 00232 // ... and remap the nn_indices values so that they represent row indices in the spfh_hist_* matrices 00233 // instead of indices into surface_->points 00234 for (size_t i = 0; i < nn_indices.size (); ++i) 00235 nn_indices[i] = spfh_hist_lookup[nn_indices[i]]; 00236 00237 // Compute the FPFH signature (i.e. compute a weighted combination of local SPFH signatures) ... 00238 weightPointSPFHSignature (hist_f1_, hist_f2_, hist_f3_, nn_indices, nn_dists, fpfh_histogram_); 00239 00240 // ...and copy it into the output cloud 00241 for (int d = 0; d < fpfh_histogram_.size (); ++d) 00242 output.points[idx].histogram[d] = fpfh_histogram_[d]; 00243 00244 fpfh_histogram_.setZero (); 00245 } 00246 00247 } 00248 00249 #define PCL_INSTANTIATE_FPFHEstimation(T,NT,OutT) template class PCL_EXPORTS pcl::FPFHEstimation<T,NT,OutT>; 00250 00251 #endif // PCL_FEATURES_IMPL_FPFH_H_ 00252