Removed normal related accessors and types from EuclideanClusterComparator

apps/cloud_composer/include/pcl/apps/cloud_composer/tools/impl/organized_segmentation.hpp

@@ -103,18 +103,12 @@
      std::vector<pcl::PointIndices> boundary_indices;
      mps.segmentAndRefine (regions, model_coefficients, inlier_indices, labels, label_indices, boundary_indices);
 
-     std::vector<bool> plane_labels;
-     plane_labels.resize (label_indices.size (), false);
-     for (size_t i = 0; i < label_indices.size (); i++)
-     {
-       if (label_indices[i].indices.size () > min_plane_size)
-       {
-         plane_labels[i] = true;
-       }
-     }
+     boost::shared_ptr<std::map<uint32_t, bool> > plane_labels = boost::make_shared<std::map<uint32_t, bool> > ();
+     for (size_t i = 0; i < label_indices.size (); ++i)
+       (*plane_labels)[i] = (label_indices[i].indices.size () > min_plane_size);
      typename PointCloud<PointT>::CloudVectorType clusters;
 
-     typename EuclideanClusterComparator<PointT, pcl::Normal, pcl::Label>::Ptr euclidean_cluster_comparator = boost::make_shared <EuclideanClusterComparator<PointT, pcl::Normal, pcl::Label> > ();
+     typename EuclideanClusterComparator<PointT, pcl::Label>::Ptr euclidean_cluster_comparator = boost::make_shared <EuclideanClusterComparator<PointT, pcl::Label> > ();
      euclidean_cluster_comparator->setInputCloud (input_cloud);
      euclidean_cluster_comparator->setLabels (labels);
      euclidean_cluster_comparator->setExcludeLabels (plane_labels);

apps/include/pcl/apps/organized_segmentation_demo.h

@@ -139,7 +139,7 @@ class OrganizedSegmentationDemo : public QMainWindow
     pcl::RGBPlaneCoefficientComparator<PointT, pcl::Normal>::Ptr rgb_comparator_;
     pcl::RGBPlaneCoefficientComparator<PointT, pcl::Normal> rgb_comp_;
     pcl::EdgeAwarePlaneComparator<PointT, pcl::Normal>::Ptr edge_aware_comparator_;
-    pcl::EuclideanClusterComparator<PointT, pcl::Normal, pcl::Label>::Ptr euclidean_cluster_comparator_;
+    pcl::EuclideanClusterComparator<PointT, pcl::Label>::Ptr euclidean_cluster_comparator_;
 
   public Q_SLOTS:
     void toggleCapturePressed()

apps/src/ni_linemod.cpp

@@ -249,7 +249,7 @@ class NILinemod
       exppd.segment (*points_above_plane);
 
       // Use an organized clustering segmentation to extract the individual clusters
-      EuclideanClusterComparator<PointT, Normal, Label>::Ptr euclidean_cluster_comparator (new EuclideanClusterComparator<PointT, Normal, Label>);
+      EuclideanClusterComparator<PointT, Label>::Ptr euclidean_cluster_comparator (new EuclideanClusterComparator<PointT, Label>);
       euclidean_cluster_comparator->setInputCloud (cloud);
       euclidean_cluster_comparator->setDistanceThreshold (0.03f, false);
       // Set the entire scene to false, and the inliers of the objects located on top of the plane to true
@@ -260,8 +260,9 @@ class NILinemod
         scene->points[points_above_plane->indices[i]].label = 1;
       euclidean_cluster_comparator->setLabels (scene);
 
-      vector<bool> exclude_labels (2);  exclude_labels[0] = true; exclude_labels[1] = false;
-      euclidean_cluster_comparator->setExcludeLabels (exclude_labels);
+      boost::shared_ptr<std::map<uint32_t, bool> > exclude_labels = boost::make_shared<std::map<uint32_t, bool> > ();
+      (*exclude_labels)[0] = true;
+      (*exclude_labels)[1] = false;
 
       OrganizedConnectedComponentSegmentation<PointT, Label> euclidean_segmentation (euclidean_cluster_comparator);
       euclidean_segmentation.setInputCloud (cloud);

apps/src/organized_segmentation_demo.cpp

@@ -254,7 +254,7 @@ OrganizedSegmentationDemo::OrganizedSegmentationDemo (pcl::Grabber& grabber) : g
   euclidean_comparator_.reset (new pcl::EuclideanPlaneCoefficientComparator<PointT, pcl::Normal> ());
   rgb_comparator_.reset (new pcl::RGBPlaneCoefficientComparator<PointT, pcl::Normal> ());
   edge_aware_comparator_.reset (new pcl::EdgeAwarePlaneComparator<PointT, pcl::Normal> ());
-  euclidean_cluster_comparator_ = pcl::EuclideanClusterComparator<PointT, pcl::Normal, pcl::Label>::Ptr (new pcl::EuclideanClusterComparator<PointT, pcl::Normal, pcl::Label> ());
+  euclidean_cluster_comparator_ = pcl::EuclideanClusterComparator<PointT, pcl::Label>::Ptr (new pcl::EuclideanClusterComparator<PointT, pcl::Label> ());
 
   // Set up Organized Multi Plane Segmentation
   mps.setMinInliers (10000);
@@ -309,15 +309,9 @@ OrganizedSegmentationDemo::cloud_cb (const CloudConstPtr& cloud)
 
   if (use_clustering_ && regions.size () > 0)
   {
-    std::vector<bool> plane_labels;
-    plane_labels.resize (label_indices.size (), false);
-    for (size_t i = 0; i < label_indices.size (); i++)
-    {
-      if (label_indices[i].indices.size () > 10000)
-      {
-        plane_labels[i] = true;
-      }
-    }  
+    boost::shared_ptr<std::map<uint32_t, bool> > plane_labels = boost::make_shared<std::map<uint32_t, bool> > ();
+    for (size_t i = 0; i < label_indices.size (); ++i)
+      (*plane_labels)[i] = (label_indices[i].indices.size () > 10000);
 
     euclidean_cluster_comparator_->setInputCloud (cloud);
     euclidean_cluster_comparator_->setLabels (labels);

apps/src/pcd_select_object_plane.cpp

@@ -194,7 +194,7 @@ class ObjectSelection
       if (cloud_->isOrganized ())
       {
         // Use an organized clustering segmentation to extract the individual clusters
-        typename EuclideanClusterComparator<PointT, Normal, Label>::Ptr euclidean_cluster_comparator (new EuclideanClusterComparator<PointT, Normal, Label>);
+        typename EuclideanClusterComparator<PointT, Label>::Ptr euclidean_cluster_comparator (new EuclideanClusterComparator<PointT, Label>);
         euclidean_cluster_comparator->setInputCloud (cloud);
         euclidean_cluster_comparator->setDistanceThreshold (0.03f, false);
         // Set the entire scene to false, and the inliers of the objects located on top of the plane to true
@@ -205,7 +205,9 @@ class ObjectSelection
           scene->points[points_above_plane->indices[i]].label = 1;
         euclidean_cluster_comparator->setLabels (scene);
 
-        vector<bool> exclude_labels (2);  exclude_labels[0] = true; exclude_labels[1] = false;
+        boost::shared_ptr<std::map<uint32_t, bool> > exclude_labels = boost::make_shared<std::map<uint32_t, bool> > ();
+        (*exclude_labels)[0] = true;
+        (*exclude_labels)[1] = false;
         euclidean_cluster_comparator->setExcludeLabels (exclude_labels);
 
         OrganizedConnectedComponentSegmentation<PointT, Label> euclidean_segmentation (euclidean_cluster_comparator);

apps/src/stereo_ground_segmentation.cpp

@@ -393,18 +393,11 @@ class HRCSSegmentation
         pcl::PointCloud<PointT>::CloudVectorType clusters;
         if (ground_cloud->points.size () > 0)
         {
-          std::vector<bool> plane_labels;
-          plane_labels.resize (region_indices.size (), false);
-          for (size_t i = 0; i < region_indices.size (); i++)
-          {
-            if (region_indices[i].indices.size () > mps.getMinInliers ())
-            {
-              plane_labels[i] = true;
-            }
-          }
+          boost::shared_ptr<std::map<uint32_t, bool> > plane_labels = boost::make_shared<std::map<uint32_t, bool> > ();
+          for (size_t i = 0; i < region_indices.size (); ++i)
+            (*plane_labels)[i] = (region_indices[i].indices.size () > mps.getMinInliers ());
 
-          pcl::EuclideanClusterComparator<PointT, pcl::Normal, pcl::Label>::Ptr euclidean_cluster_comparator_ (new pcl::EuclideanClusterComparator<PointT, pcl::Normal, pcl::Label> ());
-
+          pcl::EuclideanClusterComparator<PointT, pcl::Label>::Ptr euclidean_cluster_comparator_ (new pcl::EuclideanClusterComparator<PointT, pcl::Label> ());
           euclidean_cluster_comparator_->setInputCloud (cloud);
           euclidean_cluster_comparator_->setLabels (labels_ptr);
           euclidean_cluster_comparator_->setExcludeLabels (plane_labels);

segmentation/include/pcl/segmentation/euclidean_cluster_comparator.h

@@ -50,31 +50,31 @@ namespace pcl
     *
     * \author Alex Trevor
     */
-  template<typename PointT, typename PointNT, typename PointLT>
+  template<typename PointT, typename PointLT = pcl::Label>
   class EuclideanClusterComparator: public Comparator<PointT>
   {
+    protected:
+
+      using pcl::Comparator<PointT>::input_;
+
     public:
-      typedef typename Comparator<PointT>::PointCloud PointCloud;
-      typedef typename Comparator<PointT>::PointCloudConstPtr PointCloudConstPtr;
-
-      typedef typename pcl::PointCloud<PointNT> PointCloudN;
-      typedef typename PointCloudN::Ptr PointCloudNPtr;
-      typedef typename PointCloudN::ConstPtr PointCloudNConstPtr;
+      using typename Comparator<PointT>::PointCloud;
+      using typename Comparator<PointT>::PointCloudConstPtr;
 
       typedef typename pcl::PointCloud<PointLT> PointCloudL;
       typedef typename PointCloudL::Ptr PointCloudLPtr;
       typedef typename PointCloudL::ConstPtr PointCloudLConstPtr;
 
-      typedef boost::shared_ptr<EuclideanClusterComparator<PointT, PointNT, PointLT> > Ptr;
-      typedef boost::shared_ptr<const EuclideanClusterComparator<PointT, PointNT, PointLT> > ConstPtr;
+      typedef boost::shared_ptr<EuclideanClusterComparator<PointT, PointLT> > Ptr;
+      typedef boost::shared_ptr<const EuclideanClusterComparator<PointT, PointLT> > ConstPtr;
+
+      typedef std::map<uint32_t, bool> ExcludeLabelMap;
+      typedef boost::shared_ptr<ExcludeLabelMap> ExcludeLabelMapPtr;
+      typedef boost::shared_ptr<const ExcludeLabelMap> ExcludeLabelMapConstPtr;
 
-      using pcl::Comparator<PointT>::input_;
-
       /** \brief Empty constructor for EuclideanClusterComparator. */
       EuclideanClusterComparator ()
-        : normals_ ()
-        , angular_threshold_ (0.0f)
-        , distance_threshold_ (0.005f)
+        : distance_threshold_ (0.005f)
         , depth_dependent_ ()
         , z_axis_ ()
       {
@@ -94,38 +94,6 @@ namespace pcl
         z_axis_ = rot.col (2);
       }
 
-      /** \brief Provide a pointer to the input normals.
-        * \param[in] normals the input normal cloud
-        */
-      inline void
-      setInputNormals (const PointCloudNConstPtr &normals)
-      {
-        normals_ = normals;
-      }
-
-      /** \brief Get the input normals. */
-      inline PointCloudNConstPtr
-      getInputNormals () const
-      {
-        return (normals_);
-      }
-
-      /** \brief Set the tolerance in radians for difference in normal direction between neighboring points, to be considered part of the same plane.
-        * \param[in] angular_threshold the tolerance in radians
-        */
-      virtual inline void
-      setAngularThreshold (float angular_threshold)
-      {
-        angular_threshold_ = cosf (angular_threshold);
-      }
-
-      /** \brief Get the angular threshold in radians for difference in normal direction between neighboring points, to be considered part of the same plane. */
-      inline float
-      getAngularThreshold () const
-      {
-        return (acos (angular_threshold_) );
-      }
-
       /** \brief Set the tolerance in meters for difference in perpendicular distance (d component of plane equation) to the plane between neighboring points, to be considered part of the same plane.
         * \param[in] distance_threshold the tolerance in meters 
         * \param depth_dependent
@@ -148,36 +116,47 @@ namespace pcl
         * \param[in] labels The label cloud
         */
       void
-      setLabels (PointCloudLPtr& labels)
+      setLabels (const PointCloudLPtr& labels)
       {
         labels_ = labels;
       }
 
+      const ExcludeLabelMapConstPtr&
+      getExcludeLabels () const
+      {
+        return exclude_labels_;
+      }
+
       /** \brief Set labels in the label cloud to exclude.
         * \param exclude_labels a vector of bools corresponding to whether or not a given label should be considered
         */
       void
-      setExcludeLabels (std::vector<bool>& exclude_labels)
+      setExcludeLabels (const ExcludeLabelMapConstPtr &exclude_labels)
       {
-        exclude_labels_ = boost::make_shared<std::vector<bool> >(exclude_labels);
+        exclude_labels_ = exclude_labels;
       }
 
-      /** \brief Compare points at two indices by their plane equations.  True if the angle between the normals is less than the angular threshold,
-        * and the difference between the d component of the normals is less than distance threshold, else false
+      /** \brief Compare points at two indices by their euclidean distance
         * \param idx1 The first index for the comparison
         * \param idx2 The second index for the comparison
         */
       virtual bool
       compare (int idx1, int idx2) const
       {
-        int label1 = labels_->points[idx1].label;
-        int label2 = labels_->points[idx2].label;
-
-        if (label1 == -1 || label2 == -1)
-          return false;
-
-        if ( (*exclude_labels_)[label1] || (*exclude_labels_)[label2])
-          return false;
+        if (labels_ && exclude_labels_)
+        {
+          assert (labels_->size () == input_->size ());
+          const uint32_t &label1 = (*labels_)[idx1].label;
+          const uint32_t &label2 = (*labels_)[idx2].label;
+
+          const std::map<uint32_t, bool>::const_iterator it1 = exclude_labels_->find (label1);
+          if ((it1 == exclude_labels_->end ()) || it1->second)
+            return false;
+
+          const std::map<uint32_t, bool>::const_iterator it2 = exclude_labels_->find (label2);
+          if ((it2 == exclude_labels_->end ()) || it2->second)
+            return false;
+        }
 
         float dist_threshold = distance_threshold_;
         if (depth_dependent_)
@@ -187,22 +166,32 @@ namespace pcl
           dist_threshold *= z * z;
         }
 
-        float dx = input_->points[idx1].x - input_->points[idx2].x;
-        float dy = input_->points[idx1].y - input_->points[idx2].y;
-        float dz = input_->points[idx1].z - input_->points[idx2].z;
-        float dist = std::sqrt (dx*dx + dy*dy + dz*dz);
-
+        const float dist = ((*input_)[idx1].getVector3fMap ()
+                              - (*input_)[idx2].getVector3fMap ()).norm ();
         return (dist < dist_threshold);
       }
 
     protected:
-      PointCloudNConstPtr normals_;
+
+      /** \brief Set of labels with similar size as the input point cloud,
+        * aggregating points into groups based on a similar label identifier.
+        *
+        * It needs to be set in conjunction with the \ref exclude_labels_
+        * member in order to provided a masking functionality.
+        */
       PointCloudLPtr labels_;
 
-      boost::shared_ptr<std::vector<bool> > exclude_labels_;
-      float angular_threshold_;
+      /** \brief Specifies which labels should be excluded com being clustered.
+        *
+        * If a label is not specified, it's assumed by default that it's
+        * intended be excluded
+        */
+      ExcludeLabelMapConstPtr exclude_labels_;
+
       float distance_threshold_;
+
       bool depth_dependent_;
+
       Eigen::Vector3f z_axis_;
   };
 }