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) 2009, 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 * 00038 */ 00039 00040 #ifndef PCL_FEATURES_IMPL_SHOT_OMP_H_ 00041 #define PCL_FEATURES_IMPL_SHOT_OMP_H_ 00042 00043 #include <pcl/features/shot_omp.h> 00044 #include <pcl/common/time.h> 00045 #include <pcl/features/shot_lrf_omp.h> 00046 00047 template<typename PointInT, typename PointNT, typename PointOutT, typename PointRFT> bool 00048 pcl::SHOTEstimationOMP<PointInT, PointNT, PointOutT, PointRFT>::initCompute () 00049 { 00050 if (!FeatureFromNormals<PointInT, PointNT, PointOutT>::initCompute ()) 00051 { 00052 PCL_ERROR ("[pcl::%s::initCompute] Init failed.\n", getClassName ().c_str ()); 00053 return (false); 00054 } 00055 00056 // SHOT cannot work with k-search 00057 if (this->getKSearch () != 0) 00058 { 00059 PCL_ERROR( 00060 "[pcl::%s::initCompute] Error! Search method set to k-neighborhood. Call setKSearch(0) and setRadiusSearch( radius ) to use this class.\n", 00061 getClassName().c_str ()); 00062 return (false); 00063 } 00064 00065 // Default LRF estimation alg: SHOTLocalReferenceFrameEstimationOMP 00066 typename boost::shared_ptr<SHOTLocalReferenceFrameEstimationOMP<PointInT, PointRFT> > lrf_estimator(new SHOTLocalReferenceFrameEstimationOMP<PointInT, PointRFT>()); 00067 lrf_estimator->setRadiusSearch ((lrf_radius_ > 0 ? lrf_radius_ : search_radius_)); 00068 lrf_estimator->setInputCloud (input_); 00069 lrf_estimator->setIndices (indices_); 00070 lrf_estimator->setNumberOfThreads(threads_); 00071 00072 if (!fake_surface_) 00073 lrf_estimator->setSearchSurface(surface_); 00074 00075 if (!FeatureWithLocalReferenceFrames<PointInT, PointRFT>::initLocalReferenceFrames (indices_->size (), lrf_estimator)) 00076 { 00077 PCL_ERROR ("[pcl::%s::initCompute] Init failed.\n", getClassName ().c_str ()); 00078 return (false); 00079 } 00080 00081 return (true); 00082 } 00083 00084 ////////////////////////////////////////////////////////////////////////////////////////////// 00085 template<typename PointInT, typename PointNT, typename PointOutT, typename PointRFT> bool 00086 pcl::SHOTColorEstimationOMP<PointInT, PointNT, PointOutT, PointRFT>::initCompute () 00087 { 00088 if (!FeatureFromNormals<PointInT, PointNT, PointOutT>::initCompute ()) 00089 { 00090 PCL_ERROR ("[pcl::%s::initCompute] Init failed.\n", getClassName ().c_str ()); 00091 return (false); 00092 } 00093 00094 // SHOT cannot work with k-search 00095 if (this->getKSearch () != 0) 00096 { 00097 PCL_ERROR( 00098 "[pcl::%s::initCompute] Error! Search method set to k-neighborhood. Call setKSearch(0) and setRadiusSearch( radius ) to use this class.\n", 00099 getClassName().c_str ()); 00100 return (false); 00101 } 00102 00103 // Default LRF estimation alg: SHOTLocalReferenceFrameEstimationOMP 00104 typename boost::shared_ptr<SHOTLocalReferenceFrameEstimationOMP<PointInT, PointRFT> > lrf_estimator(new SHOTLocalReferenceFrameEstimationOMP<PointInT, PointRFT>()); 00105 lrf_estimator->setRadiusSearch ((lrf_radius_ > 0 ? lrf_radius_ : search_radius_)); 00106 lrf_estimator->setInputCloud (input_); 00107 lrf_estimator->setIndices (indices_); 00108 lrf_estimator->setNumberOfThreads(threads_); 00109 00110 if (!fake_surface_) 00111 lrf_estimator->setSearchSurface(surface_); 00112 00113 if (!FeatureWithLocalReferenceFrames<PointInT, PointRFT>::initLocalReferenceFrames (indices_->size (), lrf_estimator)) 00114 { 00115 PCL_ERROR ("[pcl::%s::initCompute] Init failed.\n", getClassName ().c_str ()); 00116 return (false); 00117 } 00118 00119 return (true); 00120 } 00121 00122 ////////////////////////////////////////////////////////////////////////////////////////////// 00123 template<typename PointInT, typename PointNT, typename PointOutT, typename PointRFT> void 00124 pcl::SHOTEstimationOMP<PointInT, PointNT, PointOutT, PointRFT>::computeFeature (PointCloudOut &output) 00125 { 00126 descLength_ = nr_grid_sector_ * (nr_shape_bins_ + 1); 00127 00128 sqradius_ = search_radius_ * search_radius_; 00129 radius3_4_ = (search_radius_ * 3) / 4; 00130 radius1_4_ = search_radius_ / 4; 00131 radius1_2_ = search_radius_ / 2; 00132 00133 assert(descLength_ == 352); 00134 00135 int data_size = static_cast<int> (indices_->size ()); 00136 00137 output.is_dense = true; 00138 // Iterating over the entire index vector 00139 #ifdef _OPENMP 00140 #pragma omp parallel for num_threads(threads_) 00141 #endif 00142 for (int idx = 0; idx < data_size; ++idx) 00143 { 00144 00145 Eigen::VectorXf shot; 00146 shot.setZero (descLength_); 00147 00148 bool lrf_is_nan = false; 00149 const PointRFT& current_frame = (*frames_)[idx]; 00150 if (!pcl_isfinite (current_frame.x_axis[0]) || 00151 !pcl_isfinite (current_frame.y_axis[0]) || 00152 !pcl_isfinite (current_frame.z_axis[0])) 00153 { 00154 PCL_WARN ("[pcl::%s::computeFeature] The local reference frame is not valid! Aborting description of point with index %d\n", 00155 getClassName ().c_str (), (*indices_)[idx]); 00156 lrf_is_nan = true; 00157 } 00158 00159 // Allocate enough space to hold the results 00160 // \note This resize is irrelevant for a radiusSearch (). 00161 std::vector<int> nn_indices (k_); 00162 std::vector<float> nn_dists (k_); 00163 00164 if (!isFinite ((*input_)[(*indices_)[idx]]) || lrf_is_nan || this->searchForNeighbors ((*indices_)[idx], search_parameter_, nn_indices, 00165 nn_dists) == 0) 00166 { 00167 // Copy into the resultant cloud 00168 for (int d = 0; d < shot.size (); ++d) 00169 output.points[idx].descriptor[d] = std::numeric_limits<float>::quiet_NaN (); 00170 for (int d = 0; d < 9; ++d) 00171 output.points[idx].rf[d] = std::numeric_limits<float>::quiet_NaN (); 00172 00173 output.is_dense = false; 00174 continue; 00175 } 00176 00177 // Estimate the SHOT at each patch 00178 this->computePointSHOT (idx, nn_indices, nn_dists, shot); 00179 00180 // Copy into the resultant cloud 00181 for (int d = 0; d < shot.size (); ++d) 00182 output.points[idx].descriptor[d] = shot[d]; 00183 for (int d = 0; d < 3; ++d) 00184 { 00185 output.points[idx].rf[d + 0] = frames_->points[idx].x_axis[d]; 00186 output.points[idx].rf[d + 3] = frames_->points[idx].y_axis[d]; 00187 output.points[idx].rf[d + 6] = frames_->points[idx].z_axis[d]; 00188 } 00189 } 00190 } 00191 00192 ////////////////////////////////////////////////////////////////////////////////////////////// 00193 template <typename PointInT, typename PointNT, typename PointOutT, typename PointRFT> void 00194 pcl::SHOTColorEstimationOMP<PointInT, PointNT, PointOutT, PointRFT>::computeFeature (PointCloudOut &output) 00195 { 00196 descLength_ = (b_describe_shape_) ? nr_grid_sector_ * (nr_shape_bins_ + 1) : 0; 00197 descLength_ += (b_describe_color_) ? nr_grid_sector_ * (nr_color_bins_ + 1) : 0; 00198 00199 assert( (!b_describe_color_ && b_describe_shape_ && descLength_ == 352) || 00200 (b_describe_color_ && !b_describe_shape_ && descLength_ == 992) || 00201 (b_describe_color_ && b_describe_shape_ && descLength_ == 1344) 00202 ); 00203 00204 sqradius_ = search_radius_ * search_radius_; 00205 radius3_4_ = (search_radius_ * 3) / 4; 00206 radius1_4_ = search_radius_ / 4; 00207 radius1_2_ = search_radius_ / 2; 00208 00209 int data_size = static_cast<int> (indices_->size ()); 00210 00211 output.is_dense = true; 00212 // Iterating over the entire index vector 00213 #ifdef _OPENMP 00214 #pragma omp parallel for num_threads(threads_) 00215 #endif 00216 for (int idx = 0; idx < data_size; ++idx) 00217 { 00218 Eigen::VectorXf shot; 00219 shot.setZero (descLength_); 00220 00221 // Allocate enough space to hold the results 00222 // \note This resize is irrelevant for a radiusSearch (). 00223 std::vector<int> nn_indices (k_); 00224 std::vector<float> nn_dists (k_); 00225 00226 bool lrf_is_nan = false; 00227 const PointRFT& current_frame = (*frames_)[idx]; 00228 if (!pcl_isfinite (current_frame.x_axis[0]) || 00229 !pcl_isfinite (current_frame.y_axis[0]) || 00230 !pcl_isfinite (current_frame.z_axis[0])) 00231 { 00232 PCL_WARN ("[pcl::%s::computeFeature] The local reference frame is not valid! Aborting description of point with index %d\n", 00233 getClassName ().c_str (), (*indices_)[idx]); 00234 lrf_is_nan = true; 00235 } 00236 00237 if (!isFinite ((*input_)[(*indices_)[idx]]) || 00238 lrf_is_nan || 00239 this->searchForNeighbors ((*indices_)[idx], search_parameter_, nn_indices, nn_dists) == 0) 00240 { 00241 // Copy into the resultant cloud 00242 for (int d = 0; d < shot.size (); ++d) 00243 output.points[idx].descriptor[d] = std::numeric_limits<float>::quiet_NaN (); 00244 for (int d = 0; d < 9; ++d) 00245 output.points[idx].rf[d] = std::numeric_limits<float>::quiet_NaN (); 00246 00247 output.is_dense = false; 00248 continue; 00249 } 00250 00251 // Estimate the SHOT at each patch 00252 this->computePointSHOT (idx, nn_indices, nn_dists, shot); 00253 00254 // Copy into the resultant cloud 00255 for (int d = 0; d < shot.size (); ++d) 00256 output.points[idx].descriptor[d] = shot[d]; 00257 for (int d = 0; d < 3; ++d) 00258 { 00259 output.points[idx].rf[d + 0] = frames_->points[idx].x_axis[d]; 00260 output.points[idx].rf[d + 3] = frames_->points[idx].y_axis[d]; 00261 output.points[idx].rf[d + 6] = frames_->points[idx].z_axis[d]; 00262 } 00263 } 00264 } 00265 00266 #define PCL_INSTANTIATE_SHOTEstimationOMP(T,NT,OutT,RFT) template class PCL_EXPORTS pcl::SHOTEstimationOMP<T,NT,OutT,RFT>; 00267 #define PCL_INSTANTIATE_SHOTColorEstimationOMP(T,NT,OutT,RFT) template class PCL_EXPORTS pcl::SHOTColorEstimationOMP<T,NT,OutT,RFT>; 00268 00269 #endif // PCL_FEATURES_IMPL_SHOT_OMP_H_