kdtree needs to be altered

cfg/config.cfg

@@ -14,6 +14,25 @@ gen.add("ybneg", double_t, 0, "Min bound of Y - Axis cropping",    -5, -5,   5)
 gen.add("zbpos", double_t, 0, "Max bound of Z - Axis cropping",    5, -5,   5)
 gen.add("zbneg", double_t, 0, "Min bound of Z - Axis cropping",    0, -5,   5)
 
+gen.add("rotx", double_t, 0, "Rotation of cloud around X - Axis",    0, 0,   180)
+gen.add("roty", double_t, 0, "Rotation of cloud around Y - Axis",    45, 0,   180)
+gen.add("rotz", double_t, 0, "Rotation of cloud around Z - Axis",    0, 0,   180)
 
+gen.add("movex", double_t, 0, "Move along X - Axis",    0, -10,   10)
+gen.add("movey", double_t, 0, "Move along Y - Axis",    0, -10,   10)
+gen.add("movez", double_t, 0, "Move along Z - Axis",    0, -10,   10)
+
+gen.add("passx_min", double_t, 0, "Start value for Passthrough Filtering for X - Axis",    0.0, -5.0 , 5.0)
+gen.add("passx_max", double_t, 0, "End value for Passthrough Filtering for X - Axis",    5.0, -5.0 , 5.0)
+gen.add("passy_min", double_t, 0, "Start value for Passthrough Filtering for Y - Axis",    -5.0, -5.0 , 5.0)
+gen.add("passy_max", double_t, 0, "End value for Passthrough Filtering for Y - Axis",    5.0, -5.0 , 5.0)
+gen.add("passz_min", double_t, 0, "Start value for Passthrough Filtering for Z - Axis",    0.0, -5.0 , 5.0)
+gen.add("passz_max", double_t, 0, "End value for Passthrough Filtering for Z - Axis",    5.0, -5.0 , 5.0)
+
+gen.add("voxel_size", double_t, 0, "Size of the Leafs for the Voxel Filter",    0.02, 0.0 , 0.1)
+
+gen.add("Pseg_dist", double_t, 0, "Point distance for planar segmentation",    0.025, 0.0 , 0.05)
+
+gen.add("green_thresh", int_t, 0, "Green threshold for color filtering",    150, 0 , 255)
 
 exit(gen.generate(PACKAGE, "pcl_tutorial", "config"))

src/initial_processing.cpp

@@ -1,23 +1,33 @@
+#include <sstream>
+#include <iostream>
+#include <math.h>
+
 #include "ros/ros.h"
 #include "std_msgs/String.h"
 #include "std_msgs/Bool.h" 
 #include "std_msgs/Float32.h" 
 #include <dynamic_reconfigure/server.h>
+#include <sensor_msgs/PointCloud2.h>
+#include <visualization_msgs/Marker.h>
+#include <visualization_msgs/MarkerArray.h>
+
 #include <pcl_tutorial/configConfig.h>
-
+#include <pcl/point_types.h>
-
+#include <pcl/filters/passthrough.h> // used for passthrough filtering
-#include <sstream>
-#include <iostream>
-#include <math.h>
-
-#include <pcl/filters/passthrough.h>
 #include <pcl/conversions.h>
 #include <pcl_conversions/pcl_conversions.h>
 #include <pcl/filters/passthrough.h>
 #include <pcl/filters/voxel_grid.h>
-#include <sensor_msgs/PointCloud2.h>
+#include <pcl/common/transforms.h>
-#include <visualization_msgs/Marker.h>
+#include <pcl/ModelCoefficients.h> // for plane segmentation
-#include <visualization_msgs/MarkerArray.h>
+#include <pcl/segmentation/sac_segmentation.h> // for plane segmentation
+#include <pcl/filters/extract_indices.h>
+#include <pcl/segmentation/extract_clusters.h>
+#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>
+
+
 
 /* 
 TBD: get to know typedef such as stringstream
@@ -25,7 +35,9 @@ TBD: get to know typedef such as stringstream
 
 // define publisher
 ros::Publisher pub;
-ros::Publisher pub_2;
+ros::Publisher pub2;
+ros::Publisher marker_pub;
+ros::Publisher marker_pub2;
 ros::Publisher visual;
 
 float xbpos = 5.0;
@@ -34,6 +46,21 @@ float ybpos = 5.0;
 float ybneg = -5.0;
 float zbpos = 5.0;
 float zbneg = 0.0;
+float rotx = 0;
+float roty = 45; 
+float rotz = 0;
+float movex = 0.0;
+float movey = 0.0;
+float movez = 0.0;
+float passx_min = 0.0;
+float passx_max = 5.0;
+float passy_min = -5.0;
+float passy_max = 5.0;
+float passz_min = 0.0;
+float passz_max = 5.0;
+float voxel_size = 0.02;
+float Pseg_dist = 0.025;
+int green_thresh = 150;
 
 typedef pcl::PointXYZRGB PointT;
 
@@ -44,49 +71,224 @@ void callback(pcl_tutorial::configConfig &config, uint32_t level)
   ybpos = config.ybpos;
   ybneg = config.ybneg;
   zbpos = config.zbpos;
-  zbneg = config.zbneg;  
+  zbneg = config.zbneg; 
+  rotx = config.rotx;
+  roty = config.roty;
+  rotz = config.rotz;
+  movex = config.movex;
+  movey = config.movey;
+  movez = config.movez;
+  passx_min = config.passx_min;
+  passx_max = config.passx_max;
+  passy_min = config.passy_min;
+  passy_max = config.passy_max;
+  passz_min = config.passz_min;
+  passz_max = config.passz_max;
+  voxel_size = config.voxel_size;
+  Pseg_dist = config.Pseg_dist;
+  green_thresh = config.green_thresh;
 }
 
 void cloud_cb (const sensor_msgs::PointCloud2ConstPtr& cloud_msg){
 
-
+    // create pointer to Point Clouds in pcl
-    // create pointer to Point Cloud in pcl
     pcl::PointCloud<PointT>::Ptr cloud(new pcl::PointCloud<PointT>);
-    pcl::PointCloud<PointT>::Ptr cloud_filtered (new pcl::PointCloud<PointT>);
+    //pcl::PointCloud<PointT>::Ptr cloud_filtered (new pcl::PointCloud<PointT>);
+    
+    // publish unprocessed point cloud 
+    sensor_msgs::PointCloud2 input;
+    input = *cloud_msg;
+    pub.publish(input);
 
     // convert ROS message data to point cloud in pcl
     pcl::fromROSMsg(*cloud_msg, *cloud);
-    
 
-    // Create filtering object
+    //Perform a translation and rotation on the object cloud using rqt: dynamic reconfigure 
+    Eigen::Affine3f transform = Eigen::Affine3f::Identity();
+    transform.translation() << movex, movey, movez;
+    transform.rotate(Eigen::AngleAxisf((rotx*M_PI) / 180, Eigen::Vector3f::UnitX()));
+    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);
     passx.setFilterFieldName ("x");
-    passx.setFilterLimits (0.0, 5.0);
+    passx.setFilterLimits (passx_min, passx_max);
     passx.filter (*cloud);
 
     pcl::PassThrough<PointT> passy;
     passy.setInputCloud (cloud);
     passy.setFilterFieldName ("y");
-    passy.setFilterLimits (-5.0, 5.0);
+    passy.setFilterLimits (passy_min, passy_max);
     passy.filter (*cloud);
 
     pcl::PassThrough<PointT> passz;
     passz.setInputCloud (cloud);
     passz.setFilterFieldName ("z");
-    passz.setFilterLimits (0.0, 5.0);
+    passz.setFilterLimits (passz_min, passz_max);
-    passz.filter (*cloud);
+    passz.filter (*cloud);  
 
+/*     // Visualizing the bound of the passthrough filter in RVIZ
+    visualization_msgs::Marker bounding_box;
+    bounding_box.id = 0;
+    bounding_box.type = visualization_msgs::Marker::CUBE;
+    bounding_box.color.g = 1.0;
+    bounding_box.color.a = 0.5;
+    bounding_box.header.frame_id = "zed_base_link";
+    bounding_box.action = visualization_msgs::Marker::ADD;
+    bounding_box.pose.orientation.w = 1.0;
+    bounding_box.pose.position.x = (passx_max+passx_min)/2;
+    bounding_box.pose.position.y = (passy_max+passy_min)/2;
+    bounding_box.pose.position.z = (passz_max+passz_min)/2;
+    bounding_box.scale.x = abs(passx_max-passx_min);
+    bounding_box.scale.y = abs(passy_max-passy_min);
+    bounding_box.scale.z = abs(passz_max-passz_min);
+    marker_pub.publish(bounding_box); */
 
-/*     pcl::VoxelGrid<PointT> vox;    // Create the object for performing voxelgrid filtering  //Maybe this has to be changes to RGB as well
+    visualization_msgs::Marker line_list;
+    line_list.id = 1;
+    line_list.type = visualization_msgs::Marker::LINE_LIST;
+    line_list.scale.x = 0.05;
+    line_list.color.g = 1.0;
+    line_list.color.a = 1.0;
+    line_list.header.frame_id = "zed_base_link";
+    line_list.action = visualization_msgs::Marker::ADD;
+    line_list.pose.orientation.w = 1.0;
+    geometry_msgs::Point p1; geometry_msgs::Point p2; geometry_msgs::Point p3; geometry_msgs::Point p4;
+    geometry_msgs::Point p5; geometry_msgs::Point p6; geometry_msgs::Point p7; geometry_msgs::Point p8;
+    p1.x = passx_min; p1.y = passy_min; p1.z = passz_min;
+    p2.x = passx_min; p2.y = passy_max; p2.z = passz_min;
+    p3.x = passx_max; p3.y = passy_min; p3.z = passz_min;
+    p4.x = passx_max; p4.y = passy_max; p4.z = passz_min;
+    p5.x = passx_min; p5.y = passy_min; p5.z = passz_max;
+    p6.x = passx_min; p6.y = passy_max; p6.z = passz_max;
+    p7.x = passx_max; p7.y = passy_min; p7.z = passz_max;
+    p8.x = passx_max; p8.y = passy_max; p8.z = passz_max;
+    line_list.points.push_back(p1); line_list.points.push_back(p2);
+    line_list.points.push_back(p1); line_list.points.push_back(p3);
+    line_list.points.push_back(p4); line_list.points.push_back(p2);
+    line_list.points.push_back(p4); line_list.points.push_back(p3);
+    line_list.points.push_back(p1); line_list.points.push_back(p5);
+    line_list.points.push_back(p2); line_list.points.push_back(p6);
+    line_list.points.push_back(p3); line_list.points.push_back(p7);
+    line_list.points.push_back(p4); line_list.points.push_back(p8);
+    line_list.points.push_back(p5); line_list.points.push_back(p6);
+    line_list.points.push_back(p5); line_list.points.push_back(p7);
+    line_list.points.push_back(p8); line_list.points.push_back(p6);
+    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
     vox.setInputCloud (cloud);            // Set input cloud
-    vox.setLeafSize (0.02, 0.02, 0.02);   // Set the size of the voxel
+    vox.setLeafSize (voxel_size, voxel_size, voxel_size);   // Set the size of the voxel [m]
-    vox.filter (*cloud); */
+    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;
+    // 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.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);
+    }
+    //Extraction step of the inliers 
+    pcl::ExtractIndices<PointT> extract;
+    extract.setInputCloud (cloud);
+    extract.setIndices (inliers);
+    extract.setNegative (true);
+    //extract.filter(*cloud); 
+    pcl::PointCloud<PointT> cloudF;
+    extract.filter(cloudF);
+    cloud->swap(cloudF);
+
+    // color filtering
+    // Building the condition for points to remain in cloud
+    pcl::ConditionAnd<PointT>::Ptr color_condition (new pcl::ConditionAnd<PointT> ());
+    pcl::PackedRGBComparison<PointT>::Ptr green_condition(new pcl::PackedRGBComparison<pcl::PointXYZRGB>("g", pcl::ComparisonOps::GT, green_thresh));
+    color_condition->addComparison(green_condition);
+    // Building the filter
+    pcl::ConditionalRemoval<PointT> color_filter;
+    color_filter.setCondition (color_condition);
+    color_filter.setInputCloud(cloud);
+    // apply the filter 
+    color_filter.filter(*cloud);
+
+
+    /* // Creating the KdTree object for the search method of the extraction
+    pcl::search::KdTree<PointT>::Ptr tree (new pcl::search::KdTree<PointT>);
+    tree->setInputCloud (cloud);
+
+    std::vector<pcl::PointIndices> cluster_indices;
+    pcl::EuclideanClusterExtraction<PointT> 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<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;
+            }
+        }
+        else{
+            centroidCENT = centroid;
+            minCENT = min;
+            maxCENT = max;
+            first_centroid_detected = true;
+        }
+    } */
 
     // convert to ROS data type
     sensor_msgs::PointCloud2 output;
     pcl::toROSMsg(*cloud, output);
-    pub.publish(output);
+    pub2.publish(output); 
+
 
 }
 
@@ -104,11 +306,13 @@ int main(int argc, char **argv){
     server.setCallback(f);
 
     // create subscriber to receive point cloud 
-    ros::Subscriber sub = n.subscribe<sensor_msgs::PointCloud2>("/zed/zed_node/point_cloud/cloud_registered", 1, cloud_cb);
+    ros::Subscriber sub = n.subscribe<sensor_msgs::PointCloud2>("/zed2i/zed_node/point_cloud/cloud_registered", 1, cloud_cb);
     
     // create publisher for output processed point cloud 
-    pub = n.advertise<sensor_msgs::PointCloud2>("processed_cloud", 1);
+    pub = n.advertise<sensor_msgs::PointCloud2>("unprocessed_cloud", 1);
-    pub_2 = n.advertise<sensor_msgs::PointCloud2>("unprocessed_cloud", 1);
+    pub2 = n.advertise<sensor_msgs::PointCloud2>("processed_cloud", 1);
+    marker_pub = n.advertise<visualization_msgs::Marker>("visualization_marker", 10);
+    marker_pub2 = n.advertise<visualization_msgs::Marker>("visualization_marker2", 10);
 
     // create publisher for visualising marker 
     visual = n.advertise<visualization_msgs::Marker> ("marker", 1);