Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

NormalSpaceSampling - fix bucket assignment, remove use of raw distribution pointer, unit-test rewriting #3862

Merged
merged 3 commits into from
Apr 20, 2020
Merged

NormalSpaceSampling - fix bucket assignment, remove use of raw distribution pointer, unit-test rewriting #3862

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
f8e6275
Remove use of raw pointer from NormalSpaceSampling.
SergioRAgostinho Apr 3, 2020
f4a1f20
Fix issue in bin assignment inside NormalSpaceSampling
SergioRAgostinho Apr 4, 2020
e490001
Rewrite unit test for NormalSpaceSampling
SergioRAgostinho Apr 4, 2020
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
79 changes: 11 additions & 68 deletions filters/include/pcl/filters/impl/normal_space.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -59,11 +59,7 @@ pcl::NormalSpaceSampling<PointT, NormalT>::initCompute ()
return false;
}

boost::mt19937 rng (static_cast<unsigned int> (seed_));
boost::uniform_int<unsigned int> uniform_distrib (0, unsigned (input_->size ()));
delete rng_uniform_distribution_;
rng_uniform_distribution_ = new boost::variate_generator<boost::mt19937, boost::uniform_int<unsigned int> > (rng, uniform_distrib);

rng_.seed (seed_);
return (true);
}

Expand All @@ -83,66 +79,12 @@ pcl::NormalSpaceSampling<PointT, NormalT>::isEntireBinSampled (boost::dynamic_bi

///////////////////////////////////////////////////////////////////////////////
template<typename PointT, typename NormalT> unsigned int
pcl::NormalSpaceSampling<PointT, NormalT>::findBin (const float *normal, unsigned int)
pcl::NormalSpaceSampling<PointT, NormalT>::findBin (const float *normal)
{
unsigned int bin_number = 0;
// Holds the bin numbers for direction cosines in x,y,z directions
unsigned int t[3] = {0,0,0};

// dcos is the direction cosine.
float dcos = 0.0;
float bin_size = 0.0;
// max_cos and min_cos are the maximum and minimum values of std::cos(theta) respectively
float max_cos = 1.0;
float min_cos = -1.0;

// dcos = std::cos (normal[0]);
dcos = normal[0];
bin_size = (max_cos - min_cos) / static_cast<float> (binsx_);

// Finding bin number for direction cosine in x direction
unsigned int k = 0;
for (float i = min_cos; (i + bin_size) < (max_cos - bin_size); i += bin_size , k++)
{
if (dcos >= i && dcos <= (i+bin_size))
{
break;
}
}
t[0] = k;

// dcos = std::cos (normal[1]);
dcos = normal[1];
bin_size = (max_cos - min_cos) / static_cast<float> (binsy_);

// Finding bin number for direction cosine in y direction
k = 0;
for (float i = min_cos; (i + bin_size) < (max_cos - bin_size); i += bin_size , k++)
{
if (dcos >= i && dcos <= (i+bin_size))
{
break;
}
}
t[1] = k;

// dcos = std::cos (normal[2]);
dcos = normal[2];
bin_size = (max_cos - min_cos) / static_cast<float> (binsz_);

// Finding bin number for direction cosine in z direction
k = 0;
for (float i = min_cos; (i + bin_size) < (max_cos - bin_size); i += bin_size , k++)
{
if (dcos >= i && dcos <= (i+bin_size))
{
break;
}
}
t[2] = k;

bin_number = t[0] * (binsy_*binsz_) + t[1] * binsz_ + t[2];
return bin_number;
const unsigned ix = static_cast<unsigned> (std::round (0.5f * (binsx_ - 1.f) * (normal[0] + 1.f)));
const unsigned iy = static_cast<unsigned> (std::round (0.5f * (binsy_ - 1.f) * (normal[1] + 1.f)));
const unsigned iz = static_cast<unsigned> (std::round (0.5f * (binsz_ - 1.f) * (normal[2] + 1.f)));
return ix * (binsy_*binsz_) + iy * binsz_ + iz;
}

///////////////////////////////////////////////////////////////////////////////
Expand All @@ -169,10 +111,10 @@ pcl::NormalSpaceSampling<PointT, NormalT>::applyFilter (std::vector<int> &indice
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)
for (const auto index : *indices_)
{
unsigned int bin_number = findBin (input_normals_->points[*it].normal, n_bins);
normals_hg[bin_number].push_back (*it);
unsigned int bin_number = findBin ((*input_normals_)[index].normal);
normals_hg[bin_number].push_back (index);
}


Expand Down Expand Up @@ -213,11 +155,12 @@ pcl::NormalSpaceSampling<PointT, NormalT>::applyFilter (std::vector<int> &indice

unsigned int pos = 0;
unsigned int random_index = 0;
std::uniform_int_distribution<unsigned> rng_uniform_distribution (0u, M - 1u);

// Picking up a sample at random from jth bin
do
{
random_index = static_cast<unsigned int> ((*rng_uniform_distribution_) () % M);
random_index = rng_uniform_distribution (rng_);
pos = start_index[j] + random_index;
} while (is_sampled_flag.test (pos));

Expand Down
16 changes: 5 additions & 11 deletions filters/include/pcl/filters/normal_space.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -39,8 +39,10 @@

#include <pcl/filters/boost.h>
#include <pcl/filters/filter_indices.h>

#include <ctime>
#include <climits>
#include <random> // std::mt19937

namespace pcl
{
Expand Down Expand Up @@ -77,17 +79,10 @@ namespace pcl
, binsy_ ()
, binsz_ ()
, input_normals_ ()
, rng_uniform_distribution_ (nullptr)
{
filter_name_ = "NormalSpaceSampling";
}

/** \brief Destructor. */
~NormalSpaceSampling ()
{
delete rng_uniform_distribution_;
}

/** \brief Set number of indices to be sampled.
* \param[in] sample the number of sample indices
*/
Expand Down Expand Up @@ -176,10 +171,9 @@ namespace pcl
private:
/** \brief Finds the bin number of the input normal, returns the bin number
* \param[in] normal the input normal
* \param[in] nbins total number of bins
*/
unsigned int
findBin (const float *normal, unsigned int nbins);
findBin (const float *normal);

/** \brief Checks of the entire bin is sampled, returns true or false
* \param[out] array flag which says whether a point is sampled or not
Expand All @@ -189,8 +183,8 @@ namespace pcl
bool
isEntireBinSampled (boost::dynamic_bitset<> &array, unsigned int start_index, unsigned int length);

/** \brief Uniform random distribution. */
boost::variate_generator<boost::mt19937, boost::uniform_int<std::uint32_t> > *rng_uniform_distribution_;
/** \brief Random engine */
std::mt19937 rng_;
};
}

Expand Down
49 changes: 31 additions & 18 deletions test/filters/test_sampling.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -105,31 +105,44 @@ TEST (CovarianceSampling, Filters)
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
TEST (NormalSpaceSampling, Filters)
{
// pcl::Normal is not precompiled by default so we use PointNormal
auto cloud = pcl::make_shared<PointCloud<PointNormal>> ();
// generate 16 points (8 unique) with unit norm
cloud->reserve (16);
// ensure that the normals have unit norm
const auto value = std::sqrt(1/3.f);
for (int unique = 0; unique < 8; ++unique) {
const auto i = ((unique % 2) < 1) ? -1 : 1; // points alternate sign
const auto j = ((unique % 4) < 2) ? -1 : 1; // 2 points negative, 2 positive
const auto k = ((unique % 8) < 4) ? -1 : 1; // first 4 points negative, rest positive
for (int duplicate = 0; duplicate < 2; ++duplicate) {
cloud->emplace_back (0.f, 0.f, 0.f, i * value, j * value, k * value);
}
}

NormalSpaceSampling<PointNormal, PointNormal> normal_space_sampling;
normal_space_sampling.setInputCloud (cloud_walls_normals);
normal_space_sampling.setNormals (cloud_walls_normals);
normal_space_sampling.setBins (4, 4, 4);
normal_space_sampling.setInputCloud (cloud);
normal_space_sampling.setNormals (cloud);
normal_space_sampling.setBins (2, 2, 2);
normal_space_sampling.setSeed (0);
normal_space_sampling.setSample (static_cast<unsigned int> (cloud_walls_normals->size ()) / 4);
normal_space_sampling.setSample (8);

IndicesPtr walls_indices (new std::vector<int> ());
IndicesPtr walls_indices = pcl::make_shared<Indices> ();
normal_space_sampling.filter (*walls_indices);

CovarianceSampling<PointNormal, PointNormal> covariance_sampling;
covariance_sampling.setInputCloud (cloud_walls_normals);
covariance_sampling.setNormals (cloud_walls_normals);
covariance_sampling.setIndices (walls_indices);
covariance_sampling.setNumberOfSamples (0);
double cond_num_walls_sampled = covariance_sampling.computeConditionNumber ();

// The orientation space of the normals is divided into 2x2x2 buckets
// points are samples arbitrarily from each bucket in succession until the
// requested number of samples is met. This means we expect to see only one index
// for every two elements in the original array e.g. 0, 3, 4, 6, etc...
// if 0 is sampled, index 1 can no longer be there and so forth
std::array<std::set<index_t>, 8> buckets;
for (const auto index : *walls_indices)
buckets[index/2].insert (index);

EXPECT_NEAR (33.04893, cond_num_walls_sampled, 1e-1);

EXPECT_EQ (1412, (*walls_indices)[0]);
EXPECT_EQ (1943, (*walls_indices)[walls_indices->size () / 4]);
EXPECT_EQ (2771, (*walls_indices)[walls_indices->size () / 2]);
EXPECT_EQ (3215, (*walls_indices)[walls_indices->size () * 3 / 4]);
EXPECT_EQ (2503, (*walls_indices)[walls_indices->size () - 1]);
EXPECT_EQ (8u, walls_indices->size ());
for (const auto& bucket : buckets)
EXPECT_EQ (1u, bucket.size ());
}

//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
Expand Down