Point Cloud Library (PCL)  1.7.0
convolution_3d.hpp
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Point Cloud Library (PCL) - www.pointclouds.org
5  * Copyright (c) 2010-2012, Willow Garage, Inc.
6  *
7  * All rights reserved.
8  *
9  * Redistribution and use in source and binary forms, with or without
10  * modification, are permitted provided that the following conditions
11  * are met:
12  *
13  * * Redistributions of source code must retain the above copyright
14  * notice, this list of conditions and the following disclaimer.
15  * * Redistributions in binary form must reproduce the above
16  * copyright notice, this list of conditions and the following
17  * disclaimer in the documentation and/or other materials provided
18  * with the distribution.
19  * * Neither the name of the copyright holder(s) nor the names of its
20  * contributors may be used to endorse or promote products derived
21  * from this software without specific prior written permission.
22  *
23  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
24  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
25  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
26  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
27  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
28  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
29  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
30  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
31  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
32  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
33  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
34  * POSSIBILITY OF SUCH DAMAGE.
35  *
36  * $Id$
37  *
38  */
39 
40 #ifndef PCL_FILTERS_CONVOLUTION_3D_IMPL_HPP
41 #define PCL_FILTERS_CONVOLUTION_3D_IMPL_HPP
42 
43 #include <pcl/pcl_config.h>
44 #include <pcl/point_types.h>
45 #include <pcl/common/point_operators.h>
46 
47 ///////////////////////////////////////////////////////////////////////////////////////////////////
48 namespace pcl
49 {
50  namespace filters
51  {
52  template <typename PointT>
54  {
55  void
57  {
58  n.normal_x = n.normal_y = n.normal_z = std::numeric_limits<float>::quiet_NaN ();
59  }
60  };
61 
62  template <typename PointT> class
64  {
65  void
66  makeInfinite (pcl::PointXY& p)
67  {
68  p.x = p.y = std::numeric_limits<float>::quiet_NaN ();
69  }
70  };
71  }
72 }
73 
74 ///////////////////////////////////////////////////////////////////////////////////////////////////
75 template<typename PointInT, typename PointOutT> bool
77 {
78  if (sigma_ == 0)
79  {
80  PCL_ERROR ("Sigma is not set or equal to 0!\n", sigma_);
81  return (false);
82  }
83  sigma_sqr_ = sigma_ * sigma_;
84 
85  if (sigma_coefficient_)
86  {
87  if ((*sigma_coefficient_) > 6 || (*sigma_coefficient_) < 3)
88  {
89  PCL_ERROR ("Sigma coefficient (%f) out of [3..6]!\n", (*sigma_coefficient_));
90  return (false);
91  }
92  else
93  threshold_ = (*sigma_coefficient_) * (*sigma_coefficient_) * sigma_sqr_;
94  }
95 
96  return (true);
97 }
98 
99 ///////////////////////////////////////////////////////////////////////////////////////////////////
100 template<typename PointInT, typename PointOutT> PointOutT
102  const std::vector<float>& distances)
103 {
104  using namespace pcl::common;
105  PointOutT result;
106  float total_weight = 0;
107  std::vector<float>::const_iterator dist_it = distances.begin ();
108 
109  for (std::vector<int>::const_iterator idx_it = indices.begin ();
110  idx_it != indices.end ();
111  ++idx_it, ++dist_it)
112  {
113  if (*dist_it <= threshold_ && isFinite ((*input_) [*idx_it]))
114  {
115  float weight = expf (-0.5f * (*dist_it) / sigma_sqr_);
116  result += weight * (*input_) [*idx_it];
117  total_weight += weight;
118  }
119  }
120  if (total_weight != 0)
121  result /= total_weight;
122  else
123  makeInfinite (result);
124 
125  return (result);
126 }
127 
128 ///////////////////////////////////////////////////////////////////////////////////////////////////////
129 template<typename PointInT, typename PointOutT> PointOutT
130 pcl::filters::GaussianKernelRGB<PointInT, PointOutT>::operator() (const std::vector<int>& indices, const std::vector<float>& distances)
131 {
132  using namespace pcl::common;
133  PointOutT result;
134  float total_weight = 0;
135  float r = 0, g = 0, b = 0;
136  std::vector<float>::const_iterator dist_it = distances.begin ();
137 
138  for (std::vector<int>::const_iterator idx_it = indices.begin ();
139  idx_it != indices.end ();
140  ++idx_it, ++dist_it)
141  {
142  if (*dist_it <= threshold_ && isFinite ((*input_) [*idx_it]))
143  {
144  float weight = expf (-0.5f * (*dist_it) / sigma_sqr_);
145  result.x += weight * (*input_) [*idx_it].x;
146  result.y += weight * (*input_) [*idx_it].y;
147  result.z += weight * (*input_) [*idx_it].z;
148  r += weight * static_cast<float> ((*input_) [*idx_it].r);
149  g += weight * static_cast<float> ((*input_) [*idx_it].g);
150  b += weight * static_cast<float> ((*input_) [*idx_it].b);
151  total_weight += weight;
152  }
153  }
154  if (total_weight != 0)
155  {
156  total_weight = 1.f/total_weight;
157  r*= total_weight; g*= total_weight; b*= total_weight;
158  result.x*= total_weight; result.y*= total_weight; result.z*= total_weight;
159  result.r = static_cast<pcl::uint8_t> (r);
160  result.g = static_cast<pcl::uint8_t> (g);
161  result.b = static_cast<pcl::uint8_t> (b);
162  }
163  else
164  makeInfinite (result);
165 
166  return (result);
167 }
168 
169 ///////////////////////////////////////////////////////////////////////////////////////////////////
170 template <typename PointInT, typename PointOutT, typename KernelT>
172  : PCLBase <PointInT> ()
173  , surface_ ()
174  , tree_ ()
175  , search_radius_ (0)
176 {}
177 
178 ///////////////////////////////////////////////////////////////////////////////////////////////////
179 template <typename PointInT, typename PointOutT, typename KernelT> bool
181 {
183  {
184  PCL_ERROR ("[pcl::filters::Convlution3D::initCompute] init failed!\n");
185  return (false);
186  }
187  // Initialize the spatial locator
188  if (!tree_)
189  {
190  if (input_->isOrganized ())
191  tree_.reset (new pcl::search::OrganizedNeighbor<PointInT> ());
192  else
193  tree_.reset (new pcl::search::KdTree<PointInT> (false));
194  }
195  // If no search surface has been defined, use the input dataset as the search surface itself
196  if (!surface_)
197  surface_ = input_;
198  // Send the surface dataset to the spatial locator
199  tree_->setInputCloud (surface_);
200  // Do a fast check to see if the search parameters are well defined
201  if (search_radius_ <= 0.0)
202  {
203  PCL_ERROR ("[pcl::filters::Convlution3D::initCompute] search radius (%f) must be > 0",
204  search_radius_);
205  return (false);
206  }
207  // Make sure the provided kernel implements the required interface
208  if (dynamic_cast<ConvolvingKernel<PointInT, PointOutT>* > (&kernel_) == 0)
209  {
210  PCL_ERROR ("[pcl::filters::Convlution3D::initCompute] init failed");
211  PCL_ERROR ("kernel_ must implement ConvolvingKernel interface\n!");
212  return (false);
213  }
214  kernel_.setInputCloud (surface_);
215  // Initialize convolving kernel
216  if (!kernel_.initCompute ())
217  {
218  PCL_ERROR ("[pcl::filters::Convlution3D::initCompute] kernel initialization failed!\n");
219  return (false);
220  }
221  return (true);
222 }
223 
224 ///////////////////////////////////////////////////////////////////////////////////////////////////
225 template <typename PointInT, typename PointOutT, typename KernelT> void
227 {
228  if (!initCompute ())
229  {
230  PCL_ERROR ("[pcl::filters::Convlution3D::convolve] init failed!\n");
231  return;
232  }
233  output.resize (surface_->size ());
234  output.width = surface_->width;
235  output.height = surface_->height;
236  output.is_dense = surface_->is_dense;
237  std::vector<int> nn_indices;
238  std::vector<float> nn_distances;
239 
240 #ifdef _OPENMP
241 #pragma omp parallel for shared (output) private (nn_indices, nn_distances) num_threads (threads_)
242 #endif
243  for (std::size_t point_idx = 0; point_idx < surface_->size (); ++point_idx)
244  {
245  const PointInT& point_in = surface_->points [point_idx];
246  PointOutT& point_out = output [point_idx];
247  if (isFinite (point_in) &&
248  tree_->radiusSearch (point_in, search_radius_, nn_indices, nn_distances))
249  {
250  point_out = kernel_ (nn_indices, nn_distances);
251  }
252  else
253  {
254  kernel_.makeInfinite (point_out);
255  output.is_dense = false;
256  }
257  }
258 }
259 
260 #endif