Add design doc for actual_arrel_segmentationn issue #569#602
Add design doc for actual_arrel_segmentationn issue #569#602
Conversation
oswinso
left a comment
There was a problem hiding this comment.
I'd like a bit more detail with regards to how the clustering algorithms would work.
| - 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. |
There was a problem hiding this comment.
I think you should describe the actual clustering algorithm(s) that you are thinking of using a bit more.
oswinso
left a comment
There was a problem hiding this comment.
Woops meant to say request changes
oswinso
left a comment
There was a problem hiding this comment.
One more small thing. Check the github render to see the formatting issues.
| ''' | ||
| 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 | ||
| ''' |
There was a problem hiding this comment.
I don't think this is displaying properly in markdown.
oswinso
left a comment
There was a problem hiding this comment.
I think you meant to put these changes on a different branch.
| #include <ros/ros.h> | ||
| #include <pcl/PointIndices.h> | ||
| #include <pcl/segmentation/extract_clusters.h> | ||
|
|
||
| ros::Publisher pub; | ||
| std::string pub_topic = ""; | ||
| std::string sub_topic = ""; | ||
|
|
||
| double clusterTolerance; | ||
| int minClusterSize; | ||
| int maxClusterSize; | ||
|
|
||
| void clusteringCallBack(pcl::PointCloud<pcl::PointXYZ>::Ptr pointcloud) { | ||
| pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ>); | ||
| tree->setInputCloud (pointcloud); | ||
|
|
||
| std::vector<pcl::PointIndices> cluster_indices; | ||
| pcl::EuclideanClusterExtraction<pcl::PointXYZ> 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<std::vector<pcl::PointIndices>>(pub_topic, 1); | ||
| ros::Subscriber pointcloud_sub = nh.subscribe(sub_topic, 1, clusteringCallBack); | ||
|
|
||
| ros::spin(); | ||
| } No newline at end of file |
There was a problem hiding this comment.
These changes should be put in a separate branch, not this one.
Add design doc for actual_arrel_segmentationn issue #569
Link to design document