From 4f9d5d272191f6e602e0e90e0d9eecbd03c425c9 Mon Sep 17 00:00:00 2001 From: caroline Date: Tue, 26 Apr 2022 17:26:32 +0200 Subject: [PATCH] clustering in progress --- cfg/config.cfg | 2 + src/initial_processing.cpp | 129 ++++++++++++++++--------------------- 2 files changed, 59 insertions(+), 72 deletions(-) diff --git a/cfg/config.cfg b/cfg/config.cfg index a6c6309..33020a9 100755 --- a/cfg/config.cfg +++ b/cfg/config.cfg @@ -35,4 +35,6 @@ gen.add("Pseg_dist", double_t, 0, "Point distance for planar segmentation", 0 gen.add("green_thresh", int_t, 0, "Green threshold for color filtering", 150, 0 , 255) +gen.add("cluster_on", bool_t, 0, "determines if Euclidean Clustering is applied", False) + exit(gen.generate(PACKAGE, "pcl_tutorial", "config")) diff --git a/src/initial_processing.cpp b/src/initial_processing.cpp index 57e0306..a83595e 100755 --- a/src/initial_processing.cpp +++ b/src/initial_processing.cpp @@ -16,7 +16,6 @@ #include // used for passthrough filtering #include #include -#include #include #include #include // for plane segmentation @@ -26,6 +25,7 @@ #include // for color filtering #include // to use the kdtree search in pcl #include +#include @@ -36,6 +36,7 @@ TBD: get to know typedef such as stringstream // define publisher ros::Publisher pub; ros::Publisher pub2; +ros::Publisher pub3; ros::Publisher marker_pub; ros::Publisher marker_pub2; ros::Publisher visual; @@ -61,6 +62,7 @@ float passz_max = 5.0; float voxel_size = 0.02; float Pseg_dist = 0.025; int green_thresh = 150; +bool cluster_on = false; typedef pcl::PointXYZRGB PointT; @@ -87,6 +89,7 @@ void callback(pcl_tutorial::configConfig &config, uint32_t level) voxel_size = config.voxel_size; Pseg_dist = config.Pseg_dist; green_thresh = config.green_thresh; + cluster_on = config.cluster_on; } void cloud_cb (const sensor_msgs::PointCloud2ConstPtr& cloud_msg){ @@ -180,40 +183,41 @@ void cloud_cb (const sensor_msgs::PointCloud2ConstPtr& cloud_msg){ line_list.points.push_back(p8); line_list.points.push_back(p7); marker_pub2.publish(line_list); - pcl::VoxelGrid vox; // Create the object for performing voxelgrid filtering //Maybe this has to be changes to RGB as well - vox.setInputCloud (cloud); // Set input cloud - vox.setLeafSize (voxel_size, voxel_size, voxel_size); // Set the size of the voxel [m] + // Create the filtering object: downsample data by using a predefined leaf size + pcl::VoxelGrid vox; // Create the object for performing voxelgrid filtering //Maybe this has to be changes to RGB as well + vox.setInputCloud (cloud); // Set input cloud + vox.setLeafSize (voxel_size, voxel_size, voxel_size); // Set the size of the voxel [m] vox.filter (*cloud); // Planar segmentation - // coefficients: used to show the contents of inliers set together with estimated plane parameters - pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients); - pcl::PointIndices::Ptr inliers(new pcl::PointIndices); // Create the segmentation object pcl::SACSegmentation seg; + // coefficients: used to show the contents of inliers set together with estimated plane parameters + pcl::PointIndices::Ptr inliers(new pcl::PointIndices); + pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients); // Optional seg.setOptimizeCoefficients (true); // Mandatory: set model and method type // distance threshold defines how close a point has to be to the model to be considered an inlier seg.setModelType (pcl::SACMODEL_PLANE); seg.setMethodType (pcl::SAC_RANSAC); // ransac is a simple and robust method + // seg.setMaxIterations (100); seg.setDistanceThreshold (Pseg_dist); seg.setInputCloud (cloud); seg.segment (*inliers, *coefficients); if (inliers->indices.size () == 0){ - PCL_ERROR ("Could not estimate a planar model for the given dataset."); - std::cout << "Could not estimate a planar model"; - //return (-1); + std::cout << "Could not estimate a planar model for the given dataset." << std::endl; } - //Extraction step of the inliers + // Extraction step of the inliers from input cloud pcl::ExtractIndices extract; extract.setInputCloud (cloud); extract.setIndices (inliers); - extract.setNegative (true); - //extract.filter(*cloud); - pcl::PointCloud cloudF; - extract.filter(cloudF); - cloud->swap(cloudF); + extract.setNegative (false); // double check if this is the correct value + // Get points associated with the planar model + extract.filter(*cloud); + // Remove planar inliers & extract the rest + extract.setNegative (true); + extract.filter(*cloud); // color filtering // Building the condition for points to remain in cloud @@ -225,64 +229,44 @@ void cloud_cb (const sensor_msgs::PointCloud2ConstPtr& cloud_msg){ color_filter.setCondition (color_condition); color_filter.setInputCloud(cloud); // apply the filter - color_filter.filter(*cloud); + color_filter.filter(*cloud); - - /* // Creating the KdTree object for the search method of the extraction - pcl::search::KdTree::Ptr tree (new pcl::search::KdTree); - tree->setInputCloud (cloud); - - std::vector cluster_indices; - pcl::EuclideanClusterExtraction ec; - ec.setClusterTolerance (0.2); // 2cm - ec.setMinClusterSize (50); - ec.setMaxClusterSize (25000); - ec.setSearchMethod (tree); - ec.setInputCloud (cloud); - ec.extract (cluster_indices); - ROS_INFO_STREAM(cluster_indices.size()<<" clusters extracted "); - - Eigen::Vector4f centroidL; - Eigen::Vector4f minL; - Eigen::Vector4f maxL; - Eigen::Vector4f centroidR; - Eigen::Vector4f minR; - Eigen::Vector4f maxR; - bool first_Lcentroid_detected = false; - bool first_Rcentroid_detected = false; - Eigen::Vector4f centroidCENT; - Eigen::Vector4f minCENT; - Eigen::Vector4f maxCENT; - bool first_centroid_detected = false; - - for (std::vector::const_iterator it = cluster_indices.begin (); it != cluster_indices.end (); ++it){ - pcl::PointCloud::Ptr cloud_clusterL (new pcl::PointCloud); - for (const auto& idx : it->indices){ - cloud_clusterL->push_back ((*cloud)[idx]); //* - cloud_clusterL->width = cloud_clusterL->size (); - cloud_clusterL->height = 1; - cloud_clusterL->is_dense = true; - Eigen::Vector4f centroid; - Eigen::Vector4f min; - Eigen::Vector4f max; - pcl::compute3DCentroid (*cloud_clusterL, centroid); - pcl::getMinMax3D (*cloud_clusterL, min, max); - } - - if (first_centroid_detected){ - if (abs(centroid[1]) < abs(centroidCENT[1])){ - centroidCENT = centroid; - minCENT = min; - maxCENT = max; + if (cluster_on == true){ + // Creating the KdTree object for the search method of the extraction + // https://pcl.readthedocs.io/projects/tutorials/en/master/cluster_extraction.html + pcl::search::KdTree::Ptr tree (new pcl::search::KdTree); + tree->setInputCloud (cloud); + // Vector of point indices containing 1 instance of point indices for each detected cluster + // cluster_indices[0] contains all indices of the 1st cluster in the point cloud + std::vector cluster_indices; + pcl::EuclideanClusterExtraction ec; + ec.setClusterTolerance (0.02); // 2cm => choose right value: too small => 1 objects appears as multiple objects + ec.setMinClusterSize (50); + ec.setMaxClusterSize (25000); + ec.setSearchMethod (tree); + ec.setInputCloud (cloud); + ec.extract (cluster_indices); // indices are saved in cluster_indices + // seperating each cluster out of vector + // => iterate through cluster_indices, create new PointCloud for each entry and write all points of respective cluster into that PointCloud + //int j = 0; + //for (std::vector::const_iterator it = cluster_indices.begin (); it != cluster_indices.end (); ++it){ + std::vector::const_iterator it = cluster_indices.begin (); + pcl::PointCloud::Ptr cloud_cluster (new pcl::PointCloud); + + for (const auto& idx : it->indices){ + cloud_cluster->push_back ((*cloud)[idx]); + cloud_cluster->width = cloud_cluster->size (); + cloud_cluster->height = 1; + cloud_cluster->is_dense = true; } - } - else{ - centroidCENT = centroid; - minCENT = min; - maxCENT = max; - first_centroid_detected = true; - } - } */ + std::cout << "size of first cluster that has been found " << cloud_cluster->size() << std::endl; + sensor_msgs::PointCloud2 clustered_cloud; + //clustered_cloud.header.frame_id = 1; + pcl::toROSMsg(*cloud_cluster, clustered_cloud); + pub3.publish(clustered_cloud); + + //} + } // convert to ROS data type sensor_msgs::PointCloud2 output; @@ -311,6 +295,7 @@ int main(int argc, char **argv){ // create publisher for output processed point cloud pub = n.advertise("unprocessed_cloud", 1); pub2 = n.advertise("processed_cloud", 1); + pub3 = n.advertise("clustered_cloud", 1); marker_pub = n.advertise("visualization_marker", 10); marker_pub2 = n.advertise("visualization_marker2", 10);