diff --git a/documents/design/actual_barrel_segmentation.md b/documents/design/actual_barrel_segmentation.md new file mode 100644 index 000000000..c0dbbaf43 --- /dev/null +++ b/documents/design/actual_barrel_segmentation.md @@ -0,0 +1,62 @@ +# 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. +- 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 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) +- 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. + +Subtask 8 (4-5 weeks into 2019 Spring): Work on calculating and outputting metrics. 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/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/launch/actual_barrel_segmentation.launch b/igvc_perception/launch/actual_barrel_segmentation.launch new file mode 100644 index 000000000..1f1f9228d --- /dev/null +++ b/igvc_perception/launch/actual_barrel_segmentation.launch @@ -0,0 +1,16 @@ + + + + + + + + + + + + + + + + diff --git a/igvc_perception/src/pointcloud_filter/CMakeLists.txt b/igvc_perception/src/pointcloud_filter/CMakeLists.txt index 16ab3ad04..187c83809 100644 --- a/igvc_perception/src/pointcloud_filter/CMakeLists.txt +++ b/igvc_perception/src/pointcloud_filter/CMakeLists.txt @@ -27,3 +27,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 new file mode 100644 index 000000000..e84a8b4f7 --- /dev/null +++ b/igvc_perception/src/pointcloud_filter/actual_barrel_segmentation/actual_barrel_segmentation.cpp @@ -0,0 +1,269 @@ +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + + +ros::Publisher clusterCountPub; +ros::Publisher clusterVisPub; +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); + ec.setSearchMethod (tree); + 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 clusterPoints; + visualization_msgs::Marker inlierPoints; + + //PointClout Init + pcl::PointCloud curr_cloud; + + //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]; + + geometry_msgs::Point p; + p.x = curr.x; + p.y = curr.y; + p.z = curr.z; + clusterPoints.points.push_back(p); + + currClus->push_back(curr); + } + // 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) +{ + ros::init(argc, argv, "actualBarrelSegmentation"); + + ros::NodeHandle nh; + + ros::NodeHandle pnh{"~"}; + 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(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