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 ());
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_ std::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
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.
NormalSpaceSampling samples the input point cloud in the space of normal directions computed at every...
FilterIndices represents the base class for filters that are about binary point removal.
PointCloud represents the base class in PCL for storing collections of 3D points. ...
void applyFilter(PointCloud &output)
Sample of point indices into a separate PointCloud.
bool is_dense
True if no points are invalid (e.g., have NaN or Inf values).