clustering in progress

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"))

src/initial_processing.cpp

@@ -16,7 +16,6 @@
 #include <pcl/filters/passthrough.h> // used for passthrough filtering
 #include <pcl/conversions.h>
 #include <pcl_conversions/pcl_conversions.h>
-#include <pcl/filters/passthrough.h>
 #include <pcl/filters/voxel_grid.h>
 #include <pcl/common/transforms.h>
 #include <pcl/ModelCoefficients.h> // for plane segmentation
@@ -26,6 +25,7 @@
 #include <pcl/filters/conditional_removal.h> // for color filtering
 #include <pcl/kdtree/kdtree_flann.h> // to use the kdtree search in pcl
 #include <pcl/kdtree/kdtree.h>
+#include <pcl/features/normal_3d.h>
 
 
 
@@ -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<PointT> vox;    // Create the object for performing voxelgrid filtering  //Maybe this has to be changes to RGB as well
+    // Create the filtering object: downsample data by using a predefined leaf size
-    vox.setInputCloud (cloud);            // Set input cloud
+    pcl::VoxelGrid<PointT> vox; // Create the object for performing voxelgrid filtering  //Maybe this has to be changes to RGB as well
-    vox.setLeafSize (voxel_size, voxel_size, voxel_size);   // Set the size of the voxel [m]
+    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<PointT> 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 for the given dataset." << std::endl;
-        std::cout << "Could not estimate a planar model";
-        //return (-1);
     }
-    //Extraction step of the inliers 
+    // Extraction step of the inliers from input cloud 
     pcl::ExtractIndices<PointT> extract;
     extract.setInputCloud (cloud);
     extract.setIndices (inliers);
-    extract.setNegative (true);
+    extract.setNegative (false); // double check if this is the correct value
-    //extract.filter(*cloud); 
+    // Get points associated with the planar model
-    pcl::PointCloud<PointT> cloudF;
+    extract.filter(*cloud);
-    extract.filter(cloudF);
+    // Remove planar inliers & extract the rest 
-    cloud->swap(cloudF);
+    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); 
 
-
+    if (cluster_on == true){
-    /* // Creating the KdTree object for the search method of the extraction
+        // Creating the KdTree object for the search method of the extraction
-    pcl::search::KdTree<PointT>::Ptr tree (new pcl::search::KdTree<PointT>);
+        // https://pcl.readthedocs.io/projects/tutorials/en/master/cluster_extraction.html
-    tree->setInputCloud (cloud);
+        pcl::search::KdTree<PointT>::Ptr tree (new pcl::search::KdTree<PointT>);
-
+        tree->setInputCloud (cloud); 
-    std::vector<pcl::PointIndices> cluster_indices;
+        // Vector of point indices containing 1 instance of point indices for each detected cluster
-    pcl::EuclideanClusterExtraction<PointT> ec;
+        // cluster_indices[0] contains all indices of the 1st cluster in the point cloud
-    ec.setClusterTolerance (0.2); // 2cm
+        std::vector<pcl::PointIndices> cluster_indices; 
-    ec.setMinClusterSize (50);
+        pcl::EuclideanClusterExtraction<PointT> ec;
-    ec.setMaxClusterSize (25000);
+        ec.setClusterTolerance (0.02); // 2cm => choose right value: too small => 1 objects appears as multiple objects
-    ec.setSearchMethod (tree);
+        ec.setMinClusterSize (50);
-    ec.setInputCloud (cloud);
+        ec.setMaxClusterSize (25000);
-    ec.extract (cluster_indices);
+        ec.setSearchMethod (tree);
-    ROS_INFO_STREAM(cluster_indices.size()<<" clusters extracted ");
+        ec.setInputCloud (cloud);
-
+        ec.extract (cluster_indices); // indices are saved in cluster_indices 
-    Eigen::Vector4f centroidL;
+        // seperating each cluster out of vector<PointIndices>
-    Eigen::Vector4f minL;
+        // => iterate through cluster_indices, create new PointCloud for each entry and write all points of respective cluster into that PointCloud
-    Eigen::Vector4f maxL;
+        //int j = 0;
-    Eigen::Vector4f centroidR;
+        //for (std::vector<pcl::PointIndices>::const_iterator it = cluster_indices.begin (); it != cluster_indices.end (); ++it){
-    Eigen::Vector4f minR;
+            std::vector<pcl::PointIndices>::const_iterator it = cluster_indices.begin ();
-    Eigen::Vector4f maxR;
+            pcl::PointCloud<PointT>::Ptr cloud_cluster (new pcl::PointCloud<PointT>);
-    bool first_Lcentroid_detected = false;
+            
-    bool first_Rcentroid_detected = false;
+            for (const auto& idx : it->indices){
-    Eigen::Vector4f centroidCENT;
+                cloud_cluster->push_back ((*cloud)[idx]); 
-    Eigen::Vector4f minCENT;
+                cloud_cluster->width = cloud_cluster->size ();
-    Eigen::Vector4f maxCENT;
+                cloud_cluster->height = 1;
-    bool first_centroid_detected = false;
+                cloud_cluster->is_dense = true;   
-    
-    for (std::vector<pcl::PointIndices>::const_iterator it = cluster_indices.begin (); it != cluster_indices.end (); ++it){
-        pcl::PointCloud<PointT>::Ptr cloud_clusterL (new pcl::PointCloud<PointT>);
-        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;
             }
-        }
+            std::cout << "size of first cluster that has been found " << cloud_cluster->size() << std::endl; 
-        else{
+            sensor_msgs::PointCloud2 clustered_cloud;
-            centroidCENT = centroid;
+            //clustered_cloud.header.frame_id = 1;
-            minCENT = min;
+            pcl::toROSMsg(*cloud_cluster, clustered_cloud);
-            maxCENT = max;
+            pub3.publish(clustered_cloud); 
-            first_centroid_detected = true;
+
-        }
+        //} 
-    } */
+    }
 
     // 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<sensor_msgs::PointCloud2>("unprocessed_cloud", 1);
     pub2 = n.advertise<sensor_msgs::PointCloud2>("processed_cloud", 1);
+    pub3 = n.advertise<sensor_msgs::PointCloud2>("clustered_cloud", 1);
     marker_pub = n.advertise<visualization_msgs::Marker>("visualization_marker", 10);
     marker_pub2 = n.advertise<visualization_msgs::Marker>("visualization_marker2", 10);