Point Cloud Library (PCL)  1.9.1-dev
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 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
45 template <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.points.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 (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_->points[(*indices_)[cp]].x) ||
90  !std::isfinite (input_->points[(*indices_)[cp]].y) ||
91  !std::isfinite (input_->points[(*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_->points[(*indices_)[cp]].x * inverse_leaf_size_[0]));
101  ijk[1] = static_cast<int> (std::floor (input_->points[(*indices_)[cp]].y * inverse_leaf_size_[1]));
102  ijk[2] = static_cast<int> (std::floor (input_->points[(*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_->points[(*indices_)[cp]].getVector4fMap () - ijk.cast<float> ()).squaredNorm ();
116  float diff_prev = (input_->points[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.points.resize (leaves_.size ());
134  int cp = 0;
135 
136  for (const auto& leaf : leaves_)
137  output.points[cp++] = input_->points[leaf.second.idx];
138  output.width = static_cast<uint32_t> (output.points.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 
std::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
Definition: point_cloud.h:423
void applyFilter(PointCloud &output) override
Downsample a Point Cloud using a voxelized grid approach.
uint32_t height
The point cloud height (if organized as an image-structure).
Definition: point_cloud.h:428
Define standard C methods and C++ classes that are common to all methods.
uint32_t width
The point cloud width (if organized as an image-structure).
Definition: point_cloud.h:426
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)...
Definition: point_cloud.h:431
Simple structure to hold an nD centroid and the number of points in a leaf.