Segmentation working

Author: heethesh

cuboid_detection/CMakeLists.txt

@@ -144,7 +144,8 @@ include
 ## ADD YOUR PROGRAMS HERE
 
 # add_executable(${PROJECT_NAME}_node src/cuboid_detection_node.cpp)
-add_executable(example src/example.cpp)
+# add_executable(example src/example.cpp)
+add_executable(ground_plane_segmentation src/ground_plane_segmentation.cpp)
 
 ## <<<<<<<<<<<<<<<<<<<<<<<
 
@@ -165,4 +166,5 @@ add_executable(example src/example.cpp)
 # target_link_libraries(${PROJECT_NAME}
 #   ${OpenCV_LIBS}
 # )
-target_link_libraries(example ${catkin_LIBRARIES})
+# target_link_libraries(example ${catkin_LIBRARIES})
+target_link_libraries(ground_plane_segmentation ${catkin_LIBRARIES})

cuboid_detection/launch/ground_plane_segmentation.launch

@@ -0,0 +1,31 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<launch>
+
+    <!-- Play rosbag file -->
+    <include file="$(find cuboid_detection)/launch/play_rosbag.launch">
+        <arg name="filename" value="2019-03-23-22-32-55.bag" />
+    </include>
+
+    <!-- Run a VoxelGrid filter to clean NaNs and downsample the data -->
+    <node
+        pkg="cuboid_detection"
+        type="ground_plane_segmentation"
+        name="ground_plane_segmentation"
+        output="screen">
+
+        <!-- Set topics and params -->
+        <rosparam>
+            <!-- invert (default: true): True > Box; False > Ground Plane -->
+            invert: true
+            <!-- voxel_size (default: 0.01): Lower the value, denser the point cloud, more CPU -->
+            voxel_size: 0.005
+            <!-- distance_threshold (default: 0.01): Lower the value, more of box + table noise -->
+            distance_threshold: 0.015
+            <!-- Topics -->
+            input: "/camera/depth/color/points"
+            output: "/ground_plane_segmentation/points"
+            plane_coefficients: "/ground_plane_segmentation/coefficients"
+        </rosparam>
+    </node>
+
+</launch>

cuboid_detection/launch/play_rosbag.launch

@@ -0,0 +1,14 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<launch>
+
+    <arg name="filename" default="2019-03-23-22-32-55.bag" />
+
+    <!-- Play rosbag file -->
+    <node 
+        pkg="rosbag"
+        type="play"
+        name="player"
+        output="screen"
+        args="--clock --loop $(find cuboid_detection)/bags/$(arg filename)" />
+
+</launch>

cuboid_detection/src/ground_plane_segmentation.cpp

@@ -15,24 +15,23 @@ target_link_libraries(ground_plane_segmentation ${catkin_LIBRARIES})
 #include <ros/ros.h>
 // PCL specific includes
 #include <sensor_msgs/PointCloud2.h>
-#include <pcl/conversions.h>
-#include <pcl_conversions/pcl_conversions.h>
-#include <pcl_ros/transforms.h>
-#include <pcl/point_cloud.h>
-#include <pcl/point_types.h>
 #include <pcl/ModelCoefficients.h>
 #include <pcl/sample_consensus/method_types.h>
 #include <pcl/sample_consensus/model_types.h>
 #include <pcl/segmentation/sac_segmentation.h>
-#include <pcl/filters/passthrough.h>
-#include <pcl/filters/voxel_grid.h>
-#include <pcl/filters/extract_indices.h>
+#include <pcl_ros/point_cloud.h>
+#include <pcl_ros/filters/passthrough.h>
+#include <pcl_ros/filters/voxel_grid.h>
+#include <pcl_ros/filters/extract_indices.h>
+#include <pcl_conversions/pcl_conversions.h>
 
 // Publishers
-ros::Publisher coef_pub;
 ros::Publisher pcl_pub;
+ros::Publisher coef_pub;
 
 // Topics
+bool invert;
+double voxel_size;
 double distance_threshold;
 std::string input_topic;
 std::string output_topic;
@@ -44,48 +43,52 @@ bool debug = false;
 void callback(const sensor_msgs::PointCloud2ConstPtr& input)
 {
     // Convert the sensor_msgs/PointCloud2 data to pcl/PointCloud
-    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_ptr(new pcl::PointCloud<pcl::PointXYZ>);
-    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered_ptr(new pcl::PointCloud<pcl::PointXYZ>);
-    pcl::fromROSMsg(*input, *cloud_ptr);
+    pcl::PCLPointCloud2::Ptr cloud_ptr(new pcl::PCLPointCloud2);
+    pcl::PCLPointCloud2::Ptr cloud_filtered_ptr(new pcl::PCLPointCloud2);
+    pcl_conversions::toPCL(*input, *cloud_ptr);
     if (debug) std::cerr << "PointCloud before filtering: " << cloud_ptr->width << " " << cloud_ptr->height << " data points." << std::endl;
 
     // Filter the points in z-axis
-    pcl::PassThrough<pcl::PointXYZ> pass;
+    pcl::PassThrough<pcl::PCLPointCloud2> pass;
     pass.setInputCloud(cloud_ptr);
     pass.setFilterFieldName("z");
     pass.setFilterLimits(0.0, 1.4);
     pass.filter(*cloud_filtered_ptr);
     if (debug) std::cerr << "PointCloud after filtering: " << cloud_filtered_ptr->width << " " << cloud_filtered_ptr->height << " data points." << std::endl;
 
     // Downsample the points
-    pcl::PointCloud<pcl::PointXYZ>::Ptr voxel_ptr(new pcl::PointCloud<pcl::PointXYZ>);
-    pcl::VoxelGrid<pcl::PointXYZ> voxel;
-    voxel.setInputCloud(cloud_filtered_ptr);
-    voxel.setLeafSize(0.01, 0.01, 0.01);
-    voxel.filter(*voxel_ptr);
+    pcl::PCLPointCloud2::Ptr voxel_ptr(new pcl::PCLPointCloud2);
+    pcl::VoxelGrid<pcl::PCLPointCloud2> downsample;
+    downsample.setInputCloud(cloud_filtered_ptr);
+    downsample.setLeafSize(voxel_size, voxel_size, voxel_size);
+    downsample.filter(*voxel_ptr);
 
     // Setup ground plane segmentation
     pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients);
     pcl::PointIndices::Ptr inliers(new pcl::PointIndices);
     pcl::SACSegmentation<pcl::PointXYZ> seg;
 
+    // Convert to the templated PointCloud
+    pcl::PointCloud<pcl::PointXYZ>::Ptr pcl_voxel_ptr(new pcl::PointCloud<pcl::PointXYZ>);
+    pcl::fromPCLPointCloud2(*voxel_ptr, *pcl_voxel_ptr);
+
     // Segmentation paramters    
     seg.setOptimizeCoefficients(true);
     seg.setModelType(pcl::SACMODEL_PLANE);
     seg.setMethodType(pcl::SAC_RANSAC);
     seg.setMaxIterations(1000);
-    seg.setDistanceThreshold(0.01);
+    seg.setDistanceThreshold(distance_threshold);
 
     // Segment the largest planar component from the cloud
-    seg.setInputCloud(voxel_ptr);
+    seg.setInputCloud(pcl_voxel_ptr);
     seg.segment(*inliers, *coefficients);
 
     // Extract the inliers
-    pcl::PointCloud<pcl::PointXYZ>::Ptr plane_cloud_ptr(new pcl::PointCloud<pcl::PointXYZ>);
-    pcl::ExtractIndices<pcl::PointXYZ> extract;
+    pcl::PCLPointCloud2::Ptr plane_cloud_ptr(new pcl::PCLPointCloud2);
+    pcl::ExtractIndices<pcl::PCLPointCloud2> extract;
     extract.setInputCloud(voxel_ptr);
     extract.setIndices(inliers);
-    extract.setNegative(true);
+    extract.setNegative(invert);
     extract.filter(*plane_cloud_ptr);
     if (debug) std::cerr << "PointCloud representing the planar component: " << plane_cloud_ptr->width << " " << plane_cloud_ptr->height << " data points." << std::endl;
 
@@ -96,7 +99,7 @@ void callback(const sensor_msgs::PointCloud2ConstPtr& input)
 
     // Convert to ROS data type
     sensor_msgs::PointCloud2 output;
-    pcl::toROSMsg(*plane_cloud_ptr, output);
+    pcl_conversions::fromPCL(*plane_cloud_ptr, output);
     pcl_pub.publish(output);
 }
 
@@ -107,11 +110,28 @@ int main(int argc, char** argv)
     ros::NodeHandle nh("~");
 
     // Get params from launch file
+    nh.getParam("invert", invert);
+    nh.getParam("voxel_size", voxel_size);
     nh.getParam("distance_threshold", distance_threshold);
     nh.getParam("input", input_topic);
     nh.getParam("output", output_topic);
-    nh.getParam("coefficients", coefficients_topic);
+    nh.getParam("plane_coefficients", coefficients_topic);
+
+    // Params defaults
+    nh.param<bool>("invert", invert, true);
+    nh.param<double>("voxel_size", voxel_size, 0.01);
     nh.param<double>("distance_threshold", distance_threshold, 0.01);
+    nh.param<std::string>("input", input_topic, "/camera/depth/color/points");
+    nh.param<std::string>("output", output_topic, "/ground_plane_segmentation/points");
+    nh.param<std::string>("plane_coefficients", coefficients_topic, "/ground_plane_segmentation/coefficients");
+
+    // Display params
+    std::cout << "\nInvert Segmentation: " << invert << std::endl;
+    std::cout << "Voxel Size: " << voxel_size << std::endl;
+    std::cout << "Distance Threshold: " << distance_threshold << std::endl;
+    std::cout << "Input Topic: " << input_topic << std::endl;
+    std::cout << "Output Topic: " << output_topic << std::endl;
+    std::cout << "Co-efficients Topic: " << coefficients_topic << std::endl;
 
     // Create a ROS subscriber for the input point cloud
     ros::Subscriber sub = nh.subscribe(input_topic, 1, callback);