Point Cloud Library (PCL) 1.12.0
Loading...
Searching...
No Matches
uniform_sampling.hpp
1/*
2 * Software License Agreement (BSD License)
3 *
4 * Copyright (c) 2009, Willow Garage, Inc.
5 * All rights reserved.
6 *
7 * Redistribution and use in source and binary forms, with or without
8 * modification, are permitted provided that the following conditions
9 * are met:
10 *
11 * * Redistributions of source code must retain the above copyright
12 * notice, this list of conditions and the following disclaimer.
13 * * Redistributions in binary form must reproduce the above
14 * copyright notice, this list of conditions and the following
15 * disclaimer in the documentation and/or other materials provided
16 * with the distribution.
17 * * Neither the name of Willow Garage, Inc. nor the names of its
18 * contributors may be used to endorse or promote products derived
19 * from this software without specific prior written permission.
20 *
21 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32 * POSSIBILITY OF SUCH DAMAGE.
33 *
34 * $Id$
35 *
36 */
37
38#ifndef PCL_FILTERS_UNIFORM_SAMPLING_IMPL_H_
39#define PCL_FILTERS_UNIFORM_SAMPLING_IMPL_H_
40
41#include <pcl/common/common.h>
42#include <pcl/filters/uniform_sampling.h>
43
44//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
45template <typename PointT> void
47{
48 // Has the input dataset been set already?
49 if (!input_)
50 {
51 PCL_WARN ("[pcl::%s::detectKeypoints] No input dataset given!\n", getClassName ().c_str ());
52 output.width = output.height = 0;
53 output.clear ();
54 return;
55 }
56
57 output.height = 1; // downsampling breaks the organized structure
58 output.is_dense = true; // we filter out invalid points
59
60 Eigen::Vector4f min_p, max_p;
61 // Get the minimum and maximum dimensions
62 pcl::getMinMax3D<PointT>(*input_, min_p, max_p);
63
64 // Compute the minimum and maximum bounding box values
65 min_b_[0] = static_cast<int> (std::floor (min_p[0] * inverse_leaf_size_[0]));
66 max_b_[0] = static_cast<int> (std::floor (max_p[0] * inverse_leaf_size_[0]));
67 min_b_[1] = static_cast<int> (std::floor (min_p[1] * inverse_leaf_size_[1]));
68 max_b_[1] = static_cast<int> (std::floor (max_p[1] * inverse_leaf_size_[1]));
69 min_b_[2] = static_cast<int> (std::floor (min_p[2] * inverse_leaf_size_[2]));
70 max_b_[2] = static_cast<int> (std::floor (max_p[2] * inverse_leaf_size_[2]));
71
72 // Compute the number of divisions needed along all axis
73 div_b_ = max_b_ - min_b_ + Eigen::Vector4i::Ones ();
74 div_b_[3] = 0;
75
76 // Clear the leaves
77 leaves_.clear ();
78
79 // Set up the division multiplier
80 divb_mul_ = Eigen::Vector4i (1, div_b_[0], div_b_[0] * div_b_[1], 0);
81
82 removed_indices_->clear();
83 // First pass: build a set of leaves with the point index closest to the leaf center
84 for (std::size_t cp = 0; cp < indices_->size (); ++cp)
85 {
86 if (!input_->is_dense)
87 {
88 // Check if the point is invalid
89 if (!std::isfinite ((*input_)[(*indices_)[cp]].x) ||
90 !std::isfinite ((*input_)[(*indices_)[cp]].y) ||
91 !std::isfinite ((*input_)[(*indices_)[cp]].z))
92 {
93 if (extract_removed_indices_)
94 removed_indices_->push_back ((*indices_)[cp]);
95 continue;
96 }
97 }
98
99 Eigen::Vector4i ijk = Eigen::Vector4i::Zero ();
100 ijk[0] = static_cast<int> (std::floor ((*input_)[(*indices_)[cp]].x * inverse_leaf_size_[0]));
101 ijk[1] = static_cast<int> (std::floor ((*input_)[(*indices_)[cp]].y * inverse_leaf_size_[1]));
102 ijk[2] = static_cast<int> (std::floor ((*input_)[(*indices_)[cp]].z * inverse_leaf_size_[2]));
103
104 // Compute the leaf index
105 int idx = (ijk - min_b_).dot (divb_mul_);
106 Leaf& leaf = leaves_[idx];
107 // First time we initialize the index
108 if (leaf.idx == -1)
109 {
110 leaf.idx = (*indices_)[cp];
111 continue;
112 }
113
114 // Check to see if this point is closer to the leaf center than the previous one we saved
115 float diff_cur = ((*input_)[(*indices_)[cp]].getVector4fMap () - ijk.cast<float> ()).squaredNorm ();
116 float diff_prev = ((*input_)[leaf.idx].getVector4fMap () - ijk.cast<float> ()).squaredNorm ();
117
118 // If current point is closer, copy its index instead
119 if (diff_cur < diff_prev)
120 {
121 if (extract_removed_indices_)
122 removed_indices_->push_back (leaf.idx);
123 leaf.idx = (*indices_)[cp];
124 }
125 else
126 {
127 if (extract_removed_indices_)
128 removed_indices_->push_back ((*indices_)[cp]);
129 }
130 }
131
132 // Second pass: go over all leaves and copy data
133 output.resize (leaves_.size ());
134 int cp = 0;
135
136 for (const auto& leaf : leaves_)
137 output[cp++] = (*input_)[leaf.second.idx];
138 output.width = output.size ();
139}
140
141#define PCL_INSTANTIATE_UniformSampling(T) template class PCL_EXPORTS pcl::UniformSampling<T>;
142
143#endif // PCL_FILTERS_UNIFORM_SAMPLING_IMPL_H_
144
void applyFilter(PointCloud &output) override
Downsample a Point Cloud using a voxelized grid approach.
Define standard C methods and C++ classes that are common to all methods.
Simple structure to hold an nD centroid and the number of points in a leaf.