38 #ifndef PCL_FILTERS_IMPL_NORMAL_SPACE_SAMPLE_H_
39 #define PCL_FILTERS_IMPL_NORMAL_SPACE_SAMPLE_H_
41 #include <pcl/filters/normal_space.h>
42 #include <pcl/common/io.h>
48 template<
typename Po
intT,
typename NormalT>
bool
55 if (sample_ >= input_->size ())
57 PCL_ERROR (
"[NormalSpaceSampling::initCompute] Requested more samples than the input cloud size: %d vs %lu\n",
58 sample_, input_->size ());
67 template<
typename Po
intT,
typename NormalT>
bool
69 unsigned int start_index,
73 for (
unsigned int i = start_index; i < start_index + length; i++)
75 status &= array.test (i);
81 template<
typename Po
intT,
typename NormalT>
unsigned int
84 const unsigned ix =
static_cast<unsigned> (std::round (0.5f * (binsx_ - 1.f) * (normal[0] + 1.f)));
85 const unsigned iy =
static_cast<unsigned> (std::round (0.5f * (binsy_ - 1.f) * (normal[1] + 1.f)));
86 const unsigned iz =
static_cast<unsigned> (std::round (0.5f * (binsz_ - 1.f) * (normal[2] + 1.f)));
87 return ix * (binsy_*binsz_) + iy * binsz_ + iz;
91 template<
typename Po
intT,
typename NormalT>
void
100 unsigned int max_values = (std::min) (sample_, static_cast<unsigned int> (input_normals_->size ()));
102 indices.resize (max_values);
103 removed_indices_->resize (max_values);
106 unsigned int n_bins = binsx_ * binsy_ * binsz_;
109 std::vector<std::list <int> > normals_hg;
110 normals_hg.reserve (n_bins);
111 for (
unsigned int i = 0; i < n_bins; i++)
112 normals_hg.emplace_back();
114 for (
const auto index : *indices_)
116 unsigned int bin_number = findBin ((*input_normals_)[index].normal);
117 normals_hg[bin_number].push_back (index);
123 std::vector<std::vector<std::list<int>::iterator> > random_access (normals_hg.size ());
124 for (std::size_t i = 0; i < normals_hg.size (); i++)
126 random_access.emplace_back();
127 random_access[i].resize (normals_hg[i].size ());
130 for (std::list<int>::iterator itr = normals_hg[i].begin (); itr != normals_hg[i].end (); ++itr, ++j)
131 random_access[i][j] = itr;
133 std::vector<std::size_t> start_index (normals_hg.size ());
135 std::size_t prev_index = 0;
136 for (std::size_t i = 1; i < normals_hg.size (); i++)
138 start_index[i] = prev_index + normals_hg[i-1].size ();
139 prev_index = start_index[i];
143 boost::dynamic_bitset<> is_sampled_flag (input_normals_->points.size ());
145 boost::dynamic_bitset<> bin_empty_flag (normals_hg.size ());
150 for (std::size_t j = 0; j < normals_hg.size (); j++)
152 unsigned int M =
static_cast<unsigned int> (normals_hg[j].size ());
153 if (M == 0 || bin_empty_flag.test (j))
156 unsigned int pos = 0;
157 unsigned int random_index = 0;
158 std::uniform_int_distribution<unsigned> rng_uniform_distribution (0u, M - 1u);
163 random_index = rng_uniform_distribution (rng_);
164 pos = start_index[j] + random_index;
165 }
while (is_sampled_flag.test (pos));
167 is_sampled_flag.flip (start_index[j] + random_index);
170 if (isEntireBinSampled (is_sampled_flag, start_index[j], static_cast<unsigned int> (normals_hg[j].size ())))
171 bin_empty_flag.flip (j);
173 unsigned int index = *(random_access[j][random_index]);
182 if (extract_removed_indices_)
184 std::vector<int> indices_temp = indices;
185 std::sort (indices_temp.begin (), indices_temp.end ());
187 std::vector<int> all_indices_temp = *indices_;
188 std::sort (all_indices_temp.begin (), all_indices_temp.end ());
189 set_difference (all_indices_temp.begin (), all_indices_temp.end (),
190 indices_temp.begin (), indices_temp.end (),
191 inserter (*removed_indices_, removed_indices_->begin ()));
195 #define PCL_INSTANTIATE_NormalSpaceSampling(T,NT) template class PCL_EXPORTS pcl::NormalSpaceSampling<T,NT>;
197 #endif // PCL_FILTERS_IMPL_NORMAL_SPACE_SAMPLE_H_
NormalSpaceSampling samples the input point cloud in the space of normal directions computed at every...
void applyFilter(std::vector< int > &indices) override
Sample of point indices.
FilterIndices represents the base class for filters that are about binary point removal.