Point Cloud Library (PCL)  1.9.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 #include <pcl/common/io.h>
43 
44 #include <vector>
45 #include <list>
46 
47 ///////////////////////////////////////////////////////////////////////////////
48 template<typename PointT, typename NormalT> bool
50 {
52  return false;
53 
54  // If sample size is 0 or if the sample size is greater then input cloud size then return entire copy of cloud
55  if (sample_ >= input_->size ())
56  {
57  PCL_ERROR ("[NormalSpaceSampling::initCompute] Requested more samples than the input cloud size: %d vs %lu\n",
58  sample_, input_->size ());
59  return false;
60  }
61 
62  boost::mt19937 rng (static_cast<unsigned int> (seed_));
63  boost::uniform_int<unsigned int> uniform_distrib (0, unsigned (input_->size ()));
64  delete rng_uniform_distribution_;
65  rng_uniform_distribution_ = new boost::variate_generator<boost::mt19937, boost::uniform_int<unsigned int> > (rng, uniform_distrib);
66 
67  return (true);
68 }
69 
70 ///////////////////////////////////////////////////////////////////////////////
71 template<typename PointT, typename NormalT> void
73 {
74  std::vector<int> indices;
75  if (keep_organized_)
76  {
77  bool temp = extract_removed_indices_;
78  extract_removed_indices_ = true;
79  applyFilter (indices);
80  extract_removed_indices_ = temp;
81 
82  output = *input_;
83  for (int rii = 0; rii < static_cast<int> (removed_indices_->size ()); ++rii) // rii = removed indices iterator
84  output.points[(*removed_indices_)[rii]].x = output.points[(*removed_indices_)[rii]].y = output.points[(*removed_indices_)[rii]].z = user_filter_value_;
85  if (!std::isfinite (user_filter_value_))
86  output.is_dense = false;
87  }
88  else
89  {
90  output.is_dense = true;
91  applyFilter (indices);
92  pcl::copyPointCloud (*input_, indices, output);
93  }
94 }
95 
96 ///////////////////////////////////////////////////////////////////////////////
97 template<typename PointT, typename NormalT> bool
99  unsigned int start_index,
100  unsigned int length)
101 {
102  bool status = true;
103  for (unsigned int i = start_index; i < start_index + length; i++)
104  {
105  status &= array.test (i);
106  }
107  return status;
108 }
109 
110 ///////////////////////////////////////////////////////////////////////////////
111 template<typename PointT, typename NormalT> unsigned int
112 pcl::NormalSpaceSampling<PointT, NormalT>::findBin (const float *normal, unsigned int)
113 {
114  unsigned int bin_number = 0;
115  // Holds the bin numbers for direction cosines in x,y,z directions
116  unsigned int t[3] = {0,0,0};
117 
118  // dcos is the direction cosine.
119  float dcos = 0.0;
120  float bin_size = 0.0;
121  // max_cos and min_cos are the maximum and minimum values of cos(theta) respectively
122  float max_cos = 1.0;
123  float min_cos = -1.0;
124 
125 // dcos = cosf (normal[0]);
126  dcos = normal[0];
127  bin_size = (max_cos - min_cos) / static_cast<float> (binsx_);
128 
129  // Finding bin number for direction cosine in x direction
130  unsigned int k = 0;
131  for (float i = min_cos; (i + bin_size) < (max_cos - bin_size); i += bin_size , k++)
132  {
133  if (dcos >= i && dcos <= (i+bin_size))
134  {
135  break;
136  }
137  }
138  t[0] = k;
139 
140 // dcos = cosf (normal[1]);
141  dcos = normal[1];
142  bin_size = (max_cos - min_cos) / static_cast<float> (binsy_);
143 
144  // Finding bin number for direction cosine in y direction
145  k = 0;
146  for (float i = min_cos; (i + bin_size) < (max_cos - bin_size); i += bin_size , k++)
147  {
148  if (dcos >= i && dcos <= (i+bin_size))
149  {
150  break;
151  }
152  }
153  t[1] = k;
154 
155 // dcos = cosf (normal[2]);
156  dcos = normal[2];
157  bin_size = (max_cos - min_cos) / static_cast<float> (binsz_);
158 
159  // Finding bin number for direction cosine in z direction
160  k = 0;
161  for (float i = min_cos; (i + bin_size) < (max_cos - bin_size); i += bin_size , k++)
162  {
163  if (dcos >= i && dcos <= (i+bin_size))
164  {
165  break;
166  }
167  }
168  t[2] = k;
169 
170  bin_number = t[0] * (binsy_*binsz_) + t[1] * binsz_ + t[2];
171  return bin_number;
172 }
173 
174 ///////////////////////////////////////////////////////////////////////////////
175 template<typename PointT, typename NormalT> void
177 {
178  if (!initCompute ())
179  {
180  indices = *indices_;
181  return;
182  }
183 
184  unsigned int max_values = (std::min) (sample_, static_cast<unsigned int> (input_normals_->size ()));
185  // Resize output indices to sample size
186  indices.resize (max_values);
187  removed_indices_->resize (max_values);
188 
189  // Allocate memory for the histogram of normals. Normals will then be sampled from each bin.
190  unsigned int n_bins = binsx_ * binsy_ * binsz_;
191  // list<int> holds the indices of points in that bin. Using list to avoid repeated resizing of vectors.
192  // Helps when the point cloud is large.
193  std::vector<std::list <int> > normals_hg;
194  normals_hg.reserve (n_bins);
195  for (unsigned int i = 0; i < n_bins; i++)
196  normals_hg.emplace_back();
197 
198  for (std::vector<int>::const_iterator it = indices_->begin (); it != indices_->end (); ++it)
199  {
200  unsigned int bin_number = findBin (input_normals_->points[*it].normal, n_bins);
201  normals_hg[bin_number].push_back (*it);
202  }
203 
204 
205  // Setting up random access for the list created above. Maintaining the iterators to individual elements of the list
206  // in a vector. Using vector now as the size of the list is known.
207  std::vector<std::vector<std::list<int>::iterator> > random_access (normals_hg.size ());
208  for (size_t i = 0; i < normals_hg.size (); i++)
209  {
210  random_access.emplace_back();
211  random_access[i].resize (normals_hg[i].size ());
212 
213  size_t j = 0;
214  for (std::list<int>::iterator itr = normals_hg[i].begin (); itr != normals_hg[i].end (); itr++, j++)
215  random_access[i][j] = itr;
216  }
217  std::vector<size_t> start_index (normals_hg.size ());
218  start_index[0] = 0;
219  size_t prev_index = 0;
220  for (size_t i = 1; i < normals_hg.size (); i++)
221  {
222  start_index[i] = prev_index + normals_hg[i-1].size ();
223  prev_index = start_index[i];
224  }
225 
226  // Maintaining flags to check if a point is sampled
227  boost::dynamic_bitset<> is_sampled_flag (input_normals_->points.size ());
228  // Maintaining flags to check if all points in the bin are sampled
229  boost::dynamic_bitset<> bin_empty_flag (normals_hg.size ());
230  unsigned int i = 0;
231  while (i < sample_)
232  {
233  // Iterating through every bin and picking one point at random, until the required number of points are sampled.
234  for (size_t j = 0; j < normals_hg.size (); j++)
235  {
236  unsigned int M = static_cast<unsigned int> (normals_hg[j].size ());
237  if (M == 0 || bin_empty_flag.test (j)) // bin_empty_flag(i) is set if all points in that bin are sampled..
238  continue;
239 
240  unsigned int pos = 0;
241  unsigned int random_index = 0;
242 
243  // Picking up a sample at random from jth bin
244  do
245  {
246  random_index = static_cast<unsigned int> ((*rng_uniform_distribution_) () % M);
247  pos = start_index[j] + random_index;
248  } while (is_sampled_flag.test (pos));
249 
250  is_sampled_flag.flip (start_index[j] + random_index);
251 
252  // Checking if all points in bin j are sampled.
253  if (isEntireBinSampled (is_sampled_flag, start_index[j], static_cast<unsigned int> (normals_hg[j].size ())))
254  bin_empty_flag.flip (j);
255 
256  unsigned int index = *(random_access[j][random_index]);
257  indices[i] = index;
258  i++;
259  if (i == sample_)
260  break;
261  }
262  }
263 
264  // If we need to return the indices that we haven't sampled
265  if (extract_removed_indices_)
266  {
267  std::vector<int> indices_temp = indices;
268  std::sort (indices_temp.begin (), indices_temp.end ());
269 
270  std::vector<int> all_indices_temp = *indices_;
271  std::sort (all_indices_temp.begin (), all_indices_temp.end ());
272  set_difference (all_indices_temp.begin (), all_indices_temp.end (),
273  indices_temp.begin (), indices_temp.end (),
274  inserter (*removed_indices_, removed_indices_->begin ()));
275  }
276 }
277 
278 #define PCL_INSTANTIATE_NormalSpaceSampling(T,NT) template class PCL_EXPORTS pcl::NormalSpaceSampling<T,NT>;
279 
280 #endif // PCL_FILTERS_IMPL_NORMAL_SPACE_SAMPLE_H_
std::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
Definition: point_cloud.h:409
PCL_EXPORTS void copyPointCloud(const pcl::PCLPointCloud2 &cloud_in, const std::vector< int > &indices, pcl::PCLPointCloud2 &cloud_out)
Extract the indices of a given point cloud as a new point cloud.
NormalSpaceSampling samples the input point cloud in the space of normal directions computed at every...
Definition: normal_space.h:51
FilterIndices represents the base class for filters that are about binary point removal.
PointCloud represents the base class in PCL for storing collections of 3D points. ...
void applyFilter(PointCloud &output) override
Sample of point indices into a separate PointCloud.
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:417