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..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,18 +103,12 @@ 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 > (); + 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..c3784347bfd 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 @@ -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 6f499c6c38a..f89b120cb2a 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); @@ -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 e7c527cf36f..052c1174d45 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 @@ -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 e14bbf68dc8..70ba86dfbaa 100755 --- a/apps/src/stereo_ground_segmentation.cpp +++ b/apps/src/stereo_ground_segmentation.cpp @@ -393,18 +393,11 @@ 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 ()); - + 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..d88f010bb35 100644 --- a/segmentation/include/pcl/segmentation/euclidean_cluster_comparator.h +++ b/segmentation/include/pcl/segmentation/euclidean_cluster_comparator.h @@ -50,31 +50,31 @@ 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; - - typedef typename pcl::PointCloud PointCloudN; - typedef typename PointCloudN::Ptr PointCloudNPtr; - typedef typename PointCloudN::ConstPtr PointCloudNConstPtr; + using typename Comparator::PointCloud; + using typename Comparator::PointCloudConstPtr; 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; + + typedef std::map ExcludeLabelMap; + typedef boost::shared_ptr ExcludeLabelMapPtr; + typedef boost::shared_ptr ExcludeLabelMapConstPtr; - 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 +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& 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 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::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; + } 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 > 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_; }; }