Point Cloud Library (PCL)  1.11.0
 All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends Modules Pages
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 (std::vector<int>::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 std::vector<int>& 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 (std::vector<int>::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",
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  std::vector<int> nn_indices;
242  std::vector<float> nn_distances;
243 
244 #pragma omp parallel for \
245  default(none) \
246  shared(output) \
247  private(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
A point structure representing normal coordinates and the surface curvature estimate.
search::KdTree is a wrapper class which inherits the pcl::KdTree class for performing search function...
Definition: kdtree.h:61
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
virtual PointOutT operator()(const std::vector< int > &indices, const std::vector< float > &distances)
Convolve point at the center of this local information.
std::uint32_t width
The point cloud width (if organized as an image-structure).
Definition: point_cloud.h:413
A 2D point structure representing Euclidean xy coordinates.
PCL base class.
Definition: pcl_base.h:69
static void makeInfinite(PointOutT &p)
Utility function that annihilates a point making it fail the pcl::isFinite test.
bool initCompute()
initialize computation
bool initCompute()
Must call this method before doing any computation.
Class ConvolvingKernel base class for all convolving kernels.
PointCloud represents the base class in PCL for storing collections of 3D points. ...
void convolve(PointCloudOut &output)
Convolve point cloud.
PointOutT operator()(const std::vector< int > &indices, const std::vector< float > &distances)
Convolve point at the center of this local information.
std::uint32_t height
The point cloud height (if organized as an image-structure).
Definition: point_cloud.h:415
OrganizedNeighbor is a class for optimized nearest neigbhor search in organized point clouds...
Definition: organized.h:63
A point structure representing Euclidean xyz coordinates, and the RGB color.
void resize(std::size_t n)
Resize the cloud.
Definition: point_cloud.h:455
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:418