EuclideanClusterExtraction in real-time with Rviz with publishing cluster centroid

asked 2022-04-08 07:45:09 -0500

hichriayoub gravatar image

updated 2022-04-17 10:57:40 -0500

lucasw gravatar image

Hello i am trying to work on UAV localization with ti_mmwave radars and my goal right now is to cluster the pointcloud in RVIZ i did it with using the nodelet but as i am not good with cpp, i wanted to use a normal node and change the current euclideancluster program with real-time one on rviz and publish the centroid while also drawing bounding boxes on the clusters but i couldn't i found many exemples out there in the internet but they are not complete and it's kinda hard to make them work by myself

so i am asking for your help guys and thank you so much in advance i would appreciate if anyone could help me write such program i will provide the exemples i found

this is an exemple of using EuclideanClusterExtraction With rviz

    sensor_msgs::PointCloud2::Ptr clusters (new sensor_msgs::PointCloud2);
pcl::PointCloud<pcl::PointXYZ>::Ptr input_cloud (new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr clustered_cloud (new pcl::PointCloud<pcl::PointXYZ>);
    /* Creating the KdTree from input point cloud*/
pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ>);
tree->setInputCloud (input_cloud);

/* Here we are creating a vector of PointIndices, which contains the actual index
 * information in a vector<int>. The indices of each detected cluster are saved here.
 * Cluster_indices is a vector containing one instance of PointIndices for each detected 
 * cluster. Cluster_indices[0] contain all indices of the first cluster in input point cloud.
std::vector<pcl::PointIndices> cluster_indices;
pcl::EuclideanClusterExtraction<pcl::PointXYZ> ec;
ec.setClusterTolerance (0.06); 
ec.setMinClusterSize (30);
ec.setMaxClusterSize (600);
ec.setSearchMethod (tree);
ec.setInputCloud (input_cloud);
/* Extract the clusters out of pc and save indices in cluster_indices.*/
ec.extract (cluster_indices);

/* To separate each cluster out of the vector<PointIndices> we have to 
 * iterate through cluster_indices, create a new PointCloud for each 
 * entry and write all points of the current cluster in the PointCloud. 

std::vector<pcl::PointIndices>::const_iterator it;
std::vector<int>::const_iterator pit;
for(it = cluster_indices.begin(); it != cluster_indices.end(); ++it) {
        pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_cluster (new pcl::PointCloud<pcl::PointXYZ>);
        for(pit = it->indices.begin(); pit != it->indices.end(); pit++) {
        //push_back: add a point to the end of the existing vector

        //Merge current clusters to whole point cloud
    *clustered_cloud += *cloud_cluster;


pcl::toROSMsg (*clustered_cloud , *clusters);
publish_clusters.publish (*clusters);

and here is an exemple of drawing bounding box with clusters in rviz :

visualization_msgs::Marker mark_cluster(pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_cluster, std::string ns ,int id, float r, float g, float b)
  Eigen::Vector4f centroid;
  Eigen::Vector4f min;
  Eigen::Vector4f max;

  pcl::compute3DCentroid (*cloud_cluster, centroid);
  pcl::getMinMax3D (*cloud_cluster, min, max);

  uint32_t shape = visualization_msgs::Marker::CUBE;
  visualization_msgs::Marker marker;
  marker.header.frame_id = cloud_cluster->header.frame_id;
  marker.header.stamp = ros::Time::now();

  marker.ns = ns; = id;
  marker.type = shape;
  marker.action = visualization_msgs::Marker::ADD;

  marker.pose.position.x = centroid[0];
  marker.pose.position.y = centroid[1];
  marker.pose.position.z = centroid[2];
  marker.pose.orientation.x = 0.0;
  marker.pose.orientation.y = 0.0;
  marker.pose.orientation.z = 0.0;
  marker.pose.orientation.w = 1.0;

  marker.scale.x ...
edit retag flag offensive close merge delete


Hi, I am also trying to extract clusters and obtain a bounding box around these clusters from the radar data. For now , i have successfully done this by creating a perception pipeline that involves voxel grid down sampling for reducing the point cloud size and used pass through filter to eliminate unwanted ground points, Euclidean clustering to cluster the targets together. I have used moment of inertia class from pcl to obtain a bounding box around each cluster.

vivekgouravs gravatar image vivekgouravs  ( 2022-07-05 06:17:56 -0500 )edit