Point Cloud Library (PCL)  1.3.1
fpfh_omp.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: fpfh_omp.hpp 3022 2011-11-01 03:42:22Z rusu $
00035  *
00036  */
00037 
00038 #ifndef PCL_FEATURES_IMPL_FPFH_OMP_H_
00039 #define PCL_FEATURES_IMPL_FPFH_OMP_H_
00040 
00041 #include "pcl/features/fpfh_omp.h"
00042 
00044 template <typename PointInT, typename PointNT, typename PointOutT> void
00045 pcl::FPFHEstimationOMP<PointInT, PointNT, PointOutT>::computeFeature (PointCloudOut &output)
00046 {
00047   std::vector<int> spfh_indices_vec;
00048   std::vector<int> spfh_hist_lookup (surface_->points.size ());
00049 
00050   // Build a list of (unique) indices for which we will need to compute SPFH signatures
00051   // (We need an SPFH signature for every point that is a neighbor of any point in input_[indices_])
00052   if (surface_ != input_ ||
00053       indices_->size () != surface_->points.size ())
00054   { 
00055     std::vector<int> nn_indices (k_); // \note These resizes are irrelevant for a radiusSearch ().
00056     std::vector<float> nn_dists (k_); 
00057 
00058     std::set<int> spfh_indices_set;
00059     for (size_t idx = 0; idx < indices_->size (); ++idx)
00060     {
00061       int p_idx = (*indices_)[idx];
00062       this->searchForNeighbors (p_idx, search_parameter_, nn_indices, nn_dists);
00063       
00064       spfh_indices_set.insert (nn_indices.begin (), nn_indices.end ());
00065     }
00066     spfh_indices_vec.resize (spfh_indices_set.size ());
00067     std::copy (spfh_indices_set.begin (), spfh_indices_set.end (), spfh_indices_vec.begin ());
00068   }
00069   else
00070   {
00071     // Special case: When a feature must be computed at every point, there is no need for a neighborhood search
00072     spfh_indices_vec.resize (indices_->size ());
00073     for (size_t idx = 0; idx < indices_->size (); ++idx)
00074       spfh_indices_vec[idx] = idx;
00075   }
00076 
00077   // Initialize the arrays that will store the SPFH signatures
00078   size_t data_size = spfh_indices_vec.size ();
00079   hist_f1_.setZero (data_size, nr_bins_f1_);
00080   hist_f2_.setZero (data_size, nr_bins_f2_);
00081   hist_f3_.setZero (data_size, nr_bins_f3_);
00082 
00083   // Compute SPFH signatures for every point that needs them
00084   
00085 #pragma omp parallel for schedule (dynamic, threads_)
00086   for (int i = 0; i < (int) spfh_indices_vec.size (); ++i)
00087   {
00088     // Get the next point index
00089     int p_idx = spfh_indices_vec[i];
00090 
00091     // Find the neighborhood around p_idx
00092     std::vector<int> nn_indices (k_); // \note These resizes are irrelevant for a radiusSearch ().
00093     std::vector<float> nn_dists (k_); 
00094     this->searchForNeighbors (*surface_, p_idx, search_parameter_, nn_indices, nn_dists);
00095 
00096     // Estimate the SPFH signature around p_idx
00097     computePointSPFHSignature (*surface_, *normals_, p_idx, i, nn_indices, hist_f1_, hist_f2_, hist_f3_);
00098 
00099     // Populate a lookup table for converting a point index to its corresponding row in the spfh_hist_* matrices
00100     spfh_hist_lookup[p_idx] = i;
00101   }
00102 
00103   // Intialize the array that will store the FPFH signature
00104   int nr_bins = nr_bins_f1_ + nr_bins_f2_ + nr_bins_f3_;
00105 
00106   // Iterate over the entire index vector
00107 #pragma omp parallel for schedule (dynamic, threads_)
00108   for (int idx = 0; idx < (int) indices_->size (); ++idx)
00109   {
00110     // Find the indices of point idx's neighbors...
00111     std::vector<int> nn_indices (k_); // \note These resizes are irrelevant for a radiusSearch ().
00112     std::vector<float> nn_dists (k_); 
00113     this->searchForNeighbors ((*indices_)[idx], search_parameter_, nn_indices, nn_dists);
00114 
00115     // ... and remap the nn_indices values so that they represent row indices in the spfh_hist_* matrices 
00116     // instead of indices into surface_->points
00117     for (size_t i = 0; i < nn_indices.size (); ++i)
00118       nn_indices[i] = spfh_hist_lookup[nn_indices[i]];
00119 
00120     // Compute the FPFH signature (i.e. compute a weighted combination of local SPFH signatures) ...
00121     Eigen::VectorXf fpfh_histogram = Eigen::VectorXf::Zero (nr_bins);
00122     weightPointSPFHSignature (hist_f1_, hist_f2_, hist_f3_, nn_indices, nn_dists, fpfh_histogram);
00123 
00124     // ...and copy it into the output cloud
00125     for (int d = 0; d < fpfh_histogram.size (); ++d)
00126       output.points[idx].histogram[d] = fpfh_histogram[d];
00127   }
00128 
00129 }
00130 
00131 /*
00133 template <typename PointInT, typename PointNT, typename PointOutT> void
00134 pcl::FPFHEstimationOMP<PointInT, PointNT, PointOutT>::computeFeature (PointCloudOut &output)
00135 {
00136   int data_size = indices_->size ();
00137   // Reset the whole thing
00138   hist_f1_.setZero (data_size, nr_bins_f1_);
00139   hist_f2_.setZero (data_size, nr_bins_f2_);
00140   hist_f3_.setZero (data_size, nr_bins_f3_);
00141 
00142   int nr_bins = nr_bins_f1_ + nr_bins_f2_ + nr_bins_f3_;
00143 
00144   // Iterating over the entire index vector
00145 #pragma omp parallel for schedule (dynamic, threads_)
00146   for (int idx = 0; idx < data_size; ++idx)
00147   {
00148     // Allocate enough space to hold the results
00149     // \note This resize is irrelevant for a radiusSearch ().
00150     std::vector<int> nn_indices (k_);
00151     std::vector<float> nn_dists (k_);
00152 
00153     this->searchForNeighbors ((*indices_)[idx], search_parameter_, nn_indices, nn_dists);
00154 
00155     // Estimate the FPFH signature at each patch
00156     this->computePointSPFHSignature (*surface_, *normals_, (*indices_)[idx], nn_indices,
00157                                hist_f1_, hist_f2_, hist_f3_);
00158   }
00159 
00160   // Iterating over the entire index vector
00161 #pragma omp parallel for schedule (dynamic, threads_)
00162   for (int idx = 0; idx < data_size; ++idx)
00163   {
00164     // Allocate enough space to hold the results
00165     // \note This resize is irrelevant for a radiusSearch ().
00166     std::vector<int> nn_indices (k_);
00167     std::vector<float> nn_dists (k_);
00168 
00169     Eigen::VectorXf fpfh_histogram = Eigen::VectorXf::Zero (nr_bins);
00170 
00171     this->searchForNeighbors ((*indices_)[idx], search_parameter_, nn_indices, nn_dists);
00172 
00173     weightPointSPFHSignature (hist_f1_, hist_f2_, hist_f3_, nn_indices, nn_dists, fpfh_histogram);
00174 
00175     // Copy into the resultant cloud
00176     for (int d = 0; d < fpfh_histogram.size (); ++d)
00177       output.points[idx].histogram[d] = fpfh_histogram[d];
00178   }
00179 }
00180 */
00181 
00182 #define PCL_INSTANTIATE_FPFHEstimationOMP(T,NT,OutT) template class PCL_EXPORTS pcl::FPFHEstimationOMP<T,NT,OutT>;
00183 
00184 #endif    // PCL_FEATURES_IMPL_FPFH_OMP_H_ 
00185 
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Defines