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 ())); } }