template<typename PointT, typename NormalT> void
pcl::NormalSpaceSampling<PointT, NormalT>::applyFilter (PointCloud &output)
{
// If sample size is 0 or if the sample size is greater then input cloud size
// then return entire copy of cloud
if (sample_ >= input_->size ())
{
output = *input_;
return;
}
// Resize output cloud to sample size
output.points.resize (sample_);
output.width = sample_;
output.height = 1;
// allocate memory for the histogram of normals.. Normals will then be sampled from each bin..
unsigned int n_bins = binsx_ * binsy_ * binsz_;
// list<int> holds the indices of points in that bin.. Using list to avoid repeated resizing of vectors.. Helps when the point cloud is
// large..
std::vector< std::list <int> > normals_hg;
normals_hg.reserve (n_bins);
for (unsigned int i = 0; i < n_bins; i++)
normals_hg.push_back (std::list<int> ());
for (unsigned int i = 0; i < input_normals_->points.size (); i++)
{
unsigned int bin_number = findBin (input_normals_->points[i].normal, n_bins);
normals_hg[bin_number].push_back (i);
}
// Setting up random access for the list created above.. Maintaining the iterators to individual elements of the list in a vector.. Using
// vector now as the size of the list is known..
std::vector< std::vector <std::list<int>::iterator> > random_access (normals_hg.size ());
for (unsigned int i = 0; i < normals_hg.size (); i++)
{
random_access.push_back (std::vector< std::list<int>::iterator> ());
random_access[i].resize (normals_hg[i].size ());
unsigned int j=0;
for (std::list<int>::iterator itr = normals_hg[i].begin (); itr != normals_hg[i].end (); itr++, j++)
{
random_access[i][j] = itr;
}
}
unsigned int* start_index = new unsigned int[normals_hg.size ()];
start_index[0] = 0;
unsigned int prev_index = start_index[0];
for (unsigned int i = 1; i < normals_hg.size (); i++)
{
start_index[i] = prev_index + normals_hg[i-1].size ();
prev_index = start_index[i];
}
// maintaining flags to check if a point is sampled
boost::dynamic_bitset<> is_sampled_flag (input_normals_->points.size ());
// maintaining flags to check if all points in the bin are sampled
boost::dynamic_bitset<> bin_empty_flag (normals_hg.size ());
unsigned int i = 0;
while (i < sample_)
{
// iterating through every bin and picking one point at random, until the required number of points are sampled..
for (unsigned int j = 0; j < normals_hg.size (); j++)
{
unsigned int M = normals_hg[j].size ();
if (M == 0 || bin_empty_flag.test (j))// bin_empty_flag(i) is set if all points in that bin are sampled..
{
continue;
}
unsigned int pos = 0;
unsigned int random_index = 0;
//picking up a sample at random from jth bin
do
{
random_index = std::rand () % M;
pos = start_index[j] + random_index;
} while (is_sampled_flag.test (pos));
is_sampled_flag.flip (start_index[j] + random_index);
// checking if all points in bin j are sampled..
if (isEntireBinSampled (is_sampled_flag, start_index[j], normals_hg[j].size ()))
{
bin_empty_flag.flip (j);
}
unsigned int index = *(random_access[j][random_index]);
output.points[i] = input_->points[index];
i++;
if (i == sample_)
{
break;
}
}
}
delete[] start_index;
}
template<typename PointT, typename NormalT> void
pcl::NormalSpaceSampling<PointT, NormalT>::applyFilter (std::vector<int> &indices)
{
if (!initCompute ())
{
indices = *indices_;
return;
}
unsigned int max_values = (std::min) (sample_, static_cast<unsigned int> (input_normals_->size ()));
// Resize output indices to sample size
indices.resize (max_values);
removed_indices_->resize (max_values);
// Allocate memory for the histogram of normals. Normals will then be sampled from each bin.
unsigned int n_bins = binsx_ * binsy_ * binsz_;
// list<int> holds the indices of points in that bin. Using list to avoid repeated resizing of vectors.
// Helps when the point cloud is large.
std::vector<std::list <int> > normals_hg;
normals_hg.reserve (n_bins);
for (unsigned int i = 0; i < n_bins; i++)
normals_hg.emplace_back();
for (std::vector<int>::const_iterator it = indices_->begin (); it != indices_->end (); ++it)
{
unsigned int bin_number = findBin (input_normals_->points[*it].normal, n_bins);
normals_hg[bin_number].push_back (*it);
}
// Setting up random access for the list created above. Maintaining the iterators to individual elements of the list
// in a vector. Using vector now as the size of the list is known.
std::vector<std::vector<std::list<int>::iterator> > random_access (normals_hg.size ());
for (size_t i = 0; i < normals_hg.size (); i++)
{
random_access.emplace_back();
random_access[i].resize (normals_hg[i].size ());
size_t j = 0;
for (std::list<int>::iterator itr = normals_hg[i].begin (); itr != normals_hg[i].end (); itr++, j++)
random_access[i][j] = itr;
}
std::vector<size_t> start_index (normals_hg.size ());
start_index[0] = 0;
size_t prev_index = 0;
for (size_t i = 1; i < normals_hg.size (); i++)
{
start_index[i] = prev_index + normals_hg[i-1].size ();
prev_index = start_index[i];
}
// Maintaining flags to check if a point is sampled
boost::dynamic_bitset<> is_sampled_flag (input_normals_->points.size ());
// Maintaining flags to check if all points in the bin are sampled
boost::dynamic_bitset<> bin_empty_flag (normals_hg.size ());
unsigned int i = 0;
while (i < sample_)
{
// Iterating through every bin and picking one point at random, until the required number of points are sampled.
for (size_t j = 0; j < normals_hg.size (); j++)
{
unsigned int M = static_cast<unsigned int> (normals_hg[j].size ());
if (M == 0 || bin_empty_flag.test (j)) // bin_empty_flag(i) is set if all points in that bin are sampled..
continue;
unsigned int pos = 0;
unsigned int random_index = 0;
// Picking up a sample at random from jth bin
do
{
random_index = static_cast<unsigned int> ((*rng_uniform_distribution_) () % M);
pos = start_index[j] + random_index;
} while (is_sampled_flag.test (pos));
is_sampled_flag.flip (start_index[j] + random_index);
// Checking if all points in bin j are sampled.
if (isEntireBinSampled (is_sampled_flag, start_index[j], static_cast<unsigned int> (normals_hg[j].size ())))
bin_empty_flag.flip (j);
unsigned int index = *(random_access[j][random_index]);
indices[i] = index;
i++;
if (i == sample_)
break;
}
}
// If we need to return the indices that we haven't sampled
if (extract_removed_indices_)
{
std::vector<int> indices_temp = indices;
std::sort (indices_temp.begin (), indices_temp.end ());
std::vector<int> all_indices_temp = *indices_;
std::sort (all_indices_temp.begin (), all_indices_temp.end ());
set_difference (all_indices_temp.begin (), all_indices_temp.end (),
indices_temp.begin (), indices_temp.end (),
inserter (*removed_indices_, removed_indices_->begin ()));
}
}
