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