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