Point Cloud Library (PCL)  1.9.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/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
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 = std::exp (-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 = std::exp (-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 ())
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",
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 (int64_t point_idx = 0; point_idx < static_cast<int64_t> (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
A point structure representing normal coordinates and the surface curvature estimate.
KdTreePtr tree_
A pointer to the spatial search object.
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:53
PointCloudInConstPtr surface_
An input point cloud describing the surface that is to be used for nearest neighbors estimation...
virtual PointOutT operator()(const std::vector< int > &indices, const std::vector< float > &distances)
Convolve point at the center of this local information.
This file defines compatibility wrappers for low level I/O functions.
Definition: convolution.h:45
KernelT kernel_
convlving kernel
A 2D point structure representing Euclidean xy coordinates.
uint32_t height
The point cloud height (if organized as an image-structure).
Definition: point_cloud.h:416
PointCloudInConstPtr input_
source cloud
uint32_t width
The point cloud width (if organized as an image-structure).
Definition: point_cloud.h:414
PCL base class.
Definition: pcl_base.h:69
double search_radius_
The nearest neighbors search radius for each point.
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.
PointCloud represents the base class in PCL for storing collections of 3D points. ...
Class ConvolvingKernel base class for all convolving kernels.
void convolve(PointCloudOut &output)
Convolve point cloud.
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:419
PointOutT operator()(const std::vector< int > &indices, const std::vector< float > &distances)
Convolve point at the center of this local information.
OrganizedNeighbor is a class for optimized nearest neigbhor search in organized point clouds...
Definition: organized.h:62
PointCloudConstPtr input_
The input point cloud dataset.
Definition: pcl_base.h:151
void resize(size_t n)
Resize the cloud.
Definition: point_cloud.h:456
A point structure representing Euclidean xyz coordinates, and the RGB color.