Point Cloud Library (PCL)  1.12.1-dev
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/search/organized.h>
44 #include <pcl/search/kdtree.h>
45 #include <pcl/pcl_config.h>
46 #include <pcl/point_types.h>
47 
48 #include <cmath>
49 #include <cstdint>
50 #include <limits>
51 #include <vector>
52 
53 ///////////////////////////////////////////////////////////////////////////////////////////////////
54 namespace pcl
55 {
56  namespace filters
57  {
58  template <typename PointT>
60  {
61  void
63  {
64  n.normal_x = n.normal_y = n.normal_z = std::numeric_limits<float>::quiet_NaN ();
65  }
66  };
67 
68  template <typename PointT> class
70  {
71  void
72  makeInfinite (pcl::PointXY& p)
73  {
74  p.x = p.y = std::numeric_limits<float>::quiet_NaN ();
75  }
76  };
77  }
78 }
79 
80 ///////////////////////////////////////////////////////////////////////////////////////////////////
81 template<typename PointInT, typename PointOutT> bool
83 {
84  if (sigma_ == 0)
85  {
86  PCL_ERROR ("Sigma is not set or equal to 0!\n", sigma_);
87  return (false);
88  }
89  sigma_sqr_ = sigma_ * sigma_;
90 
91  if (sigma_coefficient_)
92  {
93  if ((*sigma_coefficient_) > 6 || (*sigma_coefficient_) < 3)
94  {
95  PCL_ERROR ("Sigma coefficient (%f) out of [3..6]!\n", (*sigma_coefficient_));
96  return (false);
97  }
98  else
99  threshold_ = (*sigma_coefficient_) * (*sigma_coefficient_) * sigma_sqr_;
100  }
101 
102  return (true);
103 }
104 
105 ///////////////////////////////////////////////////////////////////////////////////////////////////
106 template<typename PointInT, typename PointOutT> PointOutT
108  const std::vector<float>& distances)
109 {
110  using namespace pcl::common;
111  PointOutT result;
112  float total_weight = 0;
113  std::vector<float>::const_iterator dist_it = distances.begin ();
114 
115  for (Indices::const_iterator idx_it = indices.begin ();
116  idx_it != indices.end ();
117  ++idx_it, ++dist_it)
118  {
119  if (*dist_it <= threshold_ && isFinite ((*input_) [*idx_it]))
120  {
121  float weight = std::exp (-0.5f * (*dist_it) / sigma_sqr_);
122  result += weight * (*input_) [*idx_it];
123  total_weight += weight;
124  }
125  }
126  if (total_weight != 0)
127  result /= total_weight;
128  else
129  makeInfinite (result);
130 
131  return (result);
132 }
133 
134 ///////////////////////////////////////////////////////////////////////////////////////////////////////
135 template<typename PointInT, typename PointOutT> PointOutT
136 pcl::filters::GaussianKernelRGB<PointInT, PointOutT>::operator() (const Indices& indices, const std::vector<float>& distances)
137 {
138  using namespace pcl::common;
139  PointOutT result;
140  float total_weight = 0;
141  float r = 0, g = 0, b = 0;
142  std::vector<float>::const_iterator dist_it = distances.begin ();
143 
144  for (Indices::const_iterator idx_it = indices.begin ();
145  idx_it != indices.end ();
146  ++idx_it, ++dist_it)
147  {
148  if (*dist_it <= threshold_ && isFinite ((*input_) [*idx_it]))
149  {
150  float weight = std::exp (-0.5f * (*dist_it) / sigma_sqr_);
151  result.x += weight * (*input_) [*idx_it].x;
152  result.y += weight * (*input_) [*idx_it].y;
153  result.z += weight * (*input_) [*idx_it].z;
154  r += weight * static_cast<float> ((*input_) [*idx_it].r);
155  g += weight * static_cast<float> ((*input_) [*idx_it].g);
156  b += weight * static_cast<float> ((*input_) [*idx_it].b);
157  total_weight += weight;
158  }
159  }
160  if (total_weight != 0)
161  {
162  total_weight = 1.f/total_weight;
163  r*= total_weight; g*= total_weight; b*= total_weight;
164  result.x*= total_weight; result.y*= total_weight; result.z*= total_weight;
165  result.r = static_cast<std::uint8_t> (r);
166  result.g = static_cast<std::uint8_t> (g);
167  result.b = static_cast<std::uint8_t> (b);
168  }
169  else
170  makeInfinite (result);
171 
172  return (result);
173 }
174 
175 ///////////////////////////////////////////////////////////////////////////////////////////////////
176 template <typename PointInT, typename PointOutT, typename KernelT>
178  : PCLBase <PointInT> ()
179  , surface_ ()
180  , tree_ ()
181  , search_radius_ (0)
182 {}
183 
184 ///////////////////////////////////////////////////////////////////////////////////////////////////
185 template <typename PointInT, typename PointOutT, typename KernelT> bool
187 {
189  {
190  PCL_ERROR ("[pcl::filters::Convlution3D::initCompute] init failed!\n");
191  return (false);
192  }
193  // Initialize the spatial locator
194  if (!tree_)
195  {
196  if (input_->isOrganized ())
197  tree_.reset (new pcl::search::OrganizedNeighbor<PointInT> ());
198  else
199  tree_.reset (new pcl::search::KdTree<PointInT> (false));
200  }
201  // If no search surface has been defined, use the input dataset as the search surface itself
202  if (!surface_)
203  surface_ = input_;
204  // Send the surface dataset to the spatial locator
205  tree_->setInputCloud (surface_);
206  // Do a fast check to see if the search parameters are well defined
207  if (search_radius_ <= 0.0)
208  {
209  PCL_ERROR ("[pcl::filters::Convlution3D::initCompute] search radius (%f) must be > 0\n",
210  search_radius_);
211  return (false);
212  }
213  // Make sure the provided kernel implements the required interface
214  if (dynamic_cast<ConvolvingKernel<PointInT, PointOutT>* > (&kernel_) == 0)
215  {
216  PCL_ERROR ("[pcl::filters::Convlution3D::initCompute] init failed : ");
217  PCL_ERROR ("kernel_ must implement ConvolvingKernel interface\n!");
218  return (false);
219  }
220  kernel_.setInputCloud (surface_);
221  // Initialize convolving kernel
222  if (!kernel_.initCompute ())
223  {
224  PCL_ERROR ("[pcl::filters::Convlution3D::initCompute] kernel initialization failed!\n");
225  return (false);
226  }
227  return (true);
228 }
229 
230 ///////////////////////////////////////////////////////////////////////////////////////////////////
231 template <typename PointInT, typename PointOutT, typename KernelT> void
233 {
234  if (!initCompute ())
235  {
236  PCL_ERROR ("[pcl::filters::Convlution3D::convolve] init failed!\n");
237  return;
238  }
239  output.resize (surface_->size ());
240  output.width = surface_->width;
241  output.height = surface_->height;
242  output.is_dense = surface_->is_dense;
243  Indices nn_indices;
244  std::vector<float> nn_distances;
245 
246 #pragma omp parallel for \
247  default(none) \
248  shared(output) \
249  firstprivate(nn_indices, nn_distances) \
250  num_threads(threads_)
251  for (std::int64_t point_idx = 0; point_idx < static_cast<std::int64_t> (surface_->size ()); ++point_idx)
252  {
253  const PointInT& point_in = surface_->points [point_idx];
254  PointOutT& point_out = output [point_idx];
255  if (isFinite (point_in) &&
256  tree_->radiusSearch (point_in, search_radius_, nn_indices, nn_distances))
257  {
258  point_out = kernel_ (nn_indices, nn_distances);
259  }
260  else
261  {
262  kernel_.makeInfinite (point_out);
263  output.is_dense = false;
264  }
265  }
266 }
267 
268 #endif
PCL base class.
Definition: pcl_base.h:70
PointCloud represents the base class in PCL for storing collections of 3D points.
Definition: point_cloud.h:173
bool is_dense
True if no points are invalid (e.g., have NaN or Inf values in any of their floating point fields).
Definition: point_cloud.h:403
void resize(std::size_t count)
Resizes the container to contain count elements.
Definition: point_cloud.h:462
std::uint32_t width
The point cloud width (if organized as an image-structure).
Definition: point_cloud.h:398
std::uint32_t height
The point cloud height (if organized as an image-structure).
Definition: point_cloud.h:400
bool initCompute()
initialize computation
void convolve(PointCloudOut &output)
Convolve point cloud.
Class ConvolvingKernel base class for all convolving kernels.
static void makeInfinite(PointOutT &p)
Utility function that annihilates a point making it fail the pcl::isFinite test.
virtual PointOutT operator()(const Indices &indices, const std::vector< float > &distances)
Convolve point at the center of this local information.
bool initCompute()
Must call this method before doing any computation.
PointOutT operator()(const Indices &indices, const std::vector< float > &distances)
Convolve point at the center of this local information.
search::KdTree is a wrapper class which inherits the pcl::KdTree class for performing search function...
Definition: kdtree.h:62
OrganizedNeighbor is a class for optimized nearest neigbhor search in organized point clouds.
Definition: organized.h:61
Defines all the PCL implemented PointT point type structures.
bool isFinite(const PointT &pt)
Tests if the 3D components of a point are all finite param[in] pt point to be tested return true if f...
Definition: point_tests.h:55
IndicesAllocator<> Indices
Type used for indices in PCL.
Definition: types.h:133
A point structure representing normal coordinates and the surface curvature estimate.
A 2D point structure representing Euclidean xy coordinates.
A point structure representing Euclidean xyz coordinates, and the RGB color.