From 6fb9fcf160231a706cf4f892901e82f786641d44 Mon Sep 17 00:00:00 2001 From: Frank Date: Wed, 20 Nov 2019 16:37:23 -0800 Subject: [PATCH 01/10] Add design doc for actual_arrel_segmentationn issue #569 --- .../design/actual_barrel_segmentation.md | 46 +++++++++++++++++++ 1 file changed, 46 insertions(+) create mode 100644 documents/design/actual_barrel_segmentation.md diff --git a/documents/design/actual_barrel_segmentation.md b/documents/design/actual_barrel_segmentation.md new file mode 100644 index 000000000..6afa27fbe --- /dev/null +++ b/documents/design/actual_barrel_segmentation.md @@ -0,0 +1,46 @@ +# Actual barrel segmentation + +*Issue #569* + +**Author:** +- Zhiyan Zhou (Frank) + +## The Problem +Cluster points from non-ground pointcloud into different groups representing different objects. We can potentially use the information for things like SLAM. +End goal will be separating barrels into individual groups and extracting information such as position, Num of barrels etc. + +## Proposed Solution +- Create a subscriber that suscribes to the filtered non-ground data. +- Use the clustering algorithm inside PCL. +- (If necessary)Program a clustering algorithm that take in a pointcloud and returns a clustered pointcloud. +- Visualized clustering result. +- Tune the parameter of the optimal algorithm if necessary. +- Use the clusters to calculate metrics (ex: position, num of barrels/) +- Publish the metrics to the corresponding topic. + +## Questions & Research +- What are the common clustering algorithms? +- Any known implementations in ROS/C++? +- /igvc-software/documents/research/barrel_detection.md +- https://www.ncbi.nlm.nih.gov/pmc/articles/PMC6338962/pdf/sensors-19-00172.pdf + +## Overall Scope + +### Affected Packages +- /igvc-software/igvc_perception/src/pointcloud_filter/ + +### Schedule + +Subtask 1 (12/24/2019): Program a subscriber that subscribes pointcloud data. + +Subtask 2 (2-3 week into 2019 Spring): Use the clustering algorithm inside PCL to cluster points. + +Subtask 3 (/): (If necessary)Go over the paper listed: https://www.ncbi.nlm.nih.gov/pmc/articles/PMC6338962/pdf/sensors-19-00172.pdf + +Subtask 4 (/): (If necessary) Implement a DBSCAN algorithm + +Subtask 5 (/): (If necessary) Test the original DBSCAN algorithm under a known distance + +Subtask 6 (/): (If necessary) Work on Auto Hyperparameter Tuning. + +Subtask 7 (3-4 weeks into 2019 Spring): Test and tune the algorithm under simulated data. From 5006fa4fd47b281a68cdb658abb9aeacf0d86549 Mon Sep 17 00:00:00 2001 From: Frank Date: Wed, 20 Nov 2019 17:24:02 -0800 Subject: [PATCH 02/10] Update actual barrel segmentation: Add short description on the clustering algorithm --- documents/design/actual_barrel_segmentation.md | 1 + 1 file changed, 1 insertion(+) diff --git a/documents/design/actual_barrel_segmentation.md b/documents/design/actual_barrel_segmentation.md index 6afa27fbe..86037ab84 100644 --- a/documents/design/actual_barrel_segmentation.md +++ b/documents/design/actual_barrel_segmentation.md @@ -13,6 +13,7 @@ End goal will be separating barrels into individual groups and extracting inform - Create a subscriber that suscribes to the filtered non-ground data. - Use the clustering algorithm inside PCL. - (If necessary)Program a clustering algorithm that take in a pointcloud and returns a clustered pointcloud. +- Both clustering algorithm is based on the same general idea. You pick a point in a pointcloud and get all the point within a certain radius. Repeat the last process recursively with all the points inside the radius are processed. This finish the segmentation of a cluster. After this, you pick another point and repeat this process. - Visualized clustering result. - Tune the parameter of the optimal algorithm if necessary. - Use the clusters to calculate metrics (ex: position, num of barrels/) From ffc6b82e9b8d05069c4432e4197cb8de52ae3bf9 Mon Sep 17 00:00:00 2001 From: Frank Date: Wed, 20 Nov 2019 22:25:38 -0800 Subject: [PATCH 03/10] Add pseudocode for clustering algorithm --- documents/design/actual_barrel_segmentation.md | 13 ++++++++++++- 1 file changed, 12 insertions(+), 1 deletion(-) diff --git a/documents/design/actual_barrel_segmentation.md b/documents/design/actual_barrel_segmentation.md index 86037ab84..02a638ddd 100644 --- a/documents/design/actual_barrel_segmentation.md +++ b/documents/design/actual_barrel_segmentation.md @@ -13,7 +13,18 @@ End goal will be separating barrels into individual groups and extracting inform - Create a subscriber that suscribes to the filtered non-ground data. - Use the clustering algorithm inside PCL. - (If necessary)Program a clustering algorithm that take in a pointcloud and returns a clustered pointcloud. -- Both clustering algorithm is based on the same general idea. You pick a point in a pointcloud and get all the point within a certain radius. Repeat the last process recursively with all the points inside the radius are processed. This finish the segmentation of a cluster. After this, you pick another point and repeat this process. +- The way the clustering algorithm works: (http://pointclouds.org/documentation/tutorials/cluster_extraction.php) +''' +1. create a Kd-tree representation for the input point cloud dataset P; +2. set up an empty list of clusters C, and a queue of the points that need to be checked Q; +3. then for every point pi in P, perform the following steps: + - add pi to the current queue Q; + - for every point pi in Q do: + - search for the set P^i_k of point neighbors of pi in a sphere with radius r < dth; + - for every neighbor pki in P^k_i, check if the point has already been processed, and if not add it to Q; + when the list of all points in Q has been processed, add Q to the list of clusters C, and reset Q to an empty list +4. the algorithm terminates when all points \boldsymbol{p}_i \in P have been processed and are now part of the list of point clusters C +''' - Visualized clustering result. - Tune the parameter of the optimal algorithm if necessary. - Use the clusters to calculate metrics (ex: position, num of barrels/) From 9f9a544d91a1c19e853e3262becb9fa59026b4b2 Mon Sep 17 00:00:00 2001 From: Frank Date: Thu, 21 Nov 2019 19:30:25 -0800 Subject: [PATCH 04/10] Fix Rendering --- documents/design/actual_barrel_segmentation.md | 2 ++ 1 file changed, 2 insertions(+) diff --git a/documents/design/actual_barrel_segmentation.md b/documents/design/actual_barrel_segmentation.md index 02a638ddd..e6339c752 100644 --- a/documents/design/actual_barrel_segmentation.md +++ b/documents/design/actual_barrel_segmentation.md @@ -14,6 +14,7 @@ End goal will be separating barrels into individual groups and extracting inform - Use the clustering algorithm inside PCL. - (If necessary)Program a clustering algorithm that take in a pointcloud and returns a clustered pointcloud. - The way the clustering algorithm works: (http://pointclouds.org/documentation/tutorials/cluster_extraction.php) + ''' 1. create a Kd-tree representation for the input point cloud dataset P; 2. set up an empty list of clusters C, and a queue of the points that need to be checked Q; @@ -25,6 +26,7 @@ End goal will be separating barrels into individual groups and extracting inform when the list of all points in Q has been processed, add Q to the list of clusters C, and reset Q to an empty list 4. the algorithm terminates when all points \boldsymbol{p}_i \in P have been processed and are now part of the list of point clusters C ''' + - Visualized clustering result. - Tune the parameter of the optimal algorithm if necessary. - Use the clusters to calculate metrics (ex: position, num of barrels/) From 10028a9d2f4effde01690745cab09aef41e2b722 Mon Sep 17 00:00:00 2001 From: Frank Date: Thu, 21 Nov 2019 19:32:56 -0800 Subject: [PATCH 05/10] Fix Rendering --- documents/design/actual_barrel_segmentation.md | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/documents/design/actual_barrel_segmentation.md b/documents/design/actual_barrel_segmentation.md index e6339c752..6728538a9 100644 --- a/documents/design/actual_barrel_segmentation.md +++ b/documents/design/actual_barrel_segmentation.md @@ -15,7 +15,7 @@ End goal will be separating barrels into individual groups and extracting inform - (If necessary)Program a clustering algorithm that take in a pointcloud and returns a clustered pointcloud. - The way the clustering algorithm works: (http://pointclouds.org/documentation/tutorials/cluster_extraction.php) -''' +``` 1. create a Kd-tree representation for the input point cloud dataset P; 2. set up an empty list of clusters C, and a queue of the points that need to be checked Q; 3. then for every point pi in P, perform the following steps: @@ -24,8 +24,8 @@ End goal will be separating barrels into individual groups and extracting inform - search for the set P^i_k of point neighbors of pi in a sphere with radius r < dth; - for every neighbor pki in P^k_i, check if the point has already been processed, and if not add it to Q; when the list of all points in Q has been processed, add Q to the list of clusters C, and reset Q to an empty list -4. the algorithm terminates when all points \boldsymbol{p}_i \in P have been processed and are now part of the list of point clusters C -''' +4. the algorithm terminates when all points P_i in P have been processed and are now part of the list of point clusters C +``` - Visualized clustering result. - Tune the parameter of the optimal algorithm if necessary. From a187635745134d006ca8190ed7da4321241af424 Mon Sep 17 00:00:00 2001 From: Frank Date: Thu, 21 Nov 2019 19:35:13 -0800 Subject: [PATCH 06/10] Fix Rendering --- documents/design/actual_barrel_segmentation.md | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/documents/design/actual_barrel_segmentation.md b/documents/design/actual_barrel_segmentation.md index 6728538a9..c0dbbaf43 100644 --- a/documents/design/actual_barrel_segmentation.md +++ b/documents/design/actual_barrel_segmentation.md @@ -29,7 +29,7 @@ End goal will be separating barrels into individual groups and extracting inform - Visualized clustering result. - Tune the parameter of the optimal algorithm if necessary. -- Use the clusters to calculate metrics (ex: position, num of barrels/) +- Use the clusters to calculate metrics (ex: position, num of barrels) - Publish the metrics to the corresponding topic. ## Questions & Research @@ -58,3 +58,5 @@ Subtask 5 (/): (If necessary) Test the original DBSCAN algorithm under a known d Subtask 6 (/): (If necessary) Work on Auto Hyperparameter Tuning. Subtask 7 (3-4 weeks into 2019 Spring): Test and tune the algorithm under simulated data. + +Subtask 8 (4-5 weeks into 2019 Spring): Work on calculating and outputting metrics. From 9c228889b04480d83e40f20498e3d8d58c61d4ce Mon Sep 17 00:00:00 2001 From: Frank Date: Sun, 24 Nov 2019 13:30:21 -0800 Subject: [PATCH 07/10] Add a file to test the build in Euclidean Cluster Extraction from PCL --- .../actual_barrel_segmentation.cpp | 44 +++++++++++++++++++ 1 file changed, 44 insertions(+) create mode 100644 igvc_perception/src/pointcloud_filter/actual_barrel_segmentation/actual_barrel_segmentation.cpp diff --git a/igvc_perception/src/pointcloud_filter/actual_barrel_segmentation/actual_barrel_segmentation.cpp b/igvc_perception/src/pointcloud_filter/actual_barrel_segmentation/actual_barrel_segmentation.cpp new file mode 100644 index 000000000..a4323a8e8 --- /dev/null +++ b/igvc_perception/src/pointcloud_filter/actual_barrel_segmentation/actual_barrel_segmentation.cpp @@ -0,0 +1,44 @@ +#include +#include +#include + +ros::Publisher pub; +std::string pub_topic = ""; +std::string sub_topic = ""; + +double clusterTolerance; +int minClusterSize; +int maxClusterSize; + +void clusteringCallBack(pcl::PointCloud::Ptr pointcloud) { + pcl::search::KdTree::Ptr tree (new pcl::search::KdTree); + tree->setInputCloud (pointcloud); + + std::vector cluster_indices; + pcl::EuclideanClusterExtraction ec; + ec.setClusterTolerance (clusterTolerance); + ec.setMinClusterSize (minClusterSize); + ec.setMaxClusterSize (maxClusterSize); + ec.setSearchMethod (tree); + ec.setInputCloud (pointcloud); + ec.extract (cluster_indices); + + pub.publish(cluster_indices); +} + +int main(int argc, char** argv) +{ + ros::init(argc, argv, "week5"); + + ros::NodeHandle nh; + + ros::NodeHandle pnh{"~"}; + pnh.getParam("clusterTolerance", clusterTolerance); + pnh.getParam("minClusterSize", minClusterSize); + pnh.getParam("maxClusterSize", maxClusterSize); + + pub = nh.advertise>(pub_topic, 1); + ros::Subscriber pointcloud_sub = nh.subscribe(sub_topic, 1, clusteringCallBack); + + ros::spin(); +} \ No newline at end of file From df14ead0f11926d7a603b0800348656992987eac Mon Sep 17 00:00:00 2001 From: Frank Date: Sun, 24 Nov 2019 13:32:32 -0800 Subject: [PATCH 08/10] Update Cmake file and add header file for actual barrel detection --- .../actual_barrel_segmentation.h | 48 +++++++++++++++++++ .../src/pointcloud_filter/CMakeLists.txt | 3 +- 2 files changed, 50 insertions(+), 1 deletion(-) create mode 100644 igvc_perception/include/pointcloud_filter/actual_barrel_segmentation.h diff --git a/igvc_perception/include/pointcloud_filter/actual_barrel_segmentation.h b/igvc_perception/include/pointcloud_filter/actual_barrel_segmentation.h new file mode 100644 index 000000000..9a91b57c0 --- /dev/null +++ b/igvc_perception/include/pointcloud_filter/actual_barrel_segmentation.h @@ -0,0 +1,48 @@ +#ifndef SRC_ACTUAL_BARREL_SEGMENTATION_H +#define SRC_ACTUAL_BARREL_SEGMENTATION_H + +#include +#include + +#include +#include +#include +#include +#include +#include + +namespace pointcloud_filter +{ +class PointcloudFilter +{ +public: + using PointCloud = pcl::PointCloud; + + PointcloudFilter(const ros::NodeHandle& nh = {}, const ros::NodeHandle& private_nh = { "~" }); + +private: + ros::NodeHandle nh_; + ros::NodeHandle private_nh_; + PointcloudFilterConfig config_; + + tf2_ros::Buffer buffer_; + tf2_ros::TransformListener listener_; + + BackFilter back_filter_; + RadiusFilter radius_filter_; + TFTransformFilter tf_transform_filter_; + GroundFilter ground_filter_; + RaycastFilter raycast_filter_; + + ros::Subscriber raw_pointcloud_sub_; + + ros::Publisher transformed_pointcloud_pub_; + ros::Publisher occupied_pointcloud_pub_; + ros::Publisher free_pointcloud_pub_; + + void setupPubSub(); + void pointcloudCallback(const PointCloud::ConstPtr& raw_pointcloud); +}; +} // namespace pointcloud_filter + +#endif //SRC_ACTUAL_BARREL_SEGMENTATION_H diff --git a/igvc_perception/src/pointcloud_filter/CMakeLists.txt b/igvc_perception/src/pointcloud_filter/CMakeLists.txt index ea19baef7..282e35373 100644 --- a/igvc_perception/src/pointcloud_filter/CMakeLists.txt +++ b/igvc_perception/src/pointcloud_filter/CMakeLists.txt @@ -11,7 +11,8 @@ add_library(pointcloud_filter_lib STATIC tf_transform_filter/tf_transform_filter.cpp raycast_filter/raycast_filter_config.cpp raycast_filter/raycast_filter.cpp - ) + actual_barrel_segmentation/actual_barrel_segmentation.cpp + ) add_dependencies(pointcloud_filter_lib ${catkin_EXPORTED_TARGETS}) target_link_libraries(pointcloud_filter_lib ${catkin_LIBRARIES} ${PCL_LIBRARIES}) From ced875c121a70588dbd58d6c2ec427225069695a Mon Sep 17 00:00:00 2001 From: Frank Date: Wed, 15 Jan 2020 16:34:57 -0800 Subject: [PATCH 09/10] Finish up the visualization --- .../launch/actual_barrel_segmentation.launch | 9 ++ .../src/pointcloud_filter/CMakeLists.txt | 4 +- .../actual_barrel_segmentation.cpp | 90 +++++++++++++++++-- 3 files changed, 93 insertions(+), 10 deletions(-) create mode 100644 igvc_perception/launch/actual_barrel_segmentation.launch diff --git a/igvc_perception/launch/actual_barrel_segmentation.launch b/igvc_perception/launch/actual_barrel_segmentation.launch new file mode 100644 index 000000000..27801e595 --- /dev/null +++ b/igvc_perception/launch/actual_barrel_segmentation.launch @@ -0,0 +1,9 @@ + + + + + + + + + diff --git a/igvc_perception/src/pointcloud_filter/CMakeLists.txt b/igvc_perception/src/pointcloud_filter/CMakeLists.txt index 282e35373..55fc510c3 100644 --- a/igvc_perception/src/pointcloud_filter/CMakeLists.txt +++ b/igvc_perception/src/pointcloud_filter/CMakeLists.txt @@ -11,7 +11,6 @@ add_library(pointcloud_filter_lib STATIC tf_transform_filter/tf_transform_filter.cpp raycast_filter/raycast_filter_config.cpp raycast_filter/raycast_filter.cpp - actual_barrel_segmentation/actual_barrel_segmentation.cpp ) add_dependencies(pointcloud_filter_lib ${catkin_EXPORTED_TARGETS}) target_link_libraries(pointcloud_filter_lib ${catkin_LIBRARIES} ${PCL_LIBRARIES}) @@ -26,3 +25,6 @@ install( LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} ) + +add_executable(barrel_seg actual_barrel_segmentation/actual_barrel_segmentation.cpp) +target_link_libraries(barrel_seg ${catkin_LIBRARIES} ${PCL_LIBRARIES}) \ No newline at end of file diff --git a/igvc_perception/src/pointcloud_filter/actual_barrel_segmentation/actual_barrel_segmentation.cpp b/igvc_perception/src/pointcloud_filter/actual_barrel_segmentation/actual_barrel_segmentation.cpp index a4323a8e8..4d09a196b 100644 --- a/igvc_perception/src/pointcloud_filter/actual_barrel_segmentation/actual_barrel_segmentation.cpp +++ b/igvc_perception/src/pointcloud_filter/actual_barrel_segmentation/actual_barrel_segmentation.cpp @@ -1,18 +1,40 @@ #include #include #include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include -ros::Publisher pub; -std::string pub_topic = ""; -std::string sub_topic = ""; +ros::Publisher clusterCountPub; +ros::Publisher clusterVisPub; +std::string count_pub_topic = "/countOutput"; +std::string vis_pub_topic = "/vizOutput"; +std::string sub_topic = "/nonground"; double clusterTolerance; int minClusterSize; int maxClusterSize; -void clusteringCallBack(pcl::PointCloud::Ptr pointcloud) { +void clusteringCallBack(const sensor_msgs::PointCloud2ConstPtr& pointcloudMsg) { + pcl::PCLPointCloud2* cloud = new pcl::PCLPointCloud2; + pcl_conversions::toPCL(*pointcloudMsg, *cloud); + + pcl::PointCloud *cloudXYZ = new pcl::PointCloud; + pcl::PointCloud::Ptr cloudXYZPtr (cloudXYZ); + + pcl::fromPCLPointCloud2(*cloud, *cloudXYZPtr); + pcl::search::KdTree::Ptr tree (new pcl::search::KdTree); - tree->setInputCloud (pointcloud); + tree->setInputCloud (cloudXYZPtr); std::vector cluster_indices; pcl::EuclideanClusterExtraction ec; @@ -20,15 +42,64 @@ void clusteringCallBack(pcl::PointCloud::Ptr pointcloud) { ec.setMinClusterSize (minClusterSize); ec.setMaxClusterSize (maxClusterSize); ec.setSearchMethod (tree); - ec.setInputCloud (pointcloud); + ec.setInputCloud (cloudXYZPtr); ec.extract (cluster_indices); - pub.publish(cluster_indices); + std_msgs::Int32 clusterCount; + clusterCount.data = cluster_indices.size(); + + clusterCountPub.publish(clusterCount); + + visualization_msgs::MarkerArray clusters_vis; + + for(int clusterInd = 0; clusterInd < (int)cluster_indices.size(); clusterInd++) { + //Visualization Init + visualization_msgs::Marker points; + + //PointClout Init + pcl::PointCloud curr_cloud; + + points.header.frame_id = "/lidar"; + points.header.stamp = ros::Time::now(); + points.ns = "barrel_segmentation" + std::to_string(clusterInd); + points.action = visualization_msgs::Marker::ADD; + points.pose.orientation.w = 1.0; + points.id = 0; + points.type = visualization_msgs::Marker::POINTS; + points.scale.x = 0.01; + points.scale.y = 0.01; + + points.color.a = 1.0; + points.color.r = clusterInd*0.25; + points.color.g = clusterInd*0.25; + points.color.b = clusterInd*0.25; + + for (int ptInd : cluster_indices.at(clusterInd).indices) + { + pcl::PointXYZ curr = cloudXYZPtr->points[ptInd]; + pcl::PointCloud *currClus = new pcl::PointCloud; + pcl::PointCloud::Ptr cloudXYZPtr (currClus); + pcl::SACSegmentationFromNormals seg; + pcl::PointIndices::Ptr inliers_cylinder (new pcl::PointIndices); + pcl::ModelCoefficients::Ptr coefficients_cylinder (new pcl::ModelCoefficients); + + geometry_msgs::Point p; + p.x = curr.x; + p.y = curr.y; + p.z = curr.z; + points.points.push_back(p); + + currClus->push_back(curr); + } + clusters_vis.markers.push_back(points); + } + + clusterVisPub.publish(clusters_vis); } int main(int argc, char** argv) { - ros::init(argc, argv, "week5"); + ros::init(argc, argv, "actualBarrelSegmentation"); ros::NodeHandle nh; @@ -37,7 +108,8 @@ int main(int argc, char** argv) pnh.getParam("minClusterSize", minClusterSize); pnh.getParam("maxClusterSize", maxClusterSize); - pub = nh.advertise>(pub_topic, 1); + clusterCountPub = nh.advertise(count_pub_topic, 1); + clusterVisPub = nh.advertise(vis_pub_topic, 1); ros::Subscriber pointcloud_sub = nh.subscribe(sub_topic, 1, clusteringCallBack); ros::spin(); From f11b862b900f1fb1aca6b095924db285a81b6839 Mon Sep 17 00:00:00 2001 From: Frank Date: Sun, 2 Feb 2020 13:22:50 -0800 Subject: [PATCH 10/10] Finish the barrel detention (Single only) --- igvc_msgs/CMakeLists.txt | 1 + igvc_msgs/msg/barrels.msg | 2 + .../launch/actual_barrel_segmentation.launch | 9 +- .../actual_barrel_segmentation.cpp | 219 +++++++++++++++--- 4 files changed, 197 insertions(+), 34 deletions(-) create mode 100644 igvc_msgs/msg/barrels.msg diff --git a/igvc_msgs/CMakeLists.txt b/igvc_msgs/CMakeLists.txt index e50e1ce04..8516b7cbb 100644 --- a/igvc_msgs/CMakeLists.txt +++ b/igvc_msgs/CMakeLists.txt @@ -22,6 +22,7 @@ add_message_files( igvc_path.msg trajectory_point.msg trajectory.msg + barrels.msg ) add_action_files( diff --git a/igvc_msgs/msg/barrels.msg b/igvc_msgs/msg/barrels.msg new file mode 100644 index 000000000..fa383f39f --- /dev/null +++ b/igvc_msgs/msg/barrels.msg @@ -0,0 +1,2 @@ +Header header +geometry_msgs/Point[] barrels diff --git a/igvc_perception/launch/actual_barrel_segmentation.launch b/igvc_perception/launch/actual_barrel_segmentation.launch index 27801e595..1f1f9228d 100644 --- a/igvc_perception/launch/actual_barrel_segmentation.launch +++ b/igvc_perception/launch/actual_barrel_segmentation.launch @@ -3,7 +3,14 @@ - + + + + + + + + diff --git a/igvc_perception/src/pointcloud_filter/actual_barrel_segmentation/actual_barrel_segmentation.cpp b/igvc_perception/src/pointcloud_filter/actual_barrel_segmentation/actual_barrel_segmentation.cpp index 4d09a196b..e84a8b4f7 100644 --- a/igvc_perception/src/pointcloud_filter/actual_barrel_segmentation/actual_barrel_segmentation.cpp +++ b/igvc_perception/src/pointcloud_filter/actual_barrel_segmentation/actual_barrel_segmentation.cpp @@ -13,31 +13,79 @@ #include #include #include +#include +#include + ros::Publisher clusterCountPub; ros::Publisher clusterVisPub; -std::string count_pub_topic = "/countOutput"; -std::string vis_pub_topic = "/vizOutput"; -std::string sub_topic = "/nonground"; +ros::Publisher barrelInfoPub; +std::string subTopic = "/nonground"; +std::string countPubTopic = "/countOutput"; +std::string visPubTopic = "/vizOutput"; +std::string barrelPubTopic = "/barrelPos"; double clusterTolerance; int minClusterSize; int maxClusterSize; +double cylinderMinRad; +double cylinderMaxRad; +double cylinderDistThres; +double cylinderNormalDistWeight; +bool showCyl; +bool showClus; +bool showInlier; + +void getCylinder(pcl::PointCloud::Ptr cloud, pcl::ModelCoefficients::Ptr coefficients_cylinder, + pcl::PointIndices::Ptr inliers_cylinder) +{ + //Cylinder Fitting Init + pcl::NormalEstimation ne; + pcl::SACSegmentationFromNormals seg; + pcl::ExtractIndices extract; + pcl::ExtractIndices extract_normals; + pcl::search::KdTree::Ptr tree (new pcl::search::KdTree ()); + + //Cylinder Fitting Variables + pcl::PointCloud::Ptr cloud_normals (new pcl::PointCloud); + pcl::ModelCoefficients::Ptr coefficients_plane (new pcl::ModelCoefficients); + + // Estimate point normals + ne.setSearchMethod (tree); + ne.setInputCloud (cloud); + ne.setKSearch (50); + ne.compute (*cloud_normals); + + // Create the segmentation object for cylinder segmentation and set all the parameters + seg.setOptimizeCoefficients (true); + seg.setModelType (pcl::SACMODEL_CYLINDER); + seg.setMethodType (pcl::SAC_RANSAC); + seg.setNormalDistanceWeight (cylinderNormalDistWeight); + seg.setMaxIterations (10000); + seg.setDistanceThreshold (cylinderDistThres); + seg.setRadiusLimits (cylinderMinRad, cylinderMaxRad); + seg.setInputCloud (cloud); + seg.setInputNormals (cloud_normals); + + // Obtain the cylinder inliers and coefficients + seg.segment (*inliers_cylinder, *coefficients_cylinder); +} void clusteringCallBack(const sensor_msgs::PointCloud2ConstPtr& pointcloudMsg) { + // Converting Sensor message to pcl::PCLPointCloud2 pcl::PCLPointCloud2* cloud = new pcl::PCLPointCloud2; pcl_conversions::toPCL(*pointcloudMsg, *cloud); - pcl::PointCloud *cloudXYZ = new pcl::PointCloud; pcl::PointCloud::Ptr cloudXYZPtr (cloudXYZ); - pcl::fromPCLPointCloud2(*cloud, *cloudXYZPtr); + // Clustering Init pcl::search::KdTree::Ptr tree (new pcl::search::KdTree); tree->setInputCloud (cloudXYZPtr); - std::vector cluster_indices; pcl::EuclideanClusterExtraction ec; + + // Clustering ec.setClusterTolerance (clusterTolerance); ec.setMinClusterSize (minClusterSize); ec.setMaxClusterSize (maxClusterSize); @@ -45,56 +93,153 @@ void clusteringCallBack(const sensor_msgs::PointCloud2ConstPtr& pointcloudMsg) { ec.setInputCloud (cloudXYZPtr); ec.extract (cluster_indices); + // Publish the number of clusters std_msgs::Int32 clusterCount; clusterCount.data = cluster_indices.size(); - clusterCountPub.publish(clusterCount); + // Visualization Message and Barrel Info Init visualization_msgs::MarkerArray clusters_vis; + igvc_msgs::barrels barrelInfo; for(int clusterInd = 0; clusterInd < (int)cluster_indices.size(); clusterInd++) { //Visualization Init - visualization_msgs::Marker points; + visualization_msgs::Marker clusterPoints; + visualization_msgs::Marker inlierPoints; //PointClout Init pcl::PointCloud curr_cloud; - points.header.frame_id = "/lidar"; - points.header.stamp = ros::Time::now(); - points.ns = "barrel_segmentation" + std::to_string(clusterInd); - points.action = visualization_msgs::Marker::ADD; - points.pose.orientation.w = 1.0; - points.id = 0; - points.type = visualization_msgs::Marker::POINTS; - points.scale.x = 0.01; - points.scale.y = 0.01; - - points.color.a = 1.0; - points.color.r = clusterInd*0.25; - points.color.g = clusterInd*0.25; - points.color.b = clusterInd*0.25; + //Cluster Storing + pcl::PointCloud *currClus = new pcl::PointCloud; + pcl::PointCloud::Ptr currCloudXYZPtr (currClus); + + // Barrel Points + clusterPoints.header.frame_id = "/lidar"; + clusterPoints.header.stamp = ros::Time::now(); + clusterPoints.ns = "Barrel_" + std::to_string(clusterInd); + clusterPoints.action = visualization_msgs::Marker::ADD; + clusterPoints.pose.orientation.w = 1.0; + clusterPoints.id = 0; + clusterPoints.type = visualization_msgs::Marker::POINTS; + clusterPoints.scale.x = 0.01; + clusterPoints.scale.y = 0.01; + + clusterPoints.color.a = 1.0; + clusterPoints.color.r = clusterInd * 0.25; + clusterPoints.color.g = clusterInd * 0.25; + clusterPoints.color.b = clusterInd * 0.25; + clusterPoints.lifetime = ros::Duration(0.1); for (int ptInd : cluster_indices.at(clusterInd).indices) { pcl::PointXYZ curr = cloudXYZPtr->points[ptInd]; - pcl::PointCloud *currClus = new pcl::PointCloud; - pcl::PointCloud::Ptr cloudXYZPtr (currClus); - pcl::SACSegmentationFromNormals seg; - pcl::PointIndices::Ptr inliers_cylinder (new pcl::PointIndices); - pcl::ModelCoefficients::Ptr coefficients_cylinder (new pcl::ModelCoefficients); geometry_msgs::Point p; p.x = curr.x; p.y = curr.y; p.z = curr.z; - points.points.push_back(p); + clusterPoints.points.push_back(p); currClus->push_back(curr); } - clusters_vis.markers.push_back(points); - } + // Variables to store cylinder fitting result + pcl::ModelCoefficients::Ptr coefficients_cylinder (new pcl::ModelCoefficients); + pcl::PointIndices::Ptr inliers_cylinder (new pcl::PointIndices); + // Get Cylinder Fitting from the current cluster + getCylinder(currCloudXYZPtr, coefficients_cylinder, inliers_cylinder); + if (coefficients_cylinder->values.size() == 0) { + continue; + } + + // Extract Cylinder Info + // X = a1 + v1*t + // Y = a2 + v2*t + // Z = a3 + v3*t + double a1 = coefficients_cylinder->values[0]; + double a2 = coefficients_cylinder->values[1]; + double a3 = coefficients_cylinder->values[2]; + double v1 = coefficients_cylinder->values[3]; + double v2 = coefficients_cylinder->values[4]; + double v3 = coefficients_cylinder->values[5]; + double r = coefficients_cylinder->values[6]; + + // Solve t for Z = 0 + double t = - a3/v3; + + // Cylinder Inlier Points + inlierPoints.header.frame_id = "/lidar"; + inlierPoints.header.stamp = ros::Time::now(); + inlierPoints.ns = "CylPoints_" + std::to_string(clusterInd); + inlierPoints.action = visualization_msgs::Marker::ADD; + inlierPoints.pose.orientation.w = 1.0; + inlierPoints.id = 0; + inlierPoints.type = visualization_msgs::Marker::POINTS; + inlierPoints.scale.x = 0.01; + inlierPoints.scale.y = 0.01; + inlierPoints.color.a = 1.0; + inlierPoints.color.r = 1.0; + inlierPoints.color.g = 0.0; + inlierPoints.color.b = 0.0; + inlierPoints.lifetime = ros::Duration(0.1); + + for (int ptInd : inliers_cylinder->indices) + { + pcl::PointXYZ curr = currCloudXYZPtr->points[ptInd]; + + geometry_msgs::Point pt_real; + pt_real.x = curr.x; + pt_real.y = curr.y; + pt_real.z = curr.z; + inlierPoints.points.push_back(pt_real); + } + + // Setting up the Cylinder Visualizer + visualization_msgs::Marker cylinder; + cylinder.header.frame_id = "/lidar"; + cylinder.header.stamp = ros::Time::now(); + cylinder.ns = "Cylinder_" + std::to_string(clusterInd);; + cylinder.id = 0; + cylinder.type = visualization_msgs::Marker::CYLINDER; + cylinder.action = visualization_msgs::Marker::ADD; + cylinder.pose.position.x = a1 + v1*t; + cylinder.pose.position.y = a2 + v2*t; + cylinder.pose.position.z = 0; + cylinder.pose.orientation.x = 0.0; + cylinder.pose.orientation.y = 0.0; + cylinder.pose.orientation.z = 0.0; + cylinder.pose.orientation.w = 1.0; + cylinder.scale.x = r; + cylinder.scale.y = r; + cylinder.scale.z = 1; + cylinder.color.r = 0.0f; + cylinder.color.g = 1.0f; + cylinder.color.b = 0.0f; + cylinder.color.a = 1.0; + cylinder.lifetime = ros::Duration(0.1); + + // Put barrel info into a ros message + geometry_msgs::Point currBarrel; + currBarrel.x = a1 + v1*t; + currBarrel.y = a2 + v2*t; + currBarrel.z = 0; + + barrelInfo.barrels.push_back(currBarrel); + + // Adding clusterPoints to Maker Array + if (showClus) { + clusters_vis.markers.push_back(clusterPoints); + } + if (showCyl) { + clusters_vis.markers.push_back(cylinder); + } + if (showInlier) { + clusters_vis.markers.push_back(inlierPoints); + } + } clusterVisPub.publish(clusters_vis); + barrelInfoPub.publish(barrelInfo); } int main(int argc, char** argv) @@ -107,10 +252,18 @@ int main(int argc, char** argv) pnh.getParam("clusterTolerance", clusterTolerance); pnh.getParam("minClusterSize", minClusterSize); pnh.getParam("maxClusterSize", maxClusterSize); + pnh.getParam("cylinderMinRad", cylinderMinRad); + pnh.getParam("cylinderMaxRad", cylinderMaxRad); + pnh.getParam("cylinderDistThres", cylinderDistThres); + pnh.getParam("cylinderNormalDistWeight", cylinderNormalDistWeight); + pnh.getParam("showCyl", showCyl); + pnh.getParam("showClus", showClus); + pnh.getParam("showInlier", showInlier); - clusterCountPub = nh.advertise(count_pub_topic, 1); - clusterVisPub = nh.advertise(vis_pub_topic, 1); - ros::Subscriber pointcloud_sub = nh.subscribe(sub_topic, 1, clusteringCallBack); + clusterCountPub = nh.advertise(countPubTopic, 1); + clusterVisPub = nh.advertise(visPubTopic, 1); + barrelInfoPub = nh.advertise(barrelPubTopic, 1); + ros::Subscriber pointcloud_sub = nh.subscribe(subTopic, 1, clusteringCallBack); ros::spin(); } \ No newline at end of file