Point Cloud Library (PCL)  1.7.1
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_KEYPOINTS_UNIFORM_SAMPLING_IMPL_H_
39 #define PCL_KEYPOINTS_UNIFORM_SAMPLING_IMPL_H_
40 
41 #include <pcl/common/common.h>
42 #include <pcl/keypoints/uniform_sampling.h>
43 
44 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
45 template <typename PointInT> 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<PointInT>(*input_, min_p, max_p);
63 
64  // Compute the minimum and maximum bounding box values
65  min_b_[0] = static_cast<int> (floor (min_p[0] * inverse_leaf_size_[0]));
66  max_b_[0] = static_cast<int> (floor (max_p[0] * inverse_leaf_size_[0]));
67  min_b_[1] = static_cast<int> (floor (min_p[1] * inverse_leaf_size_[1]));
68  max_b_[1] = static_cast<int> (floor (max_p[1] * inverse_leaf_size_[1]));
69  min_b_[2] = static_cast<int> (floor (min_p[2] * inverse_leaf_size_[2]));
70  max_b_[2] = static_cast<int> (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  // First pass: build a set of leaves with the point index closest to the leaf center
83  for (size_t cp = 0; cp < indices_->size (); ++cp)
84  {
85  if (!input_->is_dense)
86  // Check if the point is invalid
87  if (!pcl_isfinite (input_->points[(*indices_)[cp]].x) ||
88  !pcl_isfinite (input_->points[(*indices_)[cp]].y) ||
89  !pcl_isfinite (input_->points[(*indices_)[cp]].z))
90  continue;
91 
92  Eigen::Vector4i ijk = Eigen::Vector4i::Zero ();
93  ijk[0] = static_cast<int> (floor (input_->points[(*indices_)[cp]].x * inverse_leaf_size_[0]));
94  ijk[1] = static_cast<int> (floor (input_->points[(*indices_)[cp]].y * inverse_leaf_size_[1]));
95  ijk[2] = static_cast<int> (floor (input_->points[(*indices_)[cp]].z * inverse_leaf_size_[2]));
96 
97  // Compute the leaf index
98  int idx = (ijk - min_b_).dot (divb_mul_);
99  Leaf& leaf = leaves_[idx];
100  // First time we initialize the index
101  if (leaf.idx == -1)
102  {
103  leaf.idx = (*indices_)[cp];
104  continue;
105  }
106 
107  // Check to see if this point is closer to the leaf center than the previous one we saved
108  float diff_cur = (input_->points[(*indices_)[cp]].getVector4fMap () - ijk.cast<float> ()).squaredNorm ();
109  float diff_prev = (input_->points[leaf.idx].getVector4fMap () - ijk.cast<float> ()).squaredNorm ();
110 
111  // If current point is closer, copy its index instead
112  if (diff_cur < diff_prev)
113  leaf.idx = (*indices_)[cp];
114  }
115 
116  // Second pass: go over all leaves and copy data
117  output.points.resize (leaves_.size ());
118  int cp = 0;
119 
120  for (typename boost::unordered_map<size_t, Leaf>::const_iterator it = leaves_.begin (); it != leaves_.end (); ++it)
121  output.points[cp++] = it->second.idx;
122  output.width = static_cast<uint32_t> (output.points.size ());
123 }
124 
125 #define PCL_INSTANTIATE_UniformSampling(T) template class PCL_EXPORTS pcl::UniformSampling<T>;
126 
127 #endif // PCL_KEYPOINTS_UNIFORM_SAMPLING_IMPL_H_
128