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_OMP_H_ 00042 #define PCL_FEATURES_IMPL_FPFH_OMP_H_ 00043 00044 #include <pcl/features/fpfh_omp.h> 00045 00046 ////////////////////////////////////////////////////////////////////////////////////////////// 00047 template <typename PointInT, typename PointNT, typename PointOutT> void 00048 pcl::FPFHEstimationOMP<PointInT, PointNT, PointOutT>::computeFeature (PointCloudOut &output) 00049 { 00050 std::vector<int> spfh_indices_vec; 00051 std::vector<int> spfh_hist_lookup (surface_->points.size ()); 00052 00053 // Build a list of (unique) indices for which we will need to compute SPFH signatures 00054 // (We need an SPFH signature for every point that is a neighbor of any point in input_[indices_]) 00055 if (surface_ != input_ || 00056 indices_->size () != surface_->points.size ()) 00057 { 00058 std::vector<int> nn_indices (k_); // \note These resizes are irrelevant for a radiusSearch (). 00059 std::vector<float> nn_dists (k_); 00060 00061 std::set<int> spfh_indices_set; 00062 for (size_t idx = 0; idx < indices_->size (); ++idx) 00063 { 00064 int p_idx = (*indices_)[idx]; 00065 if (this->searchForNeighbors (p_idx, search_parameter_, nn_indices, nn_dists) == 0) 00066 continue; 00067 00068 spfh_indices_set.insert (nn_indices.begin (), nn_indices.end ()); 00069 } 00070 spfh_indices_vec.resize (spfh_indices_set.size ()); 00071 std::copy (spfh_indices_set.begin (), spfh_indices_set.end (), spfh_indices_vec.begin ()); 00072 } 00073 else 00074 { 00075 // Special case: When a feature must be computed at every point, there is no need for a neighborhood search 00076 spfh_indices_vec.resize (indices_->size ()); 00077 for (int idx = 0; idx < static_cast<int> (indices_->size ()); ++idx) 00078 spfh_indices_vec[idx] = idx; 00079 } 00080 00081 // Initialize the arrays that will store the SPFH signatures 00082 size_t data_size = spfh_indices_vec.size (); 00083 hist_f1_.setZero (data_size, nr_bins_f1_); 00084 hist_f2_.setZero (data_size, nr_bins_f2_); 00085 hist_f3_.setZero (data_size, nr_bins_f3_); 00086 00087 std::vector<int> nn_indices (k_); // \note These resizes are irrelevant for a radiusSearch (). 00088 std::vector<float> nn_dists (k_); 00089 00090 // Compute SPFH signatures for every point that needs them 00091 00092 #ifdef _OPENMP 00093 #pragma omp parallel for shared (spfh_hist_lookup) private (nn_indices, nn_dists) num_threads(threads_) 00094 #endif 00095 for (int i = 0; i < static_cast<int> (spfh_indices_vec.size ()); ++i) 00096 { 00097 // Get the next point index 00098 int p_idx = spfh_indices_vec[i]; 00099 00100 // Find the neighborhood around p_idx 00101 if (this->searchForNeighbors (*surface_, p_idx, search_parameter_, nn_indices, nn_dists) == 0) 00102 continue; 00103 00104 // Estimate the SPFH signature around p_idx 00105 this->computePointSPFHSignature (*surface_, *normals_, p_idx, i, nn_indices, hist_f1_, hist_f2_, hist_f3_); 00106 00107 // Populate a lookup table for converting a point index to its corresponding row in the spfh_hist_* matrices 00108 spfh_hist_lookup[p_idx] = i; 00109 } 00110 00111 // Intialize the array that will store the FPFH signature 00112 int nr_bins = nr_bins_f1_ + nr_bins_f2_ + nr_bins_f3_; 00113 00114 nn_indices.clear(); 00115 nn_dists.clear(); 00116 00117 // Iterate over the entire index vector 00118 #ifdef _OPENMP 00119 #pragma omp parallel for shared (output) private (nn_indices, nn_dists) num_threads(threads_) 00120 #endif 00121 for (int idx = 0; idx < static_cast<int> (indices_->size ()); ++idx) 00122 { 00123 // Find the indices of point idx's neighbors... 00124 if (!isFinite ((*input_)[(*indices_)[idx]]) || 00125 this->searchForNeighbors ((*indices_)[idx], search_parameter_, nn_indices, nn_dists) == 0) 00126 { 00127 for (int d = 0; d < nr_bins; ++d) 00128 output.points[idx].histogram[d] = std::numeric_limits<float>::quiet_NaN (); 00129 00130 output.is_dense = false; 00131 continue; 00132 } 00133 00134 00135 // ... and remap the nn_indices values so that they represent row indices in the spfh_hist_* matrices 00136 // instead of indices into surface_->points 00137 for (size_t i = 0; i < nn_indices.size (); ++i) 00138 nn_indices[i] = spfh_hist_lookup[nn_indices[i]]; 00139 00140 // Compute the FPFH signature (i.e. compute a weighted combination of local SPFH signatures) ... 00141 Eigen::VectorXf fpfh_histogram = Eigen::VectorXf::Zero (nr_bins); 00142 weightPointSPFHSignature (hist_f1_, hist_f2_, hist_f3_, nn_indices, nn_dists, fpfh_histogram); 00143 00144 // ...and copy it into the output cloud 00145 for (int d = 0; d < nr_bins; ++d) 00146 output.points[idx].histogram[d] = fpfh_histogram[d]; 00147 } 00148 00149 } 00150 00151 #define PCL_INSTANTIATE_FPFHEstimationOMP(T,NT,OutT) template class PCL_EXPORTS pcl::FPFHEstimationOMP<T,NT,OutT>; 00152 00153 #endif // PCL_FEATURES_IMPL_FPFH_OMP_H_ 00154