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 * 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_