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-2012, Willow Garage, Inc. 00006 * 00007 * All rights reserved. 00008 * 00009 * Redistribution and use in source and binary forms, with or without 00010 * modification, are permitted provided that the following conditions 00011 * are met: 00012 * 00013 * * Redistributions of source code must retain the above copyright 00014 * notice, this list of conditions and the following disclaimer. 00015 * * Redistributions in binary form must reproduce the above 00016 * copyright notice, this list of conditions and the following 00017 * disclaimer in the documentation and/or other materials provided 00018 * with the distribution. 00019 * * Neither the name of the copyright holder(s) nor the names of its 00020 * contributors may be used to endorse or promote products derived 00021 * from this software without specific prior written permission. 00022 * 00023 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 00024 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 00025 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 00026 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 00027 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 00028 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 00029 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 00030 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 00031 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 00032 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 00033 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 00034 * POSSIBILITY OF SUCH DAMAGE. 00035 * 00036 * $Id$ 00037 * 00038 */ 00039 00040 #ifndef PCL_FILTERS_CONVOLUTION_3D_IMPL_HPP 00041 #define PCL_FILTERS_CONVOLUTION_3D_IMPL_HPP 00042 00043 #include <pcl/pcl_config.h> 00044 #include <pcl/point_types.h> 00045 #include <pcl/common/point_operators.h> 00046 00047 /////////////////////////////////////////////////////////////////////////////////////////////////// 00048 namespace pcl 00049 { 00050 namespace filters 00051 { 00052 template <typename PointT> 00053 class ConvolvingKernel<PointT, pcl::Normal> 00054 { 00055 void 00056 makeInfinite (pcl::Normal& n) 00057 { 00058 n.normal_x = n.normal_y = n.normal_z = std::numeric_limits<float>::quiet_NaN (); 00059 } 00060 }; 00061 00062 template <typename PointT> class 00063 ConvolvingKernel<PointT, pcl::PointXY> 00064 { 00065 void 00066 makeInfinite (pcl::PointXY& p) 00067 { 00068 p.x = p.y = std::numeric_limits<float>::quiet_NaN (); 00069 } 00070 }; 00071 } 00072 } 00073 00074 /////////////////////////////////////////////////////////////////////////////////////////////////// 00075 template<typename PointInT, typename PointOutT> bool 00076 pcl::filters::GaussianKernel<PointInT, PointOutT>::initCompute () 00077 { 00078 if (sigma_ == 0) 00079 { 00080 PCL_ERROR ("Sigma is not set or equal to 0!\n", sigma_); 00081 return (false); 00082 } 00083 sigma_sqr_ = sigma_ * sigma_; 00084 00085 if (sigma_coefficient_) 00086 { 00087 if ((*sigma_coefficient_) > 6 || (*sigma_coefficient_) < 3) 00088 { 00089 PCL_ERROR ("Sigma coefficient (%f) out of [3..6]!\n", (*sigma_coefficient_)); 00090 return (false); 00091 } 00092 else 00093 threshold_ = (*sigma_coefficient_) * (*sigma_coefficient_) * sigma_sqr_; 00094 } 00095 00096 return (true); 00097 } 00098 00099 /////////////////////////////////////////////////////////////////////////////////////////////////// 00100 template<typename PointInT, typename PointOutT> PointOutT 00101 pcl::filters::GaussianKernel<PointInT, PointOutT>::operator() (const std::vector<int>& indices, 00102 const std::vector<float>& distances) 00103 { 00104 using namespace pcl::common; 00105 PointOutT result; 00106 float total_weight = 0; 00107 std::vector<float>::const_iterator dist_it = distances.begin (); 00108 00109 for (std::vector<int>::const_iterator idx_it = indices.begin (); 00110 idx_it != indices.end (); 00111 ++idx_it, ++dist_it) 00112 { 00113 if (*dist_it <= threshold_ && isFinite ((*input_) [*idx_it])) 00114 { 00115 float weight = expf (-0.5f * (*dist_it) / sigma_sqr_); 00116 result += weight * (*input_) [*idx_it]; 00117 total_weight += weight; 00118 } 00119 } 00120 if (total_weight != 0) 00121 result /= total_weight; 00122 else 00123 makeInfinite (result); 00124 00125 return (result); 00126 } 00127 00128 /////////////////////////////////////////////////////////////////////////////////////////////////////// 00129 template<typename PointInT, typename PointOutT> PointOutT 00130 pcl::filters::GaussianKernelRGB<PointInT, PointOutT>::operator() (const std::vector<int>& indices, const std::vector<float>& distances) 00131 { 00132 using namespace pcl::common; 00133 PointOutT result; 00134 float total_weight = 0; 00135 float r = 0, g = 0, b = 0; 00136 std::vector<float>::const_iterator dist_it = distances.begin (); 00137 00138 for (std::vector<int>::const_iterator idx_it = indices.begin (); 00139 idx_it != indices.end (); 00140 ++idx_it, ++dist_it) 00141 { 00142 if (*dist_it <= threshold_ && isFinite ((*input_) [*idx_it])) 00143 { 00144 float weight = expf (-0.5f * (*dist_it) / sigma_sqr_); 00145 result.x += weight * (*input_) [*idx_it].x; 00146 result.y += weight * (*input_) [*idx_it].y; 00147 result.z += weight * (*input_) [*idx_it].z; 00148 r += weight * static_cast<float> ((*input_) [*idx_it].r); 00149 g += weight * static_cast<float> ((*input_) [*idx_it].g); 00150 b += weight * static_cast<float> ((*input_) [*idx_it].b); 00151 total_weight += weight; 00152 } 00153 } 00154 if (total_weight != 0) 00155 { 00156 total_weight = 1.f/total_weight; 00157 r*= total_weight; g*= total_weight; b*= total_weight; 00158 result.x*= total_weight; result.y*= total_weight; result.z*= total_weight; 00159 result.r = static_cast<pcl::uint8_t> (r); 00160 result.g = static_cast<pcl::uint8_t> (g); 00161 result.b = static_cast<pcl::uint8_t> (b); 00162 } 00163 else 00164 makeInfinite (result); 00165 00166 return (result); 00167 } 00168 00169 /////////////////////////////////////////////////////////////////////////////////////////////////// 00170 template <typename PointInT, typename PointOutT, typename KernelT> 00171 pcl::filters::Convolution3D<PointInT, PointOutT, KernelT>::Convolution3D () 00172 : PCLBase <PointInT> () 00173 , surface_ () 00174 , tree_ () 00175 , search_radius_ (0) 00176 {} 00177 00178 /////////////////////////////////////////////////////////////////////////////////////////////////// 00179 template <typename PointInT, typename PointOutT, typename KernelT> bool 00180 pcl::filters::Convolution3D<PointInT, PointOutT, KernelT>::initCompute () 00181 { 00182 if (!PCLBase<PointInT>::initCompute ()) 00183 { 00184 PCL_ERROR ("[pcl::filters::Convlution3D::initCompute] init failed!\n"); 00185 return (false); 00186 } 00187 // Initialize the spatial locator 00188 if (!tree_) 00189 { 00190 if (input_->isOrganized ()) 00191 tree_.reset (new pcl::search::OrganizedNeighbor<PointInT> ()); 00192 else 00193 tree_.reset (new pcl::search::KdTree<PointInT> (false)); 00194 } 00195 // If no search surface has been defined, use the input dataset as the search surface itself 00196 if (!surface_) 00197 surface_ = input_; 00198 // Send the surface dataset to the spatial locator 00199 tree_->setInputCloud (surface_); 00200 // Do a fast check to see if the search parameters are well defined 00201 if (search_radius_ <= 0.0) 00202 { 00203 PCL_ERROR ("[pcl::filters::Convlution3D::initCompute] search radius (%f) must be > 0", 00204 search_radius_); 00205 return (false); 00206 } 00207 // Make sure the provided kernel implements the required interface 00208 if (dynamic_cast<ConvolvingKernel<PointInT, PointOutT>* > (&kernel_) == 0) 00209 { 00210 PCL_ERROR ("[pcl::filters::Convlution3D::initCompute] init failed"); 00211 PCL_ERROR ("kernel_ must implement ConvolvingKernel interface\n!"); 00212 return (false); 00213 } 00214 kernel_.setInputCloud (surface_); 00215 // Initialize convolving kernel 00216 if (!kernel_.initCompute ()) 00217 { 00218 PCL_ERROR ("[pcl::filters::Convlution3D::initCompute] kernel initialization failed!\n"); 00219 return (false); 00220 } 00221 return (true); 00222 } 00223 00224 /////////////////////////////////////////////////////////////////////////////////////////////////// 00225 template <typename PointInT, typename PointOutT, typename KernelT> void 00226 pcl::filters::Convolution3D<PointInT, PointOutT, KernelT>::convolve (PointCloudOut& output) 00227 { 00228 if (!initCompute ()) 00229 { 00230 PCL_ERROR ("[pcl::filters::Convlution3D::convolve] init failed!\n"); 00231 return; 00232 } 00233 output.resize (surface_->size ()); 00234 output.width = surface_->width; 00235 output.height = surface_->height; 00236 output.is_dense = surface_->is_dense; 00237 std::vector<int> nn_indices; 00238 std::vector<float> nn_distances; 00239 00240 #ifdef _OPENMP 00241 #pragma omp parallel for shared (output) private (nn_indices, nn_distances) num_threads (threads_) 00242 #endif 00243 for (std::size_t point_idx = 0; point_idx < surface_->size (); ++point_idx) 00244 { 00245 const PointInT& point_in = surface_->points [point_idx]; 00246 PointOutT& point_out = output [point_idx]; 00247 if (isFinite (point_in) && 00248 tree_->radiusSearch (point_in, search_radius_, nn_indices, nn_distances)) 00249 { 00250 point_out = kernel_ (nn_indices, nn_distances); 00251 } 00252 else 00253 { 00254 kernel_.makeInfinite (point_out); 00255 output.is_dense = false; 00256 } 00257 } 00258 } 00259 00260 #endif