Add design doc for actual_arrel_segmentationn issue #569

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. 

igvc_msgs/CMakeLists.txt

@@ -22,6 +22,7 @@ add_message_files(
     igvc_path.msg
     trajectory_point.msg
     trajectory.msg
+    barrels.msg
 )
 
 add_action_files(

igvc_msgs/msg/barrels.msg

@@ -0,0 +1,2 @@
+Header header
+geometry_msgs/Point[] barrels

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 <ros/ros.h>
+#include <tf/transform_listener.h>
+
+#include <pointcloud_filter/back_filter/back_filter.h>
+#include <pointcloud_filter/ground_filter/ground_filter.h>
+#include <pointcloud_filter/pointcloud_filter_config.h>
+#include <pointcloud_filter/radius_filter/radius_filter.h>
+#include <pointcloud_filter/raycast_filter/raycast_filter.h>
+#include <pointcloud_filter/tf_transform_filter/tf_transform_filter.h>
+
+namespace pointcloud_filter
+{
+class PointcloudFilter
+{
+public:
+  using PointCloud = pcl::PointCloud<velodyne_pointcloud::PointXYZIR>;
+
+  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

igvc_perception/launch/actual_barrel_segmentation.launch

@@ -0,0 +1,16 @@
+<?xml version="1.0"?>
+
+<launch>
+  <node name="barrel_seg" pkg="igvc_perception" type="barrel_seg" output="screen">
+    <param name="clusterTolerance" type="double" value="0.3" />
+    <param name="minClusterSize" type="int" value="50" />
+    <param name="maxClusterSize" type="int" value="500" />
+    <param name="cylinderMinRad" type="double" value="0.22" />
+    <param name="cylinderMaxRad" type="double" value="0.4" />
+    <param name="cylinderDistThres" type="double" value="0.1" />
+    <param name="cylinderNormalDistWeight" type="double" value="0.1" />
+    <param name="showCyl" type="bool" value="true" />
+    <param name="showClus" type="bool" value="true" />
+    <param name="showInlier" type="bool" value="false" />
+  </node>
+</launch>

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})