Point Cloud Library (PCL) 1.12.1
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///////////////////////////////////////////////////////////////////////////////
47template<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///////////////////////////////////////////////////////////////////////////////
66template<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///////////////////////////////////////////////////////////////////////////////
80template<typename PointT, typename NormalT> unsigned int
82{
83 const unsigned ix = static_cast<unsigned> (std::round (0.5f * (binsx_ - 1.f) * (normal[0] + 1.f)));
84 const unsigned iy = static_cast<unsigned> (std::round (0.5f * (binsy_ - 1.f) * (normal[1] + 1.f)));
85 const unsigned iz = static_cast<unsigned> (std::round (0.5f * (binsz_ - 1.f) * (normal[2] + 1.f)));
86 return ix * (binsy_*binsz_) + iy * binsz_ + iz;
87}
88
89///////////////////////////////////////////////////////////////////////////////
90template<typename PointT, typename NormalT> void
92{
93 if (!initCompute ())
94 {
95 indices = *indices_;
96 return;
97 }
98
99 unsigned int max_values = (std::min) (sample_, static_cast<unsigned int> (input_normals_->size ()));
100 // Resize output indices to sample size
101 indices.resize (max_values);
102 removed_indices_->resize (max_values);
103
104 // Allocate memory for the histogram of normals. Normals will then be sampled from each bin.
105 unsigned int n_bins = binsx_ * binsy_ * binsz_;
106 // list<int> holds the indices of points in that bin. Using list to avoid repeated resizing of vectors.
107 // Helps when the point cloud is large.
108 std::vector<std::list <int> > normals_hg;
109 normals_hg.reserve (n_bins);
110 for (unsigned int i = 0; i < n_bins; i++)
111 normals_hg.emplace_back();
112
113 for (const auto index : *indices_)
114 {
115 unsigned int bin_number = findBin ((*input_normals_)[index].normal);
116 normals_hg[bin_number].push_back (index);
117 }
118
119
120 // Setting up random access for the list created above. Maintaining the iterators to individual elements of the list
121 // in a vector. Using vector now as the size of the list is known.
122 std::vector<std::vector<std::list<int>::iterator> > random_access (normals_hg.size ());
123 for (std::size_t i = 0; i < normals_hg.size (); i++)
124 {
125 random_access.emplace_back();
126 random_access[i].resize (normals_hg[i].size ());
127
128 std::size_t j = 0;
129 for (std::list<int>::iterator itr = normals_hg[i].begin (); itr != normals_hg[i].end (); ++itr, ++j)
130 random_access[i][j] = itr;
131 }
132 std::vector<std::size_t> start_index (normals_hg.size ());
133 start_index[0] = 0;
134 std::size_t prev_index = 0;
135 for (std::size_t i = 1; i < normals_hg.size (); i++)
136 {
137 start_index[i] = prev_index + normals_hg[i-1].size ();
138 prev_index = start_index[i];
139 }
140
141 // Maintaining flags to check if a point is sampled
142 boost::dynamic_bitset<> is_sampled_flag (input_normals_->size ());
143 // Maintaining flags to check if all points in the bin are sampled
144 boost::dynamic_bitset<> bin_empty_flag (normals_hg.size ());
145 unsigned int i = 0;
146 while (i < sample_)
147 {
148 // Iterating through every bin and picking one point at random, until the required number of points are sampled.
149 for (std::size_t j = 0; j < normals_hg.size (); j++)
150 {
151 unsigned int M = static_cast<unsigned int> (normals_hg[j].size ());
152 if (M == 0 || bin_empty_flag.test (j)) // bin_empty_flag(i) is set if all points in that bin are sampled..
153 continue;
154
155 unsigned int pos = 0;
156 unsigned int random_index = 0;
157 std::uniform_int_distribution<unsigned> rng_uniform_distribution (0u, M - 1u);
158
159 // Picking up a sample at random from jth bin
160 do
161 {
162 random_index = rng_uniform_distribution (rng_);
163 pos = start_index[j] + random_index;
164 } while (is_sampled_flag.test (pos));
165
166 is_sampled_flag.flip (start_index[j] + random_index);
167
168 // Checking if all points in bin j are sampled.
169 if (isEntireBinSampled (is_sampled_flag, start_index[j], static_cast<unsigned int> (normals_hg[j].size ())))
170 bin_empty_flag.flip (j);
171
172 unsigned int index = *(random_access[j][random_index]);
173 indices[i] = index;
174 i++;
175 if (i == sample_)
176 break;
177 }
178 }
179
180 // If we need to return the indices that we haven't sampled
181 if (extract_removed_indices_)
182 {
183 Indices indices_temp = indices;
184 std::sort (indices_temp.begin (), indices_temp.end ());
185
186 Indices all_indices_temp = *indices_;
187 std::sort (all_indices_temp.begin (), all_indices_temp.end ());
188 set_difference (all_indices_temp.begin (), all_indices_temp.end (),
189 indices_temp.begin (), indices_temp.end (),
190 inserter (*removed_indices_, removed_indices_->begin ()));
191 }
192}
193
194#define PCL_INSTANTIATE_NormalSpaceSampling(T,NT) template class PCL_EXPORTS pcl::NormalSpaceSampling<T,NT>;
195
196#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