Merge pull request #3862 from SergioRAgostinho/normalspace

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

Author: SergioRAgostinho

filters/include/pcl/filters/impl/normal_space.hpp

@@ -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);
 }
 
@@ -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;
 }
 
 ///////////////////////////////////////////////////////////////////////////////
@@ -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);
   }
 
 
@@ -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));
 

filters/include/pcl/filters/normal_space.h

@@ -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
 {
@@ -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
         */
@@ -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
@@ -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_;
   };
 }
 

test/filters/test_sampling.cpp

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