Merge pull request #109 from sdmiller/fix_clustering

Fixed clustering logic

search/include/pcl/search/impl/search.hpp

@@ -63,6 +63,13 @@ pcl::search::Search<PointT>::setSortedResults (bool sorted)
 {
   sorted_results_ = sorted;
 }
+
+///////////////////////////////////////////////////////////////////////////////////////////
+template <typename PointT> bool
+pcl::search::Search<PointT>::getSortedResults ()
+{
+  return (sorted_results_);
+}
 
 ///////////////////////////////////////////////////////////////////////////////////////////
 template <typename PointT> void

search/include/pcl/search/search.h

@@ -103,6 +103,13 @@ namespace pcl
           */
         virtual void 
         setSortedResults (bool sorted);
+
+        /** \brief Gets whether the results should be sorted (ascending in the distance) or not
+          * Otherwise the results may be returned in any order.
+          */
+        virtual bool 
+        getSortedResults ();
+
 
         /** \brief Pass the input dataset that the search will be performed on.
           * \param[in] cloud a const pointer to the PointCloud data

segmentation/include/pcl/segmentation/impl/extract_clusters.hpp

@@ -53,6 +53,8 @@ pcl::extractEuclideanClusters (const PointCloud<PointT> &cloud,
     PCL_ERROR ("[pcl::extractEuclideanClusters] Tree built for a different point cloud dataset (%zu) than the input cloud (%zu)!\n", tree->getInputCloud ()->points.size (), cloud.points.size ());
     return;
   }
+  // Check if the tree is sorted -- if it is we don't need to check the first element
+  int nn_start_idx = tree->getSortedResults () ? 1 : 0;
   // Create a bool vector of processed point indices, and initialize it to false
   std::vector<bool> processed (cloud.points.size (), false);
 
@@ -79,7 +81,7 @@ pcl::extractEuclideanClusters (const PointCloud<PointT> &cloud,
         continue;
       }
 
-      for (size_t j = 1; j < nn_indices.size (); ++j)             // nn_indices[0] should be sq_idx
+      for (size_t j = nn_start_idx; j < nn_indices.size (); ++j)             // can't assume sorted (default isn't!)
       {
         if (nn_indices[j] == -1 || processed[nn_indices[j]])        // Has this point been processed before ?
           continue;
@@ -132,6 +134,8 @@ pcl::extractEuclideanClusters (const PointCloud<PointT> &cloud,
     PCL_ERROR ("[pcl::extractEuclideanClusters] Tree built for a different set of indices (%zu) than the input set (%zu)!\n", tree->getIndices ()->size (), indices.size ());
     return;
   }
+  // Check if the tree is sorted -- if it is we don't need to check the first element
+  int nn_start_idx = tree->getSortedResults () ? 1 : 0;
 
   // Create a bool vector of processed point indices, and initialize it to false
   std::vector<bool> processed (cloud.points.size (), false);
@@ -165,7 +169,7 @@ pcl::extractEuclideanClusters (const PointCloud<PointT> &cloud,
         continue;
       }
 
-      for (size_t j = 1; j < nn_indices.size (); ++j)             // nn_indices[0] should be sq_idx
+      for (size_t j = nn_start_idx; j < nn_indices.size (); ++j)             // can't assume sorted (default isn't!)
       {
         if (nn_indices[j] == -1 || processed[nn_indices[j]])        // Has this point been processed before ?
           continue;