Point Cloud Library (PCL)  1.3.1
shot_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  *
00035  */
00036 
00037 #ifndef PCL_FEATURES_IMPL_SHOT_OMP_H_
00038 #define PCL_FEATURES_IMPL_SHOT_OMP_H_
00039 
00040 #include "pcl/features/shot_omp.h"
00041 
00043 template<typename PointInT, typename PointNT, typename PointOutT>
00044 void
00045 pcl::SHOTEstimationOMP<PointInT, PointNT, PointOutT>::computeFeature (PointCloudOut &output)
00046 {
00047   if (threads_ < 0)
00048     threads_ = 1;
00049 
00050   descLength_ = nr_grid_sector_ * (nr_shape_bins_ + 1);
00051 
00052   sqradius_ = search_radius_ * search_radius_;
00053   radius3_4_ = (search_radius_ * 3) / 4;
00054   radius1_4_ = search_radius_ / 4;
00055   radius1_2_ = search_radius_ / 2;
00056 
00057   if (output.points[0].descriptor.size () != (size_t)descLength_)
00058     for (size_t idx = 0; idx < indices_->size (); ++idx)
00059       output.points[idx].descriptor.resize (descLength_);
00060 
00061   int data_size = indices_->size ();
00062   Eigen::VectorXf *shot = new Eigen::VectorXf[threads_];
00063   std::vector<std::vector<Eigen::Vector4f, Eigen::aligned_allocator<Eigen::Vector4f> > > rfs (threads_);
00064   for (size_t i = 0; i < rfs.size (); ++i)
00065     rfs[i].resize (3);
00066 
00067   for (int i = 0; i < threads_; i++)
00068     shot[i].setZero (descLength_);
00069 
00070   
00071 
00072   // Iterating over the entire index vector
00073   #pragma omp parallel for num_threads(threads_)
00074   for (int idx = 0; idx < data_size; ++idx)
00075   {
00076    // Allocate enough space to hold the results
00077     // \note This resize is irrelevant for a radiusSearch ().
00078     std::vector<int> nn_indices (k_);
00079     std::vector<float> nn_dists (k_);
00080 
00081     this->searchForNeighbors ((*indices_)[idx], search_parameter_, nn_indices, nn_dists);
00082 
00083   // Estimate the SHOT at each patch
00084 #ifdef _OPENMP
00085     int tid = omp_get_thread_num ();
00086 #else
00087     int tid = 0;
00088 #endif
00089   computePointSHOT ((*indices_)[idx], nn_indices, nn_dists, shot[tid], rfs[tid]);
00090 
00091   // Copy into the resultant cloud
00092     for (int d = 0; d < shot[tid].size (); ++d)
00093       output.points[idx].descriptor[d] = shot[tid][d];
00094     for (int d = 0; d < 9; ++d)
00095       output.points[idx].rf[d] = rfs[tid][d/3][d % 3];
00096   }
00097   delete[] shot;
00098 }
00099 
00101 template<typename PointNT, typename PointOutT>
00102 void
00103 pcl::SHOTEstimationOMP<pcl::PointXYZRGBA, PointNT, PointOutT>::computeFeature (PointCloudOut &output)
00104 {
00105   if (threads_ < 0)
00106     threads_ = 1;
00107 
00108   descLength_ = (b_describe_shape_) ? nr_grid_sector_ * (nr_shape_bins_ + 1) : 0;
00109   descLength_ += (b_describe_color_) ? nr_grid_sector_ * (nr_color_bins_ + 1) : 0;
00110 
00111   sqradius_ = search_radius_ * search_radius_;
00112   radius3_4_ = (search_radius_ * 3) / 4;
00113   radius1_4_ = search_radius_ / 4;
00114   radius1_2_ = search_radius_ / 2;
00115 
00116   if (output.points[0].descriptor.size () != (size_t)descLength_)
00117     for (size_t idx = 0; idx < indices_->size (); ++idx)
00118       output.points[idx].descriptor.resize (descLength_);
00119 
00120   int data_size = indices_->size ();
00121   Eigen::VectorXf *shot = new Eigen::VectorXf[threads_];
00122   std::vector<std::vector<Eigen::Vector4f, Eigen::aligned_allocator<Eigen::Vector4f> > > rfs (threads_);
00123   for (size_t i = 0; i < rfs.size (); ++i)
00124     rfs[i].resize (3);
00125 
00126   for (int i = 0; i < threads_; i++)
00127     shot[i].setZero (descLength_);
00128 
00129   // Iterating over the entire index vector
00130 #pragma omp parallel for num_threads(threads_)
00131   for (int idx = 0; idx < data_size; ++idx)
00132   {
00133     // Allocate enough space to hold the results
00134     // \note This resize is irrelevant for a radiusSearch ().
00135     std::vector<int> nn_indices (k_);
00136     std::vector<float> nn_dists (k_);
00137 
00138     this->searchForNeighbors ((*indices_)[idx], search_parameter_, nn_indices, nn_dists);
00139 
00140     // Estimate the SHOT at each patch
00141 #ifdef _OPENMP
00142     int tid = omp_get_thread_num ();
00143 #else
00144     int tid = 0;
00145 #endif
00146     computePointSHOT ((*indices_)[idx], nn_indices, nn_dists, shot[tid], rfs[tid]);
00147 
00148     // Copy into the resultant cloud
00149     for (int d = 0; d < shot[tid].size (); ++d)
00150       output.points[idx].descriptor[d] = shot[tid][d];
00151     for (int d = 0; d < 9; ++d)
00152       output.points[idx].rf[d] = rfs[tid][d / 3][d % 3];
00153   }
00154 
00155   delete[] shot;
00156 }
00157 
00158 #define PCL_INSTANTIATE_SHOTEstimationOMP(T,NT,OutT) template class PCL_EXPORTS pcl::SHOTEstimationOMP<T,NT,OutT>;
00159 
00160 #endif    // PCL_FEATURES_IMPL_SHOT_OMP_H_
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Defines