From f171ad417fa3687cdb58eac6af27c2059a01f275 Mon Sep 17 00:00:00 2001 From: Sergio Agostinho Date: Mon, 10 Jul 2017 15:21:46 +0100 Subject: [PATCH 1/3] Removed normal related accessors and members from EuclideanClusterComparator Fixes #1514. The functionality intended for the EuclideanClusterComparator appears to be clustering based on Euclidean distance only, hence all normal pertaining information is misleading. --- .../tools/impl/organized_segmentation.hpp | 2 +- .../pcl/apps/organized_segmentation_demo.h | 2 +- apps/src/ni_linemod.cpp | 2 +- apps/src/organized_segmentation_demo.cpp | 2 +- apps/src/pcd_select_object_plane.cpp | 2 +- apps/src/stereo_ground_segmentation.cpp | 3 +- .../euclidean_cluster_comparator.h | 52 ++----------------- 7 files changed, 11 insertions(+), 54 deletions(-) diff --git a/apps/cloud_composer/include/pcl/apps/cloud_composer/tools/impl/organized_segmentation.hpp b/apps/cloud_composer/include/pcl/apps/cloud_composer/tools/impl/organized_segmentation.hpp index 50295f7b9c3..1855fb2d3b5 100644 --- a/apps/cloud_composer/include/pcl/apps/cloud_composer/tools/impl/organized_segmentation.hpp +++ b/apps/cloud_composer/include/pcl/apps/cloud_composer/tools/impl/organized_segmentation.hpp @@ -114,7 +114,7 @@ } typename PointCloud::CloudVectorType clusters; - typename EuclideanClusterComparator::Ptr euclidean_cluster_comparator = boost::make_shared > (); + typename EuclideanClusterComparator::Ptr euclidean_cluster_comparator = boost::make_shared > (); euclidean_cluster_comparator->setInputCloud (input_cloud); euclidean_cluster_comparator->setLabels (labels); euclidean_cluster_comparator->setExcludeLabels (plane_labels); diff --git a/apps/include/pcl/apps/organized_segmentation_demo.h b/apps/include/pcl/apps/organized_segmentation_demo.h index d2c11796849..2d873a347ab 100644 --- a/apps/include/pcl/apps/organized_segmentation_demo.h +++ b/apps/include/pcl/apps/organized_segmentation_demo.h @@ -139,7 +139,7 @@ class OrganizedSegmentationDemo : public QMainWindow pcl::RGBPlaneCoefficientComparator::Ptr rgb_comparator_; pcl::RGBPlaneCoefficientComparator rgb_comp_; pcl::EdgeAwarePlaneComparator::Ptr edge_aware_comparator_; - pcl::EuclideanClusterComparator::Ptr euclidean_cluster_comparator_; + pcl::EuclideanClusterComparator::Ptr euclidean_cluster_comparator_; public Q_SLOTS: void toggleCapturePressed() diff --git a/apps/src/ni_linemod.cpp b/apps/src/ni_linemod.cpp index 9e436a2bfa9..a03efdd9eff 100644 --- a/apps/src/ni_linemod.cpp +++ b/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::Ptr euclidean_cluster_comparator (new EuclideanClusterComparator); + EuclideanClusterComparator::Ptr euclidean_cluster_comparator (new EuclideanClusterComparator); 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 diff --git a/apps/src/organized_segmentation_demo.cpp b/apps/src/organized_segmentation_demo.cpp index 6f499c6c38a..dab2d040289 100644 --- a/apps/src/organized_segmentation_demo.cpp +++ b/apps/src/organized_segmentation_demo.cpp @@ -254,7 +254,7 @@ OrganizedSegmentationDemo::OrganizedSegmentationDemo (pcl::Grabber& grabber) : g euclidean_comparator_.reset (new pcl::EuclideanPlaneCoefficientComparator ()); rgb_comparator_.reset (new pcl::RGBPlaneCoefficientComparator ()); edge_aware_comparator_.reset (new pcl::EdgeAwarePlaneComparator ()); - euclidean_cluster_comparator_ = pcl::EuclideanClusterComparator::Ptr (new pcl::EuclideanClusterComparator ()); + euclidean_cluster_comparator_ = pcl::EuclideanClusterComparator::Ptr (new pcl::EuclideanClusterComparator ()); // Set up Organized Multi Plane Segmentation mps.setMinInliers (10000); diff --git a/apps/src/pcd_select_object_plane.cpp b/apps/src/pcd_select_object_plane.cpp index e7c527cf36f..4ecd418ad94 100644 --- a/apps/src/pcd_select_object_plane.cpp +++ b/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::Ptr euclidean_cluster_comparator (new EuclideanClusterComparator); + typename EuclideanClusterComparator::Ptr euclidean_cluster_comparator (new EuclideanClusterComparator); 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 diff --git a/apps/src/stereo_ground_segmentation.cpp b/apps/src/stereo_ground_segmentation.cpp index e14bbf68dc8..23460d11516 100755 --- a/apps/src/stereo_ground_segmentation.cpp +++ b/apps/src/stereo_ground_segmentation.cpp @@ -403,8 +403,7 @@ class HRCSSegmentation } } - pcl::EuclideanClusterComparator::Ptr euclidean_cluster_comparator_ (new pcl::EuclideanClusterComparator ()); - + pcl::EuclideanClusterComparator::Ptr euclidean_cluster_comparator_ (new pcl::EuclideanClusterComparator ()); euclidean_cluster_comparator_->setInputCloud (cloud); euclidean_cluster_comparator_->setLabels (labels_ptr); euclidean_cluster_comparator_->setExcludeLabels (plane_labels); diff --git a/segmentation/include/pcl/segmentation/euclidean_cluster_comparator.h b/segmentation/include/pcl/segmentation/euclidean_cluster_comparator.h index 7cff39fc5a9..418d03029df 100644 --- a/segmentation/include/pcl/segmentation/euclidean_cluster_comparator.h +++ b/segmentation/include/pcl/segmentation/euclidean_cluster_comparator.h @@ -50,31 +50,25 @@ namespace pcl * * \author Alex Trevor */ - template + template class EuclideanClusterComparator: public Comparator { public: typedef typename Comparator::PointCloud PointCloud; typedef typename Comparator::PointCloudConstPtr PointCloudConstPtr; - typedef typename pcl::PointCloud PointCloudN; - typedef typename PointCloudN::Ptr PointCloudNPtr; - typedef typename PointCloudN::ConstPtr PointCloudNConstPtr; - typedef typename pcl::PointCloud PointCloudL; typedef typename PointCloudL::Ptr PointCloudLPtr; typedef typename PointCloudL::ConstPtr PointCloudLConstPtr; - typedef boost::shared_ptr > Ptr; - typedef boost::shared_ptr > ConstPtr; + typedef boost::shared_ptr > Ptr; + typedef boost::shared_ptr > ConstPtr; using pcl::Comparator::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 +88,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 @@ -162,8 +124,7 @@ namespace pcl exclude_labels_ = boost::make_shared >(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 */ @@ -191,16 +152,13 @@ namespace pcl 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); - return (dist < dist_threshold); } protected: - PointCloudNConstPtr normals_; PointCloudLPtr labels_; boost::shared_ptr > exclude_labels_; - float angular_threshold_; float distance_threshold_; bool depth_dependent_; Eigen::Vector3f z_axis_; From 4dd532eb597f6d619aa32191e8f8ead26aea213b Mon Sep 17 00:00:00 2001 From: Sergio Agostinho Date: Wed, 12 Jul 2017 17:12:15 +0100 Subject: [PATCH 2/3] Euclidean Cluster Comparator can now cope with situations where labels are not specified --- .../segmentation/euclidean_cluster_comparator.h | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/segmentation/include/pcl/segmentation/euclidean_cluster_comparator.h b/segmentation/include/pcl/segmentation/euclidean_cluster_comparator.h index 418d03029df..d1c699b9e35 100644 --- a/segmentation/include/pcl/segmentation/euclidean_cluster_comparator.h +++ b/segmentation/include/pcl/segmentation/euclidean_cluster_comparator.h @@ -131,14 +131,14 @@ namespace pcl 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_->at (idx1).label; + const uint32_t &label2 = labels_->at (idx2).label; + if ( (*exclude_labels_)[label1] || (*exclude_labels_)[label2]) + return false; + } float dist_threshold = distance_threshold_; if (depth_dependent_) From 94335182aeb84bb880f94aa0bb4fdc3ec16a4714 Mon Sep 17 00:00:00 2001 From: Sergio Agostinho Date: Fri, 14 Jul 2017 13:45:44 +0100 Subject: [PATCH 3/3] Modified the exclude_label_ container to a map. Class cleanup. --- .../tools/impl/organized_segmentation.hpp | 12 +--- apps/src/ni_linemod.cpp | 5 +- apps/src/organized_segmentation_demo.cpp | 12 +--- apps/src/pcd_select_object_plane.cpp | 4 +- apps/src/stereo_ground_segmentation.cpp | 12 +--- .../euclidean_cluster_comparator.h | 63 ++++++++++++++----- 6 files changed, 62 insertions(+), 46 deletions(-) diff --git a/apps/cloud_composer/include/pcl/apps/cloud_composer/tools/impl/organized_segmentation.hpp b/apps/cloud_composer/include/pcl/apps/cloud_composer/tools/impl/organized_segmentation.hpp index 1855fb2d3b5..f38c3489710 100644 --- a/apps/cloud_composer/include/pcl/apps/cloud_composer/tools/impl/organized_segmentation.hpp +++ b/apps/cloud_composer/include/pcl/apps/cloud_composer/tools/impl/organized_segmentation.hpp @@ -103,15 +103,9 @@ std::vector boundary_indices; mps.segmentAndRefine (regions, model_coefficients, inlier_indices, labels, label_indices, boundary_indices); - std::vector 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 > plane_labels = boost::make_shared > (); + for (size_t i = 0; i < label_indices.size (); ++i) + (*plane_labels)[i] = (label_indices[i].indices.size () > min_plane_size); typename PointCloud::CloudVectorType clusters; typename EuclideanClusterComparator::Ptr euclidean_cluster_comparator = boost::make_shared > (); diff --git a/apps/src/ni_linemod.cpp b/apps/src/ni_linemod.cpp index a03efdd9eff..c3784347bfd 100644 --- a/apps/src/ni_linemod.cpp +++ b/apps/src/ni_linemod.cpp @@ -260,8 +260,9 @@ class NILinemod scene->points[points_above_plane->indices[i]].label = 1; euclidean_cluster_comparator->setLabels (scene); - vector exclude_labels (2); exclude_labels[0] = true; exclude_labels[1] = false; - euclidean_cluster_comparator->setExcludeLabels (exclude_labels); + boost::shared_ptr > exclude_labels = boost::make_shared > (); + (*exclude_labels)[0] = true; + (*exclude_labels)[1] = false; OrganizedConnectedComponentSegmentation euclidean_segmentation (euclidean_cluster_comparator); euclidean_segmentation.setInputCloud (cloud); diff --git a/apps/src/organized_segmentation_demo.cpp b/apps/src/organized_segmentation_demo.cpp index dab2d040289..f89b120cb2a 100644 --- a/apps/src/organized_segmentation_demo.cpp +++ b/apps/src/organized_segmentation_demo.cpp @@ -309,15 +309,9 @@ OrganizedSegmentationDemo::cloud_cb (const CloudConstPtr& cloud) if (use_clustering_ && regions.size () > 0) { - std::vector 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 > plane_labels = boost::make_shared > (); + 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); diff --git a/apps/src/pcd_select_object_plane.cpp b/apps/src/pcd_select_object_plane.cpp index 4ecd418ad94..052c1174d45 100644 --- a/apps/src/pcd_select_object_plane.cpp +++ b/apps/src/pcd_select_object_plane.cpp @@ -205,7 +205,9 @@ class ObjectSelection scene->points[points_above_plane->indices[i]].label = 1; euclidean_cluster_comparator->setLabels (scene); - vector exclude_labels (2); exclude_labels[0] = true; exclude_labels[1] = false; + boost::shared_ptr > exclude_labels = boost::make_shared > (); + (*exclude_labels)[0] = true; + (*exclude_labels)[1] = false; euclidean_cluster_comparator->setExcludeLabels (exclude_labels); OrganizedConnectedComponentSegmentation euclidean_segmentation (euclidean_cluster_comparator); diff --git a/apps/src/stereo_ground_segmentation.cpp b/apps/src/stereo_ground_segmentation.cpp index 23460d11516..70ba86dfbaa 100755 --- a/apps/src/stereo_ground_segmentation.cpp +++ b/apps/src/stereo_ground_segmentation.cpp @@ -393,15 +393,9 @@ class HRCSSegmentation pcl::PointCloud::CloudVectorType clusters; if (ground_cloud->points.size () > 0) { - std::vector 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 > plane_labels = boost::make_shared > (); + for (size_t i = 0; i < region_indices.size (); ++i) + (*plane_labels)[i] = (region_indices[i].indices.size () > mps.getMinInliers ()); pcl::EuclideanClusterComparator::Ptr euclidean_cluster_comparator_ (new pcl::EuclideanClusterComparator ()); euclidean_cluster_comparator_->setInputCloud (cloud); diff --git a/segmentation/include/pcl/segmentation/euclidean_cluster_comparator.h b/segmentation/include/pcl/segmentation/euclidean_cluster_comparator.h index d1c699b9e35..d88f010bb35 100644 --- a/segmentation/include/pcl/segmentation/euclidean_cluster_comparator.h +++ b/segmentation/include/pcl/segmentation/euclidean_cluster_comparator.h @@ -50,12 +50,16 @@ namespace pcl * * \author Alex Trevor */ - template + template class EuclideanClusterComparator: public Comparator { + protected: + + using pcl::Comparator::input_; + public: - typedef typename Comparator::PointCloud PointCloud; - typedef typename Comparator::PointCloudConstPtr PointCloudConstPtr; + using typename Comparator::PointCloud; + using typename Comparator::PointCloudConstPtr; typedef typename pcl::PointCloud PointCloudL; typedef typename PointCloudL::Ptr PointCloudLPtr; @@ -64,8 +68,10 @@ namespace pcl typedef boost::shared_ptr > Ptr; typedef boost::shared_ptr > ConstPtr; - using pcl::Comparator::input_; - + typedef std::map ExcludeLabelMap; + typedef boost::shared_ptr ExcludeLabelMapPtr; + typedef boost::shared_ptr ExcludeLabelMapConstPtr; + /** \brief Empty constructor for EuclideanClusterComparator. */ EuclideanClusterComparator () : distance_threshold_ (0.005f) @@ -110,18 +116,24 @@ 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& exclude_labels) + setExcludeLabels (const ExcludeLabelMapConstPtr &exclude_labels) { - exclude_labels_ = boost::make_shared >(exclude_labels); + exclude_labels_ = exclude_labels; } /** \brief Compare points at two indices by their euclidean distance @@ -134,9 +146,15 @@ namespace pcl if (labels_ && exclude_labels_) { assert (labels_->size () == input_->size ()); - const uint32_t &label1 = labels_->at (idx1).label; - const uint32_t &label2 = labels_->at (idx2).label; - if ( (*exclude_labels_)[label1] || (*exclude_labels_)[label2]) + const uint32_t &label1 = (*labels_)[idx1].label; + const uint32_t &label2 = (*labels_)[idx2].label; + + const std::map::const_iterator it1 = exclude_labels_->find (label1); + if ((it1 == exclude_labels_->end ()) || it1->second) + return false; + + const std::map::const_iterator it2 = exclude_labels_->find (label2); + if ((it2 == exclude_labels_->end ()) || it2->second) return false; } @@ -148,19 +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: + + /** \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 > exclude_labels_; + /** \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_; }; }