Point Cloud Library (PCL)  1.14.1-dev
normal_space.hpp
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Point Cloud Library (PCL) - www.pointclouds.org
5  * Copyright (c) 2012-, Open Perception, 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  */
37 
38 #ifndef PCL_FILTERS_IMPL_NORMAL_SPACE_SAMPLE_H_
39 #define PCL_FILTERS_IMPL_NORMAL_SPACE_SAMPLE_H_
40 
41 #include <pcl/filters/normal_space.h>
42 
43 #include <vector>
44 #include <list>
45 
46 ///////////////////////////////////////////////////////////////////////////////
47 template<typename PointT, typename NormalT> bool
49 {
51  return false;
52 
53  // If sample size is 0 or if the sample size is greater then input cloud size then return entire copy of cloud
54  if (sample_ >= input_->size ())
55  {
56  PCL_ERROR ("[NormalSpaceSampling::initCompute] Requested more samples than the input cloud size: %d vs %lu\n",
57  sample_, input_->size ());
58  return false;
59  }
60 
61  rng_.seed (seed_);
62  return (true);
63 }
64 
65 ///////////////////////////////////////////////////////////////////////////////
66 template<typename PointT, typename NormalT> bool
68  unsigned int start_index,
69  unsigned int length)
70 {
71  bool status = true;
72  for (unsigned int i = start_index; i < start_index + length; i++)
73  {
74  status &= array.test (i);
75  }
76  return status;
77 }
78 
79 ///////////////////////////////////////////////////////////////////////////////
80 template<typename PointT, typename NormalT> unsigned int
82 {
83  // in the case where normal[0] == 1.0f, ix will be binsx_, which is out of range.
84  // thus, use std::min to avoid this situation.
85  const auto ix = std::min (binsx_ - 1, static_cast<unsigned> (std::max (0.0f, std::floor (0.5f * (binsx_) * (normal[0] + 1.f)))));
86  const auto iy = std::min (binsy_ - 1, static_cast<unsigned> (std::max (0.0f, std::floor (0.5f * (binsy_) * (normal[1] + 1.f)))));
87  const auto iz = std::min (binsz_ - 1, static_cast<unsigned> (std::max (0.0f, std::floor (0.5f * (binsz_) * (normal[2] + 1.f)))));
88  return ix * (binsy_*binsz_) + iy * binsz_ + iz;
89 }
90 
91 ///////////////////////////////////////////////////////////////////////////////
92 template<typename PointT, typename NormalT> void
94 {
95  if (!initCompute ())
96  {
97  indices = *indices_;
98  return;
99  }
100 
101  unsigned int max_values = (std::min) (sample_, static_cast<unsigned int> (input_normals_->size ()));
102  // Resize output indices to sample size
103  indices.resize (max_values);
104  removed_indices_->resize (max_values);
105 
106  // Allocate memory for the histogram of normals. Normals will then be sampled from each bin.
107  unsigned int n_bins = binsx_ * binsy_ * binsz_;
108  // list<int> holds the indices of points in that bin. Using list to avoid repeated resizing of vectors.
109  // Helps when the point cloud is large.
110  std::vector<std::list <int> > normals_hg;
111  normals_hg.reserve (n_bins);
112  for (unsigned int i = 0; i < n_bins; i++)
113  normals_hg.emplace_back();
114 
115  for (const auto index : *indices_)
116  {
117  unsigned int bin_number = findBin ((*input_normals_)[index].normal);
118  normals_hg[bin_number].push_back (index);
119  }
120 
121 
122  // Setting up random access for the list created above. Maintaining the iterators to individual elements of the list
123  // in a vector. Using vector now as the size of the list is known.
124  std::vector<std::vector<std::list<int>::iterator> > random_access (normals_hg.size ());
125  for (std::size_t i = 0; i < normals_hg.size (); i++)
126  {
127  random_access.emplace_back();
128  random_access[i].resize (normals_hg[i].size ());
129 
130  std::size_t j = 0;
131  for (auto itr = normals_hg[i].begin (); itr != normals_hg[i].end (); ++itr, ++j)
132  random_access[i][j] = itr;
133  }
134  std::vector<std::size_t> start_index (normals_hg.size ());
135  start_index[0] = 0;
136  std::size_t prev_index = 0;
137  for (std::size_t i = 1; i < normals_hg.size (); i++)
138  {
139  start_index[i] = prev_index + normals_hg[i-1].size ();
140  prev_index = start_index[i];
141  }
142 
143  // Maintaining flags to check if a point is sampled
144  boost::dynamic_bitset<> is_sampled_flag (input_normals_->size ());
145  // Maintaining flags to check if all points in the bin are sampled
146  boost::dynamic_bitset<> bin_empty_flag (normals_hg.size ());
147  unsigned int i = 0;
148  while (i < sample_)
149  {
150  // Iterating through every bin and picking one point at random, until the required number of points are sampled.
151  for (std::size_t j = 0; j < normals_hg.size (); j++)
152  {
153  auto M = static_cast<unsigned int> (normals_hg[j].size ());
154  if (M == 0 || bin_empty_flag.test (j)) // bin_empty_flag(i) is set if all points in that bin are sampled..
155  continue;
156 
157  unsigned int pos = 0;
158  unsigned int random_index = 0;
159  std::uniform_int_distribution<unsigned> rng_uniform_distribution (0u, M - 1u);
160 
161  // Picking up a sample at random from jth bin
162  do
163  {
164  random_index = rng_uniform_distribution (rng_);
165  pos = start_index[j] + random_index;
166  } while (is_sampled_flag.test (pos));
167 
168  is_sampled_flag.flip (start_index[j] + random_index);
169 
170  // Checking if all points in bin j are sampled.
171  if (isEntireBinSampled (is_sampled_flag, start_index[j], static_cast<unsigned int> (normals_hg[j].size ())))
172  bin_empty_flag.flip (j);
173 
174  unsigned int index = *(random_access[j][random_index]);
175  indices[i] = index;
176  i++;
177  if (i == sample_)
178  break;
179  }
180  }
181 
182  // If we need to return the indices that we haven't sampled
183  if (extract_removed_indices_)
184  {
185  Indices indices_temp = indices;
186  std::sort (indices_temp.begin (), indices_temp.end ());
187 
188  Indices all_indices_temp = *indices_;
189  std::sort (all_indices_temp.begin (), all_indices_temp.end ());
190  set_difference (all_indices_temp.begin (), all_indices_temp.end (),
191  indices_temp.begin (), indices_temp.end (),
192  inserter (*removed_indices_, removed_indices_->begin ()));
193  }
194 }
195 
196 #define PCL_INSTANTIATE_NormalSpaceSampling(T,NT) template class PCL_EXPORTS pcl::NormalSpaceSampling<T,NT>;
197 
198 #endif // PCL_FILTERS_IMPL_NORMAL_SPACE_SAMPLE_H_
FilterIndices represents the base class for filters that are about binary point removal.
NormalSpaceSampling samples the input point cloud in the space of normal directions computed at every...
Definition: normal_space.h:52
void applyFilter(Indices &indices) override
Sample of point indices.
IndicesAllocator<> Indices
Type used for indices in PCL.
Definition: types.h:133