in progress: differently colored batches

src/initial_processing.cpp

@@ -26,7 +26,7 @@
 #include <pcl/kdtree/kdtree_flann.h> // to use the kdtree search in pcl
 #include <pcl/kdtree/kdtree.h>
 #include <pcl/features/normal_3d.h>
-
+#include <pcl/visualization/pcl_visualizer.h>
 
 
 /* 
@@ -62,12 +62,13 @@ float passz_max = 5.0;
 float voxel_size = 0.02;
 float Pseg_dist = 0.025;
 int green_thresh = 150;
-bool cluster_on = false;
+bool cluster_on = true;
 
 typedef pcl::PointXYZRGB PointT;
 
 void callback(pcl_tutorial::configConfig &config, uint32_t level) 
 {
+  std::cout << "dynamic parameters have been updated";
   xbpos = config.xbpos;
   xbneg = config.xbneg;
   ybpos = config.ybpos;
@@ -113,7 +114,7 @@ void cloud_cb (const sensor_msgs::PointCloud2ConstPtr& cloud_msg){
     transform.rotate(Eigen::AngleAxisf((roty*M_PI) / 180, Eigen::Vector3f::UnitY()));
     transform.rotate(Eigen::AngleAxisf((rotz*M_PI) / 180, Eigen::Vector3f::UnitZ()));
     pcl::transformPointCloud(*cloud, *cloud, transform);
-
+    
     // Create filtering objects for all 3 coordinates
     pcl::PassThrough<PointT> passx;
     passx.setInputCloud (cloud);
@@ -240,7 +241,7 @@ void cloud_cb (const sensor_msgs::PointCloud2ConstPtr& cloud_msg){
         // cluster_indices[0] contains all indices of the 1st cluster in the point cloud
         std::vector<pcl::PointIndices> cluster_indices; 
         pcl::EuclideanClusterExtraction<PointT> ec;
-        ec.setClusterTolerance (0.02); // 2cm => choose right value: too small => 1 objects appears as multiple objects
+        ec.setClusterTolerance (0.2); // 2cm => choose right value: too small => 1 objects appears as multiple objects
         ec.setMinClusterSize (50);
         ec.setMaxClusterSize (25000);
         ec.setSearchMethod (tree);
@@ -249,31 +250,79 @@ void cloud_cb (const sensor_msgs::PointCloud2ConstPtr& cloud_msg){
         // seperating each cluster out of vector<PointIndices>
         // => 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<pcl::PointIndices>::const_iterator it = cluster_indices.begin (); it != cluster_indices.end (); ++it){
+
-            std::vector<pcl::PointIndices>::const_iterator it = cluster_indices.begin ();
+
-            pcl::PointCloud<PointT>::Ptr cloud_cluster (new pcl::PointCloud<PointT>);
+        // std::vector<std::vector<pcl::PointCloud<PointT>>> cluster_vector(2, std::vector<pcl::PointCloud<PointT>>());
-            
+        //std::vector<pcl::PointCloud<PointT>> cluster_vector{{}};
+        pcl::PointCloud<PointT> cluster_vector[cluster_indices.size ()];
+        //std::vector<pcl::PointCloud<PointT>> myRow(cluster_indices.size ());
+        //std::cout << "größe von cluster_vector: " << cluster_vector.size ();
+        
+        
+        pcl::PointCloud<PointT>::Ptr cloud_cluster (new pcl::PointCloud<PointT>);
+        int count = 0;
+        
+        for (std::vector<pcl::PointIndices>::const_iterator it = cluster_indices.begin (); it != cluster_indices.end (); ++it){
+            //std::vector<pcl::PointIndices>::const_iterator it = cluster_indices.begin ();
+            std::vector<pcl::PointCloud<PointT>> single_cluster;
             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;   
+                cloud_cluster->is_dense = true;
+                cluster_vector[count] = *cloud_cluster;
+            }
+            count++;
+        }
+            //std::cout << "size of first cluster that has been found " << cloud_cluster->size() << std::endl; 
+            //std::cout << "size of first cluster that has been found " << cluster_indices.size() << std::endl;
+            std::cout << "size of first cluster that has been found " << sizeof(cluster_vector)/sizeof(cluster_vector[0]) << std::endl;
+            //std::cout << "size of first cluster that has been found " << cluster_vector.size() << std::endl;
+            int color_id = 1;
+            for (int i = 0; i < cluster_indices.size(); i++){
+                if (color_id == 1){
+                    for (int l = 0; l < cluster_vector[i].points.size(); l++){
+                        cluster_vector[i].points[l].r = 200;
+                        cluster_vector[i].points[l].g = 0;
+                        cluster_vector[i].points[l].b = 0;
+                    }
+                }
+                if (color_id == 2){
+                    for (int l = 0; l < cluster_vector[i].points.size(); l++){
+                        cluster_vector[i].points[l].r = 0;
+                        cluster_vector[i].points[l].g = 0;
+                        cluster_vector[i].points[l].b = 200;
+                    }
+                }
+                if (color_id == 3){
+                    for (int l = 0; l < cluster_vector[i].points.size(); l++){
+                        cluster_vector[i].points[l].r = 0;
+                        cluster_vector[i].points[l].g = 200;
+                        cluster_vector[i].points[l].b = 0;
+                        color_id = 0;
+                    }
+                }
+                std::cout << color_id;
+                color_id++;
+                
+               
+            }
+            pcl::PointCloud<PointT> clusters;
+            for (int j = 0; j < cluster_indices.size(); j++){
+                clusters += cluster_vector[j];
             }
-            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(clusters, clustered_cloud); // publish all points in all rows 
-            pcl::toROSMsg(*cloud_cluster, clustered_cloud);
+            //pcl::toROSMsg(*cloud_cluster, clustered_cloud);
+            clustered_cloud.header.frame_id = "zed2i_base_link";
             pub3.publish(clustered_cloud); 
 
-        //} 
     }
-
+ 
     // convert to ROS data type
     sensor_msgs::PointCloud2 output;
     pcl::toROSMsg(*cloud, output);
     pub2.publish(output); 
-
-
 }
 
 int main(int argc, char **argv){