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