void cloud_cb (const sensor_msgs::PointCloud2ConstPtr& input)
{
// ... do data processing
sensor_msgs::PointCloud2 output;
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr clustered_cloud (new pcl::PointCloud<pcl::PointXYZ>);
pcl::fromROSMsg (*input, *cloud);
// Creating the KdTree object for the search method of the extraction
pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ>);
tree->setInputCloud (cloud);
std::vector<pcl::PointIndices> cluster_indices;
pcl::EuclideanClusterExtraction<pcl::PointXYZ> ec;
ec.setClusterTolerance (0.10); // 2cm
ec.setMinClusterSize (20);
ec.setMaxClusterSize (2500);
ec.setSearchMethod (tree);
ec.setInputCloud(cloud);
ec.extract (cluster_indices);
ROS_INFO("cluster_indices has %d data points.", (int) cluster_indices.size());
ROS_INFO("cloud has %d data points.", (int) cloud->points.size());
visualization_msgs::Marker marker;
marker.header = cloud->header;
marker.id = 1;
marker.type = visualization_msgs::Marker::CUBE_LIST;
marker.action = visualization_msgs::Marker::ADD;
marker.color.r = 1;
marker.color.g = 0;
marker.color.b = 0;
marker.color.a = 0.7;
marker.scale.x = 0.2;
marker.scale.y = 0.2;
marker.scale.z = 0.2;
marker.lifetime = ros::Duration(60.0);
Eigen::Vector4f minPoint;
Eigen::Vector4f maxPoint;
// pcl::getMinMax3D(*cloud, minPoint, maxPoint);
for (std::vector<pcl::PointIndices>::const_iterator it = cluster_indices.begin (); it != cluster_indices.end (); ++it)
{
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_cluster (new pcl::PointCloud<pcl::PointXYZ>);
for (std::vector<int>::const_iterator pit = it->indices.begin (); pit != it->indices.end (); pit++)
cloud_cluster->points.push_back (cloud->points[*pit]);
std::cout << "PointCloud representing the Cluster: " << cloud_cluster->points.size () << " data points." << std::endl;
// Merge current clusters to whole point cloud
*clustered_cloud += *cloud_cluster;
// for(size_t j = 0; j < cloud_cluster->points.size() - 1; j++)
// {
/*
geometry_msgs::Point pt1;
pt1.x = cloud_cluster->points[j].x;
pt1.y = cloud_cluster->points[j].y;
pt1.z = cloud_cluster->points[j].z;
geometry_msgs::Point pt2;
pt2.x = cloud_cluster->points[j+1].x;
pt2.y = cloud_cluster->points[j+1].y;
pt2.z = cloud_cluster->points[j+1].z;
marker.points.push_back(pt1);
marker.points.push_back(pt2);
*/
//Seg for top of prism
geometry_msgs::Point pt3;
pt3.x = 0.0;
pt3.y = 0.0;
pt3.z = 0.0;
std_msgs::ColorRGBA colors;
colors.r = 0.0;
colors.g = 0.0;
colors.b = 0.0;
for(size_t i=0; i<cloud_cluster->points.size(); i++)
{
pt3.x += cloud_cluster->points[i].x;
pt3.y += cloud_cluster->points[i].y;
pt3.z += cloud_cluster->points[i].z;
}
pt3.x /= cloud_cluster->points.size();
pt3.y /= cloud_cluster->points.size();
pt3.z /= cloud_cluster->points.size();
pcl::getMinMax3D(*cloud_cluster, minPoint, maxPoint);
marker.scale.y= maxPoint.y();
//marker.scale.x= maxPoint.x();
//marker.scale.z= maxPoint.z();
marker.scale.x= maxPoint.x()-minPoint.x();
colors.r = marker.scale.x;
// colors.g = marker.scale.y;
//marker.scale.z= maxPoint.z()-minPoint.z();
//pt3.z = maxPoint.z();
//geometry_msgs::Point pt4;
//pt4.x = cloud_cluster->points[j+1].x;
//pt4.y = cloud_cluster->points[j+1].y;
//pt4.z = cloud_cluster->points[j+1].z;
//pt4.z = maxPoint.z();
//marker.pose.position.x = pt3.x;
//marker.pose.position.y = pt3.y;
//marker.pose.position.z = pt3.z;
//marker.colors.push_back(colors);
marker.points.push_back(pt3);
//marker.points.push_back(pt4);
//Seg for bottom vertices to top vertices
// marker.points.push_back(pt1);
//marker.points.push_back(pt3);
// }
object_pub.publish(marker);
}
// Publish the data
pcl::toROSMsg(*clustered_cloud, output);
output.header = cloud->header;
// output.header.frame_id="/camera_link";
// output.header.stamp = ros::Time::now();
pub.publish (output);
}
void cloud_cb(const sensor_msgs::PointCloud2ConstPtr& input){
sensor_msgs::PointCloud2::Ptr clusters (new sensor_msgs::PointCloud2);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>), cloud_f (new pcl::PointCloud<pcl::PointXYZ>);
pcl::fromROSMsg(*input, *cloud);
pcl::PointCloud<pcl::PointXYZ>::Ptr clustered_cloud (new pcl::PointCloud<pcl::PointXYZ>);
std::cout << "PointCloud before filtering has: " << cloud->points.size () << " data points." << std::endl;
// Create the filtering object: downsample the dataset using a leaf size of 1cm
pcl::VoxelGrid<pcl::PointXYZ> vg;
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered (new pcl::PointCloud<pcl::PointXYZ>);
vg.setInputCloud (cloud);
vg.setLeafSize (0.01f, 0.01f, 0.01f);
vg.filter (*cloud_filtered);
std::cout << "PointCloud after filtering has: " << cloud_filtered->points.size () << " data points." << std::endl;
// Create the segmentation object for the planar model and set all the parameters
pcl::SACSegmentation<pcl::PointXYZ> seg;
pcl::PointIndices::Ptr inliers (new pcl::PointIndices);
pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_plane (new pcl::PointCloud<pcl::PointXYZ> ());
pcl::PCDWriter writer;
seg.setOptimizeCoefficients (true);
seg.setModelType (pcl::SACMODEL_PLANE);
seg.setMethodType (pcl::SAC_RANSAC);
seg.setMaxIterations (100);
seg.setDistanceThreshold (0.02);
int i=0, nr_points = (int) cloud_filtered->points.size ();
while (cloud_filtered->points.size () > 0.3 * nr_points)
{
// Segment the largest planar component from the remaining cloud
seg.setInputCloud (cloud_filtered);
seg.segment (*inliers, *coefficients);
if (inliers->indices.size () == 0)
{
std::cout << "Could not estimate a planar model for the given dataset." << std::endl;
break;
}
// Extract the planar inliers from the input cloud
pcl::ExtractIndices<pcl::PointXYZ> extract;
extract.setInputCloud (cloud_filtered);
extract.setIndices (inliers);
extract.setNegative (false);
// Get the points associated with the planar surface
extract.filter (*cloud_plane);
std::cout << "PointCloud representing the planar component: " << cloud_plane->points.size () << " data points." << std::endl;
// Remove the planar inliers, extract the rest
extract.setNegative (true);
extract.filter (*cloud_f);
*cloud_filtered = *cloud_f;
}
// Creating the KdTree object for the search method of the extraction
pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ>);
tree->setInputCloud (cloud_filtered);
std::vector<pcl::PointIndices> cluster_indices;
pcl::EuclideanClusterExtraction<pcl::PointXYZ> ec;
ec.setClusterTolerance (0.02); // 2cm
ec.setMinClusterSize (10);
ec.setMaxClusterSize (2500);
ec.setSearchMethod (tree);
ec.setInputCloud (cloud_filtered);
ec.extract (cluster_indices);
std::vector<pcl::PointIndices>::const_iterator it;
std::vector<int>::const_iterator pit;
int j = 0;
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
cloud_cluster->points.push_back(cloud_filtered->points[*pit]);
/*cloud_cluster->width = cloud_cluster->points.size ();
cloud_cluster->height = 1;
cloud_cluster->is_dense = true;
std::stringstream ss;
ss << "cloud_cluster_" << j << ".pcd";
writer.write<pcl::PointXYZ> (ss.str (), *cloud_cluster, false); //*
j++;*/
}
//Merge current clusters to whole point cloud
*clustered_cloud += *cloud_cluster;
}
pcl::toROSMsg (*clustered_cloud , *clusters);
clusters->header.frame_id = "/camera_depth_frame";
clusters->header.stamp=ros::Time::now();
pub.publish (*clusters);
//sensor_msgs::PointCloud2::Ptr output (new sensor_msgs::PointCloud2);
//pcl::toROSMsg(*cloud_filtered, *output);
//pub_plane.publish(*output);
}
