Point Cloud Library (PCL) 1.13.1
Loading...
Searching...
No Matches
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 auto ix = static_cast<unsigned> (std::round (0.5f * (binsx_ - 1.f) * (normal[0] + 1.f)));
84 const auto iy = static_cast<unsigned> (std::round (0.5f * (binsy_ - 1.f) * (normal[1] + 1.f)));
85 const auto 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 (auto 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 auto 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...
void applyFilter(Indices &indices) override
Sample of point indices.
IndicesAllocator<> Indices
Type used for indices in PCL.
Definition types.h:133