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