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 %zu\n",
58 sample_, input_->size ());
62 boost::mt19937 rng (static_cast<unsigned int> (seed_));
63 boost::uniform_int<unsigned int> uniform_distrib (0,
unsigned (input_->size ()));
64 if (rng_uniform_distribution_ != NULL)
65 delete rng_uniform_distribution_;
66 rng_uniform_distribution_ =
new boost::variate_generator<boost::mt19937, boost::uniform_int<unsigned int> > (rng, uniform_distrib);
72 template<
typename Po
intT,
typename NormalT>
void
75 std::vector<int> indices;
78 bool temp = extract_removed_indices_;
79 extract_removed_indices_ =
true;
80 applyFilter (indices);
81 extract_removed_indices_ = temp;
84 for (
int rii = 0; rii < static_cast<int> (removed_indices_->size ()); ++rii)
85 output.
points[(*removed_indices_)[rii]].x = output.
points[(*removed_indices_)[rii]].y = output.
points[(*removed_indices_)[rii]].z = user_filter_value_;
86 if (!pcl_isfinite (user_filter_value_))
92 applyFilter (indices);
98 template<
typename Po
intT,
typename NormalT>
bool
100 unsigned int start_index,
104 for (
unsigned int i = start_index; i < start_index + length; i++)
106 status = status & array.test (i);
112 template<
typename Po
intT,
typename NormalT>
unsigned int
115 unsigned int bin_number = 0;
117 unsigned int t[3] = {0,0,0};
121 float bin_size = 0.0;
124 float min_cos = -1.0;
128 bin_size = (max_cos - min_cos) / static_cast<float> (binsx_);
132 for (
float i = min_cos; (i + bin_size) < (max_cos - bin_size); i += bin_size , k++)
134 if (dcos >= i && dcos <= (i+bin_size))
143 bin_size = (max_cos - min_cos) / static_cast<float> (binsy_);
147 for (
float i = min_cos; (i + bin_size) < (max_cos - bin_size); i += bin_size , k++)
149 if (dcos >= i && dcos <= (i+bin_size))
158 bin_size = (max_cos - min_cos) / static_cast<float> (binsz_);
162 for (
float i = min_cos; (i + bin_size) < (max_cos - bin_size); i += bin_size , k++)
164 if (dcos >= i && dcos <= (i+bin_size))
171 bin_number = t[0] * (binsy_*binsz_) + t[1] * binsz_ + t[2];
176 template<
typename Po
intT,
typename NormalT>
void
185 unsigned int max_values = (std::min) (sample_, static_cast<unsigned int> (input_normals_->size ()));
187 indices.resize (max_values);
188 removed_indices_->resize (max_values);
191 unsigned int n_bins = binsx_ * binsy_ * binsz_;
194 std::vector<std::list <int> > normals_hg;
195 normals_hg.reserve (n_bins);
196 for (
unsigned int i = 0; i < n_bins; i++)
197 normals_hg.push_back (std::list<int> ());
199 for (std::vector<int>::const_iterator it = indices_->begin (); it != indices_->end (); ++it)
201 unsigned int bin_number = findBin (input_normals_->points[*it].normal, n_bins);
202 normals_hg[bin_number].push_back (*it);
208 std::vector<std::vector<std::list<int>::iterator> > random_access (normals_hg.size ());
209 for (
unsigned int i = 0; i < normals_hg.size (); i++)
211 random_access.push_back (std::vector<std::list<int>::iterator> ());
212 random_access[i].resize (normals_hg[i].size ());
215 for (std::list<int>::iterator itr = normals_hg[i].begin (); itr != normals_hg[i].end (); itr++, j++)
216 random_access[i][j] = itr;
218 std::vector<unsigned int> start_index (normals_hg.size ());
220 unsigned int prev_index = start_index[0];
221 for (
unsigned int i = 1; i < normals_hg.size (); i++)
223 start_index[i] = prev_index +
static_cast<unsigned int> (normals_hg[i-1].size ());
224 prev_index = start_index[i];
228 boost::dynamic_bitset<> is_sampled_flag (input_normals_->points.size ());
230 boost::dynamic_bitset<> bin_empty_flag (normals_hg.size ());
235 for (
unsigned int j = 0; j < normals_hg.size (); j++)
237 unsigned int M =
static_cast<unsigned int> (normals_hg[j].size ());
238 if (M == 0 || bin_empty_flag.test (j))
241 unsigned int pos = 0;
242 unsigned int random_index = 0;
247 random_index =
static_cast<unsigned int> ((*rng_uniform_distribution_) () % M);
248 pos = start_index[j] + random_index;
249 }
while (is_sampled_flag.test (pos));
251 is_sampled_flag.flip (start_index[j] + random_index);
254 if (isEntireBinSampled (is_sampled_flag, start_index[j], static_cast<unsigned int> (normals_hg[j].size ())))
255 bin_empty_flag.flip (j);
257 unsigned int index = *(random_access[j][random_index]);
266 if (extract_removed_indices_)
268 std::vector<int> indices_temp = indices;
269 std::sort (indices_temp.begin (), indices_temp.end ());
271 std::vector<int> all_indices_temp = *indices_;
272 std::sort (all_indices_temp.begin (), all_indices_temp.end ());
273 set_difference (all_indices_temp.begin (), all_indices_temp.end (),
274 indices_temp.begin (), indices_temp.end (),
275 inserter (*removed_indices_, removed_indices_->begin ()));
279 #define PCL_INSTANTIATE_NormalSpaceSampling(T,NT) template class PCL_EXPORTS pcl::NormalSpaceSampling<T,NT>;
281 #endif // PCL_FILTERS_IMPL_NORMAL_SPACE_SAMPLE_H_
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.
FilterIndices represents the base class for filters that are about binary point removal.
bool is_dense
True if no points are invalid (e.g., have NaN or Inf values).
NormalSpaceSampling samples the input point cloud in the space of normal directions computed at every...
std::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
void applyFilter(PointCloud &output)
Sample of point indices into a separate PointCloud.