Point Cloud Library (PCL) 1.15.0
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 // 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///////////////////////////////////////////////////////////////////////////////
92template<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...
void applyFilter(Indices &indices) override
Sample of point indices.
IndicesAllocator<> Indices
Type used for indices in PCL.
Definition types.h:133