void Segmentation::euclidianClustering( pcl::PointCloud<pcl::PointXYZRGBA>::Ptr &cloud, std::vector < pcl::PointCloud<pcl::PointXYZRGBA>::Ptr > &clusters)
{
pcl::search::KdTree<pcl::PointXYZRGBA>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZRGBA>);
tree->setInputCloud (cloud);
std::vector<pcl::PointIndices> cluster_indices;
pcl::EuclideanClusterExtraction<pcl::PointXYZRGBA> ec;
ec.setClusterTolerance (0.02); // 2cm
ec.setMinClusterSize (300); //1000
ec.setMaxClusterSize (100000000);
ec.setSearchMethod (tree);
ec.setInputCloud (cloud);
ec.extract (cluster_indices);
for (std::vector<pcl::PointIndices>::const_iterator it = cluster_indices.begin (); it != cluster_indices.end (); ++it)
{
pcl::PointCloud<pcl::PointXYZRGBA>::Ptr cloud_cluster (new pcl::PointCloud<pcl::PointXYZRGBA>);
for (std::vector<int>::const_iterator pit = it->indices.begin (); pit != it->indices.end (); ++pit)
{
cloud_cluster->points.push_back (cloud->points[*pit]);
}
cloud_cluster->width = cloud_cluster->points.size ();
cloud_cluster->height = 1;
cloud_cluster->is_dense = true;
clusters.push_back(cloud_cluster);
}
}
int clusterExtraction(pcl::PointCloud<pcl::PointXYZRGB>::Ptr input, std::vector<pcl::PointCloud<pcl::PointXYZRGB>::Ptr> *clouds, std::vector<pcl::PointXYZRGB> *labels)
{
pcl::KdTree<pcl::PointXYZRGB>::Ptr tree(new pcl::KdTreeFLANN<pcl::PointXYZRGB>);
tree->setInputCloud(input);
std::vector<pcl::PointIndices> cluster_indices;
pcl::EuclideanClusterExtraction<pcl::PointXYZRGB> extraction;
extraction.setClusterTolerance(0.010); // 1cm -- decreasing makes more clusters
extraction.setMinClusterSize(120);
extraction.setMaxClusterSize(4000);
extraction.setSearchMethod(tree);
extraction.setInputCloud(input);
extraction.extract(cluster_indices);
for (std::vector<pcl::PointIndices>::const_iterator it = cluster_indices.begin(); it != cluster_indices.end(); it++)
{
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_cluster(new pcl::PointCloud<pcl::PointXYZRGB>);
for (std::vector<int>::const_iterator pit = it->indices.begin (); pit != it->indices.end (); pit++)
cloud_cluster->points.push_back (input->points[*pit]);
cloud_cluster->width = cloud_cluster->points.size ();
cloud_cluster->height = 1;
cloud_cluster->is_dense = true;
std::cout << "PointCloud has " << cloud_cluster->points.size () << " data points." << std::endl;
clouds->push_back(cloud_cluster);
labels->push_back(cloud_cluster->points[0]);
}
return cluster_indices.size();
}
std::vector<PointCloudT::Ptr > computeClusters(PointCloudT::Ptr in, double tolerance){
std::vector<PointCloudT::Ptr > clusters;
pcl::search::KdTree<PointT>::Ptr tree (new pcl::search::KdTree<PointT>);
tree->setInputCloud (in);
std::vector<pcl::PointIndices> cluster_indices;
pcl::EuclideanClusterExtraction<PointT> ec;
ec.setClusterTolerance (tolerance); // 2cm
ec.setMinClusterSize (50);
ec.setMaxClusterSize (25000);
ec.setSearchMethod (tree);
ec.setInputCloud (in);
ec.extract (cluster_indices);
int j = 0;
for (std::vector<pcl::PointIndices>::const_iterator it = cluster_indices.begin (); it != cluster_indices.end (); ++it)
{
PointCloudT::Ptr cloud_cluster (new PointCloudT);
for (std::vector<int>::const_iterator pit = it->indices.begin (); pit != it->indices.end (); pit++)
cloud_cluster->points.push_back (in->points[*pit]); //*
cloud_cluster->width = cloud_cluster->points.size ();
cloud_cluster->height = 1;
cloud_cluster->is_dense = true;
clusters.push_back(cloud_cluster);
}
return clusters;
}
bool TOMPCFilter<T>::segmentation(CloudPtr &cloud_in, VecCloud &clouds_out, double tolerance, double minSize, double maxSize)
{
// std::cout << "Object segmentation" << std::endl;
// object clustering
// Creating the KdTree object for the search method of the extraction
typename pcl::search::KdTree<T>::Ptr tree (new pcl::search::KdTree<T>);
tree->setInputCloud (cloud_in);
std::vector<pcl::PointIndices> cluster_indices;
pcl::EuclideanClusterExtraction<T> ec;
ec.setClusterTolerance (tolerance); // 2cm
ec.setMinClusterSize (minSize);
ec.setMaxClusterSize (maxSize);
ec.setSearchMethod (tree);
ec.setInputCloud( cloud_in);
ec.extract (cluster_indices);
int j = 0;
clouds_out.clear();
for (std::vector<pcl::PointIndices>::const_iterator it = cluster_indices.begin (); it != cluster_indices.end (); ++it)
{
CloudPtr cloud_cluster (new pcl::PointCloud<T>);
for (std::vector<int>::const_iterator pit = it->indices.begin (); pit != it->indices.end (); pit++)
cloud_cluster->points.push_back (cloud_in->points[*pit]); //*
clouds_out.push_back(cloud_cluster);
}
return 1;
}
pcl::PointCloud<PointT>::Ptr extract_object_from_indices(pcl::PointCloud<PointT>::Ptr cloud_input,pcl::PointIndices object_indices){
pcl::PointCloud<PointT>::Ptr cloud_cluster(new pcl::PointCloud<PointT>);
for (int j=0; j<object_indices.indices.size(); j++){
cloud_cluster->points.push_back (cloud_input->points[object_indices.indices[j]]);
}
return cloud_cluster;
}
void redblade_laser::cluster(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud,
pcl::PointCloud<pcl::PointXYZ>::Ptr cluster,
double tolerance){
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(tolerance);
ec.setMinClusterSize(1);
ec.setMaxClusterSize(1000);
ec.setSearchMethod(tree);
ec.setInputCloud(cloud);
ec.extract(cluster_indices);
int curSize = 0;
for(std::vector<pcl::PointIndices>::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]);
}
cloud_cluster->width = cluster->points.size();
cloud_cluster->height = 1;
if(cloud_cluster->points.size()>curSize){
cluster->points.resize(cloud_cluster->points.size());
std::copy(cloud_cluster->points.begin(),
cloud_cluster->points.end(),
cluster->points.begin());
cloud_cluster->width = cluster->points.size();
cloud_cluster->height = 1;
curSize = cloud_cluster->points.size();
}
}
}
/* ========================================
* Fill Code: EUCLIDEAN CLUSTER EXTRACTION
* ========================================*/
std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr>
clusterExtraction(const pcl::PointCloud<pcl::PointXYZ>::ConstPtr &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 (input_cloud);
std::vector<pcl::PointIndices> cluster_indices;
pcl::EuclideanClusterExtraction<pcl::PointXYZ> ec;
ec.setClusterTolerance (0.02); // 2cm
ec.setMinClusterSize (25);
ec.setMaxClusterSize (10000);
ec.setSearchMethod (tree);
ec.setInputCloud (input_cloud);
ec.extract (cluster_indices);
std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> clusters;
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(input_cloud->points[*pit]);
cloud_cluster->width = cloud_cluster->points.size ();
cloud_cluster->height = 1;
cloud_cluster->is_dense = true;
std::cout << "Cluster has " << cloud_cluster->points.size() << " points.\n";
clusters.push_back(cloud_cluster);
}
return clusters;
}
void ObjectDetector::clusterCloud(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud)
{
// Create the cluster msg
pcl_msgs::Clusters clusters_msg;
int j = 0;
if((cloud->width * cloud->height) != 0){
// 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);
// Use Euclidean clustering
std::vector<pcl::PointIndices> cluster_indices;
pcl::EuclideanClusterExtraction<pcl::PointXYZ> ec;
ec.setClusterTolerance (cluster_tolerance);
ec.setMinClusterSize (cluster_min);
ec.setMaxClusterSize (cluster_max);
ec.setSearchMethod (tree);
ec.setInputCloud (cloud);
ec.extract (cluster_indices);
// Find all clusters
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]);
cloud_cluster->width = cloud_cluster->points.size ();
cloud_cluster->height = 1;
cloud_cluster->is_dense = true;
// Compute cluster size
int cluster_size = (int) cloud_cluster->points.size();
// Compute cluster centroid
Eigen::Vector4f centroid;
pcl::compute3DCentroid(*cloud_cluster,centroid);
// Add cluster to msg
pcl_msgs::Cluster cluster;
cluster.x = centroid[0];
cluster.y = centroid[1];
cluster.z = -centroid[2];
cluster.isDebris = false;
clusters_msg.cluster.push_back(cluster);
j++;
}
}
// Add number of clusters to msg
clusters_msg.cluster_len = j;
ROS_INFO("Clusters: %d", j);
// Publish the data
pub.publish(clusters_msg);
}
int
main (int, char **argv)
{
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_ptr (new pcl::PointCloud<pcl::PointXYZ> ());
pcl::PointCloud<pcl::Normal>::Ptr cloud_normals (new pcl::PointCloud<pcl::Normal> ());
pcl::PCDWriter writer;
if (pcl::io::loadPCDFile<pcl::PointXYZ> (argv[1], *cloud_ptr) == -1)
{
std::cout<<"Couldn't read the file "<<argv[1]<<std::endl;
return (-1);
}
std::cout << "Loaded pcd file " << argv[1] << " with " << cloud_ptr->points.size () << std::endl;
// Normal estimation
pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> ne;
ne.setInputCloud (cloud_ptr);
pcl::search::KdTree<pcl::PointXYZ>::Ptr tree_n (new pcl::search::KdTree<pcl::PointXYZ>());
ne.setSearchMethod (tree_n);
ne.setRadiusSearch (0.03);
ne.compute (*cloud_normals);
std::cout << "Estimated the normals" << std::endl;
// Creating the kdtree object for the search method of the extraction
boost::shared_ptr<pcl::KdTree<pcl::PointXYZ> > tree_ec (new pcl::KdTreeFLANN<pcl::PointXYZ> ());
tree_ec->setInputCloud (cloud_ptr);
// Extracting Euclidean clusters using cloud and its normals
std::vector<int> indices;
std::vector<pcl::PointIndices> cluster_indices;
const float tolerance = 0.5f; // 50cm tolerance in (x, y, z) coordinate system
const double eps_angle = 5 * (M_PI / 180.0); // 5degree tolerance in normals
const unsigned int min_cluster_size = 50;
pcl::extractEuclideanClusters (*cloud_ptr, *cloud_normals, tolerance, tree_ec, cluster_indices, eps_angle, min_cluster_size);
std::cout << "No of clusters formed are " << cluster_indices.size () << std::endl;
// Saving the clusters in seperate pcd files
int j = 0;
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_ptr->points[*pit]);
cloud_cluster->width = static_cast<uint32_t> (cloud_cluster->points.size ());
cloud_cluster->height = 1;
cloud_cluster->is_dense = true;
std::cout << "PointCloud representing the Cluster using xyzn: " << cloud_cluster->points.size () << " data points." << std::endl;
std::stringstream ss;
ss << "./cloud_cluster_" << j << ".pcd";
writer.write<pcl::PointXYZ> (ss.str (), *cloud_cluster, false);
j++;
}
return (0);
}
void Planar_filter::Segment_cloud()
{
pcl::PointIndices::Ptr inliers (new pcl::PointIndices);
pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients);
PointCloudT::Ptr cloud_plane (new PointCloudT ());
seg.setOptimizeCoefficients (true);
seg.setModelType (pcl::SACMODEL_PLANE);
seg.setMethodType (pcl::SAC_RANSAC);
seg.setMaxIterations (100);
seg.setDistanceThreshold (0.02);
std::vector<pcl::PointIndices> cluster_indices;
seg.setInputCloud (_cloud);
seg.segment (*inliers, *coefficients);
if (inliers->indices.size () == 0)
{
std::cout << "Could not estimate a planar model for the given dataset." << std::endl;
}
// Extract the planar inliers from the input cloud
pcl::ExtractIndices<PointT> extract;
extract.setInputCloud (_cloud);
extract.setIndices (inliers);
extract.setNegative (false);
// Get the points associated with the planar surface
PointCloudT::Ptr cloud_f(new PointCloudT);
extract.filter (*cloud_plane);
//std::cout << "PointCloud representing the planar component: " << cloud_plane->points.size () << " data points." << std::endl;
extract.setNegative (true);
extract.filter (*cloud_f);
*_cloud = *cloud_f;
// Creating the KdTree object for the search method of the extraction
pcl::search::KdTree<PointT>::Ptr tree (new pcl::search::KdTree<PointT>);
tree->setInputCloud (_cloud);
pcl::EuclideanClusterExtraction<PointT> ec;
ec.setClusterTolerance (0.02); // 2cm
ec.setMinClusterSize (100);
ec.setMaxClusterSize (25000);
ec.setSearchMethod (tree);
ec.setInputCloud (_cloud);
ec.extract (cluster_indices);
for (std::vector<pcl::PointIndices>::const_iterator it = cluster_indices.begin (); it != cluster_indices.end (); ++it)
{
PointCloudT::Ptr cloud_cluster (new PointCloudT);
for (std::vector<int>::const_iterator pit = it->indices.begin (); pit != it->indices.end (); ++pit)
cloud_cluster->points.push_back (_cloud->points[*pit]); //*
cloud_cluster->width = cloud_cluster->points.size();
cloud_cluster->height = 1;
cloud_cluster->is_dense = true;
_Segmented_clouds.push_back(cloud_cluster);
}
}
void search_sphere(pcl::PointCloud<pcl::PointXYZ>::Ptr src, pcl::PointCloud<pcl::PointXYZ>::Ptr& dst,
pcl::ModelCoefficients::Ptr& coeffs)
{
// 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 (src);
std::vector<pcl::PointIndices> cluster_indices;
pcl::EuclideanClusterExtraction<pcl::PointXYZ> ec;
ec.setClusterTolerance (cluster_tolerance_);
ec.setMinClusterSize (50);
ec.setMaxClusterSize (25000);
ec.setSearchMethod (tree);
ec.setInputCloud (src);
ec.extract (cluster_indices);
for (size_t i=0; i<cluster_indices.size(); i++)
{
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_cluster (new pcl::PointCloud<pcl::PointXYZ>);
pcl::copyPointCloud<pcl::PointXYZ>(*src, cluster_indices[i], *cloud_cluster);
pcl::PointCloud<pcl::Normal>::Ptr cloud_cluster_normals (new pcl::PointCloud<pcl::Normal>);
estimate_normals(cloud_cluster, cloud_cluster_normals);
//Look for a sphere
pcl::SACSegmentationFromNormals<pcl::PointXYZ,pcl::Normal> seg;
pcl::ModelCoefficients::Ptr coefficients_sphere (new pcl::ModelCoefficients);
pcl::PointIndices::Ptr inliers_sphere (new pcl::PointIndices);
seg.setOptimizeCoefficients (true);
seg.setModelType (pcl::SACMODEL_SPHERE);
seg.setMethodType (pcl::SAC_RANSAC);
seg.setNormalDistanceWeight (5);
seg.setMaxIterations (100);
seg.setDistanceThreshold (sphere_threshold_);
seg.setRadiusLimits (target_radius_ - 0.05, target_radius_ + 0.05);
seg.setInputCloud (cloud_cluster);
seg.setInputNormals (cloud_cluster_normals);
seg.segment(*inliers_sphere, *coefficients_sphere);
if(inliers_sphere->indices.size() > 0)
{
pcl::ExtractIndices<pcl::PointXYZ> extract;
extract.setInputCloud(cloud_cluster);
extract.setIndices(inliers_sphere);
extract.setNegative(false);
extract.filter(*dst);
*coeffs = *coefficients_sphere;
found_ = true;
}
}
}
void PCLClusteringFunction::parallelClustering(pcl::PointIndices &aIndices)
{
cloudPtrType cloud_cluster (new cloudType);
for (std::vector<int>::const_iterator pit = aIndices.indices.begin (); pit != aIndices.indices.end (); pit++)
cloud_cluster->points.push_back (workingCloud->points[*pit]); //*
cloud_cluster->width = cloud_cluster->points.size ();
cloud_cluster->height = 1;
cloud_cluster->is_dense = true;
clusteringParallelMuting.lock();
cloud_clustered.push_back(cloud_cluster);
clusteringParallelMuting.unlock();
}
/**
*Segments the input point cloud into several clusters
* @param input_cloud_ptr
* @param clusters
*/
void segment_clusters(const pcl::PointCloud<PointT>::Ptr input_cloud_ptr,
std::vector<pcl::PointCloud<PointT> > & clusters) {
pcl::search::KdTree<PointT>::Ptr cluster_tree (new pcl::search::KdTree<PointT> ());
std::vector<pcl::PointIndices> cluster_indices;
sensor_msgs::PointCloud2 cluster_msg;
pcl::EuclideanClusterExtraction<pcl::PointXYZ> ec;
ec.setClusterTolerance(0.08);
ec.setMinClusterSize(50);
ec.setMaxClusterSize(10000);
ec.setSearchMethod(cluster_tree);
ec.setInputCloud(input_cloud_ptr);
ec.extract(cluster_indices);
ROS_INFO("Found %d Clusters\n",(int)cluster_indices.size());
for (std::vector<pcl::PointIndices>::const_iterator it =cluster_indices.begin(); it != cluster_indices.end(); ++it) {
pcl::PointCloud<PointT>::Ptr cloud_cluster( new pcl::PointCloud<PointT>);
for (std::vector<int>::const_iterator pit = it->indices.begin();pit != it->indices.end(); pit++)
cloud_cluster->points.push_back(input_cloud_ptr->points[*pit]); //*
cloud_cluster->width = cloud_cluster->points.size();
cloud_cluster->height = 1;
cloud_cluster->is_dense = true;
ROS_INFO("Cluster Size: %d \n",(int)cloud_cluster->points.size());
//adds a cluster to the output list
clusters.push_back(*cloud_cluster);
pcl::toROSMsg(*cloud_cluster, cluster_msg);
cluster_msg.header.frame_id = frame_id_;
cluster_msg.header.stamp=ros::Time::now();
pub_cluster_.publish(cluster_msg);
/*slow down ouput
ros::Rate r(2);
r.sleep();
*/
}
}
pcl::PointCloud<pcl::PointXYZ>::Ptr cluster_clouds(pcl::PointCloud<pcl::PointXYZ>::Ptr input, double dist)
{
pcl::PointCloud<pcl::PointXYZ>::Ptr center_points (new pcl::PointCloud<pcl::PointXYZ>);
// 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 (input);
std::vector<pcl::PointIndices> cluster_indices;
pcl::EuclideanClusterExtraction<pcl::PointXYZ> ec;
ec.setClusterTolerance (dist); // in cm
ec.setMinClusterSize (10);
ec.setMaxClusterSize (25000);
ec.setSearchMethod (tree);
ec.setInputCloud (input);
ec.extract (cluster_indices);
int j = 0;
//now move trough all clusters of the point cloud...
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 (input->points[*pit]);
cloud_cluster->width = cloud_cluster->points.size ();
cloud_cluster->height = 1;
cloud_cluster->is_dense = true;
std::cout << "PointCloud representing the Cluster: " << cloud_cluster->points.size () << " data points." << std::endl;
//compute centroid of point cloud
pcl::PointXYZ center;
Eigen::Vector4f centroid;
pcl::compute3DCentroid(*cloud_cluster,centroid);
center.x=centroid[0];
center.y=centroid[1];
center.z=centroid[2];
center_points->push_back(center);
j++;
}
center_points->width=center_points->points.size();
//return input;
return center_points;
}
void compute_cluster_from_cloud(const Cloud::ConstPtr& cloud_msg) {
maggieDebug2("compute_cluster_from_cloud()");
// Creating the KdTree object for the search method of the extraction
Timer timer;
_tree_ptr->setInputCloud (cloud_msg);
_cluster_indices.clear();
pcl::EuclideanClusterExtraction<pcl::PointXYZ> ec;
ec.setClusterTolerance (CLUSTER_TOLERANCE);
ec.setMinClusterSize (MIN_CLUSTER_SIZE);
ec.setMaxClusterSize (25000);
ec.setSearchMethod (_tree_ptr);
ec.setInputCloud( cloud_msg);
ec.extract (_cluster_indices);
_n_clusters = (int) _cluster_indices.size();
maggieDebug2("euclidin clustering:%g ms, \tnumber of clusters:%i",
timer.getTimeMilliseconds(),
_n_clusters);
// Write the planar inliers to disk
#if 0
timer.reset();
pcl::PCDWriter writer;
int j = 0;
for (std::vector<pcl::PointIndices>::const_iterator it = _cluster_indices.begin ();
it != _cluster_indices.end ();
++it) {
Cloud::Ptr cloud_cluster(new Cloud);
for (std::vector<int>::const_iterator pit = it->indices.begin (); pit
!= it->indices.end (); pit++) {
cloud_cluster->points.push_back (cloud_msg->points[*pit]);
} // end loop cluster point
cloud_cluster->width = cloud_cluster->points.size ();
cloud_cluster->height = 1;
cloud_cluster->is_dense = true;
std::cout << "cloud representing the Cluster: " <<
cloud_cluster->points.size () << " data points." << std::endl;
std::stringstream ss;
ss << "cloud_cluster_" << j << ".pcd";
writer.write<pcl::PointXYZ> (ss.str (), *cloud_cluster, false); //*
j++;
} // end loop cluster index
#endif
compute_bounding_boxes(cloud_msg);
} // end compute_cluster_from_cloud();
pcl::PointCloud<pcl::PointXYZRGB>::Ptr extract_clusters (pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud, pcl::PointXYZRGB point)
{
cloud->push_back(point); // append clicked_point to cloud
int point_index = cloud->points.size() - 1; // get index of clicked_point
// Creating the KdTree object for the search method of the extraction
pcl::search::KdTree<pcl::PointXYZRGB>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZRGB>);
tree->setInputCloud (cloud);
std::vector<pcl::PointIndices> cluster_indices;
pcl::EuclideanClusterExtraction<pcl::PointXYZRGB> ec;
ec.setClusterTolerance (0.02); // 2cm
ec.setMinClusterSize (100);
ec.setMaxClusterSize (640*480);
ec.setSearchMethod (tree);
ec.setInputCloud (cloud);
ec.extract (cluster_indices);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_cluster (new pcl::PointCloud<pcl::PointXYZRGB>);
int j = 0;
for (std::vector<pcl::PointIndices>::const_iterator it = cluster_indices.begin (); it != cluster_indices.end (); ++it)
{
if(std::find(it->indices.begin(), it->indices.end(), point_index) != it->indices.end())
{
// Pointer-ify the indices for the current cluster
pcl::PointIndicesPtr indices_ptr (new pcl::PointIndices);
indices_ptr->indices = it->indices;
// Extract the cluster points
pcl::ExtractIndices<pcl::PointXYZRGB> extract;
extract.setInputCloud (cloud);
extract.setIndices (indices_ptr);
extract.setNegative (false);
extract.filter (*cloud_cluster);
std::cout << "PointCloud representing cluster #" << j << ": " << cloud_cluster->points.size () << " data points." << std::endl;
j++;
}
}
return cloud_cluster;
}
pcl::PointCloud<pcl::PointXYZ>::Ptr PlaneSegmentationPclRgbAlgorithm::getBiggestClusterPC (pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_orig)
{
pcl::EuclideanClusterExtraction<pcl::PointXYZ> pcl_cluster;
// Min distance between two clusters
pcl_cluster.setClusterTolerance (plane_min_cluster_distance);
// Min number of points for a cluster
pcl_cluster.setMinClusterSize (plane_min_cluster_size);
ROS_DEBUG("Clustering");
ROS_DEBUG_STREAM("Original pointcloud size is" << cloud_orig->points.size());
std::vector<pcl::PointIndices> clusters;
pcl_cluster.setInputCloud (cloud_orig);
pcl_cluster.extract (clusters);
//converts clusters into the PointCloud message
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_cluster (new pcl::PointCloud<pcl::PointXYZ>);
ROS_DEBUG_STREAM("Number of clusters is " << clusters.size());
if ((int)clusters.size () > 0 ) {
// get biggest cluster
size_t biggest_cluster = 0;
for (size_t j = 1; j < clusters.size (); ++j) {
if (clusters[biggest_cluster].indices.size () < clusters[j].indices.size ())
biggest_cluster = j;
}
ROS_DEBUG_STREAM("Biggest cluster is " << clusters[biggest_cluster].indices.size());
cloud_cluster->points.resize (clusters[biggest_cluster].indices.size ());
for (size_t j = 0; j < cloud_cluster->points.size (); ++j)
{
cloud_cluster->points[j].x = cloud_orig->points[clusters[biggest_cluster].indices[j]].x;
cloud_cluster->points[j].y = cloud_orig->points[clusters[biggest_cluster].indices[j]].y;
cloud_cluster->points[j].z = cloud_orig->points[clusters[biggest_cluster].indices[j]].z;
}
}
else
*cloud_cluster = *cloud_orig;
return cloud_cluster;
}
std::vector<pcl::PointCloud<PointT>::Ptr> segment_objects(pcl::PointCloud<PointT>::Ptr cloud_input, double tolerance, int minClusterSize, int maxClusterSize){
std::vector<pcl::PointIndices> cluster_indices;
pcl::EuclideanClusterExtraction<PointT> ec;
ec.setClusterTolerance (tolerance); // 2cm
ec.setMinClusterSize (minClusterSize);
ec.setMaxClusterSize (maxClusterSize);
pcl::search::KdTree<PointT>::Ptr tree (new pcl::search::KdTree<PointT>);
tree->setInputCloud (cloud_input);
ec.setSearchMethod (tree);
ec.setInputCloud (cloud_input);
ec.extract (cluster_indices);
// returns a vector of all the objects
std::vector<pcl::PointCloud<PointT>::Ptr> object_vector_temp;
for (int i=0; i<cluster_indices.size(); i++){
pcl::PointIndices cloud_indices = cluster_indices.at(i);
pcl::PointCloud<PointT>::Ptr cloud_cluster(new pcl::PointCloud<PointT>);
cloud_cluster = extract_object_from_indices(cloud_input,cloud_indices);
object_vector_temp.push_back(cloud_cluster);
}
return object_vector_temp;
}
std::vector<Box2DPoint> Cluster(const pcl::PointCloud<PointT>::Ptr input_cloud)
{
pcl::PointCloud<PointT>::Ptr cloud_filtered(new pcl::PointCloud<PointT>);
cloud_filtered = RemovePlane (input_cloud);
//Creating the KdTree object for the search method of the extraction
pcl::search::KdTree<PointT>::Ptr tree (new pcl::search::KdTree<PointT>);
tree->setInputCloud (cloud_filtered);
std::vector<pcl::PointIndices> cluster_indices;
pcl::EuclideanClusterExtraction<PointT> ec;
ec.setClusterTolerance (0.02); // 2cm
ec.setMinClusterSize (100);
ec.setMaxClusterSize (1000);//
ec.setSearchMethod (tree);
ec.setInputCloud (cloud_filtered);
ec.extract (cluster_indices);
// pcl::visualization::PCLVisualizer viewer ("cluster");
std::vector<Box2DPoint> my_points;
int j = 0;
for (std::vector<pcl::PointIndices>::const_iterator it = cluster_indices.begin (); it != cluster_indices.end (); ++it)
{
pcl::PointCloud<PointT>::Ptr cloud_cluster (new pcl::PointCloud<PointT>);
for (std::vector<int>::const_iterator pit = it->indices.begin (); pit != it->indices.end (); pit++)
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;
// viewer.addPointCloud (cloud_cluster, "cloud");
float x_min=999,y_min=999,z_min=999;
float x_max=0,y_max=0,z_max=0;
for(int i=0;i<cloud_cluster->size();i++)
{
if(cloud_cluster->points[i].x<x_min)
x_min=cloud_cluster->points[i].x;
if(cloud_cluster->points[i].y<y_min)
y_min=cloud_cluster->points[i].y;
if(cloud_cluster->points[i].z<z_min)
z_min=cloud_cluster->points[i].z;
if(cloud_cluster->points[i].x>x_max)
x_max=cloud_cluster->points[i].x;
if(cloud_cluster->points[i].y>y_max)
y_max=cloud_cluster->points[i].y;
if(cloud_cluster->points[i].z>z_max)
z_max=cloud_cluster->points[i].z;
}
// viewer.addCube(x_min, x_max, y_min, y_max, z_min, z_max, 0, 255, 0, "cloud");
Box2DPoint pp;
pp.x_left_down = x_min*FOCAL/z_min+320;
pp.y_left_down = y_min*FOCAL/z_min+240;
pp.x_right_up = x_max*FOCAL/z_max+320;
pp.y_right_up = y_max*FOCAL/z_max+240;
my_points.push_back(pp);
j++;
}
return my_points;
}
void cloud_cb (const sensor_msgs::PointCloud2ConstPtr& cloud_msg){
// Updating parameters from the parameter server
ros::param::getCached("/PCL_object_clustering/cluster_tolerans",cluster_tolerans);
ros::param::getCached("/PCL_object_clustering/cluster_size/min",cluster_size_min);
ros::param::getCached("/PCL_object_clustering/cluster_size/max",cluster_size_max);
ros::param::getCached("/PCL_object_clustering/clusters_highest_point",clusters_highest_point);
// Converting from PointCloud2 msg to pcl::PointCloud
pcl::PCLPointCloud2 pcl_pc2;
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_without_planes (new pcl::PointCloud<pcl::PointXYZ>);
pcl_conversions::toPCL(*cloud_msg,pcl_pc2);
pcl::fromPCLPointCloud2(pcl_pc2,*cloud_without_planes);
if(!(*cloud_without_planes).empty()){
pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ>);
tree->setInputCloud (cloud_without_planes);
// Getting indeces for each found cluster with parameters cluster_tolerans,
// cluster_size_min and cluster_size_max
std::vector<pcl::PointIndices> cluster_indices;
pcl::EuclideanClusterExtraction<pcl::PointXYZ> ec;
ec.setClusterTolerance (cluster_tolerans);
ec.setMinClusterSize (cluster_size_min);
ec.setMaxClusterSize (cluster_size_max);
ec.setSearchMethod (tree);
ec.setInputCloud (cloud_without_planes);
ec.extract (cluster_indices);
/* For each cluster we store cluster in tmp_cloud, calculate its centroids.
If cluster highest point is less than clusters_highest_point we publish
centroid and append tmp_cloud to cloud_cluster. When we checked through all
possible centroids we publish cloud_cluster to visualize in rviz.*/
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_cluster (new pcl::PointCloud<pcl::PointXYZ>);
for (std::vector<pcl::PointIndices>::const_iterator it = cluster_indices.begin (); it != cluster_indices.end (); ++it){
pcl::PointCloud<pcl::PointXYZ>::Ptr tmp_cloud (new pcl::PointCloud<pcl::PointXYZ>);
for (std::vector<int>::const_iterator pit = it->indices.begin (); pit != it->indices.end (); ++pit){
tmp_cloud->points.push_back (cloud_without_planes->points[*pit]);
}
pcl::PointXYZ min_point, max_point;
pcl::getMinMax3D(*tmp_cloud,min_point,max_point);
if(max_point.z < clusters_highest_point){
Eigen::Vector4f c;
pcl::compute3DCentroid(*tmp_cloud, c);
//std::cout << "Centroid is " << c << std::endl;
object_detecter_2d::object_loc object_loc_msg;
object_loc_msg.ID = 10;
object_loc_msg.point.x = c(0);
object_loc_msg.point.y = c(1);
object_loc_msg.point.z = c(2);
pub_centroid.publish(object_loc_msg);
(*cloud_cluster) = (*cloud_cluster)+(*tmp_cloud);
}
}
cloud_cluster->width = cloud_cluster->points.size ();
cloud_cluster->height = 1;
cloud_cluster->is_dense = true;
(*cloud_cluster).header.frame_id = (*cloud_without_planes).header.frame_id;
sensor_msgs::PointCloud2 output;
pcl::toROSMsg(*cloud_cluster, output);
pub_debug_pcl.publish(output);
}
}
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);
}
int main (int argc, char** argv) {
pcl::PointCloud<pcl::PointXYZRGBA>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZRGBA>);
pcl::PointCloud<pcl::PointXYZRGBA>::Ptr cloud_filtered (new pcl::PointCloud<pcl::PointXYZRGBA>);
if (pcl::io::loadPCDFile<pcl::PointXYZRGBA> ("cloud_inliers.pcd", *cloud) == -1) {
PCL_ERROR ("Failed to read file cloud_inliers.pcd \n");
return (-1);
}
std::cout << "Loaded "
<< cloud->width * cloud->height
<< " data points from cloud_inliers.pcd."
<< std::endl;
pcl::SACSegmentation<pcl::PointXYZRGBA> seg;
pcl::PointIndices::Ptr inliers (new pcl::PointIndices);
pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients);
pcl::PointCloud<pcl::PointXYZRGBA>::Ptr cloud_plane (new pcl::PointCloud<pcl::PointXYZRGBA> ());
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;
int num_points = (int) cloud->points.size();
while (cloud->points.size() > 0.3 * num_points) {
seg.setInputCloud (cloud);
seg.segment (*inliers, *coefficients);
if (inliers->indices.size() == 0) {
std::cout << "Could not estimate a planar model for the given dataset." << std::endl;
break;
}
pcl::ExtractIndices<pcl::PointXYZRGBA> extract;
extract.setInputCloud (cloud);
extract.setIndices (inliers);
extract.setNegative (false);
extract.filter (*cloud_plane);
std::cout << "PointCloud representing the planar component: " << cloud_plane->points.size() << " data points." << std::endl;
extract.setNegative (true);
extract.filter (*cloud_filtered);
*cloud = *cloud_filtered;
}
pcl::search::KdTree<pcl::PointXYZRGBA>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZRGBA>);
tree->setInputCloud (cloud);
std::vector<pcl::PointIndices> cluster_indices;
pcl::EuclideanClusterExtraction<pcl::PointXYZRGBA> ec;
ec.setClusterTolerance (0.02);
ec.setMinClusterSize (100);
ec.setMaxClusterSize (25000);
ec.setSearchMethod (tree);
ec.setInputCloud (cloud);
ec.extract (cluster_indices);
int j = 0;
for (std::vector<pcl::PointIndices>::const_iterator it = cluster_indices.begin (); it != cluster_indices.end (); ++it) {
pcl::PointCloud<pcl::PointXYZRGBA>::Ptr cloud_cluster (new pcl::PointCloud<pcl::PointXYZRGBA>);
for (std::vector<int>::const_iterator pit = it->indices.begin (); pit != it->indices.end (); pit++)
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::cout << "PointCloud representing the Cluster: " << cloud_cluster->points.size () << " data points." << std::endl;
std::stringstream ss;
ss << "cloud_cluster_" << j << ".pcd";
writer.write<pcl::PointXYZRGBA> (ss.str (), *cloud_cluster, false); //*
j++;
}
return (0);
}
// this function gets called every time new pcl data comes in
void cloudcb(const sensor_msgs::PointCloud2ConstPtr &scan)
{
ROS_INFO("Kinect point cloud receieved");
ros::Time start_time = ros::Time::now();
ros::Time tcur = ros::Time::now();
Eigen::Vector4f centroid;
geometry_msgs::Point point;
pcl::PassThrough<pcl::PointXYZ> pass;
Eigen::VectorXf lims(6);
sensor_msgs::PointCloud2::Ptr
ros_cloud (new sensor_msgs::PointCloud2 ()),
ros_cloud_filtered (new sensor_msgs::PointCloud2 ());
pcl::PointCloud<pcl::PointXYZ>::Ptr
cloud (new pcl::PointCloud<pcl::PointXYZ> ()),
cloud_downsampled (new pcl::PointCloud<pcl::PointXYZ> ()),
cloud_filtered (new pcl::PointCloud<pcl::PointXYZ> ());
// set a parameter telling the world that I am tracking the robot
ros::param::set("/tracking_robot", true);
ROS_DEBUG("finished declaring vars : %f", (ros::Time::now()-tcur).toSec());
tcur = ros::Time::now();
ROS_DEBUG("About to transform cloud");
// New sensor message for holding the transformed data
sensor_msgs::PointCloud2::Ptr scan_transformed
(new sensor_msgs::PointCloud2());
try{
pcl_ros::transformPointCloud("/oriented_optimization_frame",
tf, *scan, *scan_transformed);
}
catch(tf::TransformException ex) {
ROS_ERROR("%s", ex.what());
}
scan_transformed->header.frame_id = "/oriented_optimization_frame";
// Convert to pcl
ROS_DEBUG("Convert cloud to pcd from rosmsg");
pcl::fromROSMsg(*scan_transformed, *cloud);
ROS_DEBUG("cloud transformed and converted to pcl : %f",
(ros::Time::now()-tcur).toSec());
tcur = ros::Time::now();
// set time stamp and frame id
ros::Time tstamp = ros::Time::now();
// run through pass-through filter to eliminate tarp and below robots.
ROS_DEBUG("Pass-through filter");
pass_through(cloud, cloud_filtered, robot_limits);
ROS_DEBUG("done with pass-through : %f", (ros::Time::now()-tcur).toSec());
tcur = ros::Time::now();
// now let's publish that filtered cloud
ROS_DEBUG("Converting and publishing cloud");
pcl::toROSMsg(*cloud_filtered, *ros_cloud_filtered);
ros_cloud_filtered->header.frame_id =
"/oriented_optimization_frame";
cloud_pub[0].publish(ros_cloud_filtered);
ROS_DEBUG("published filtered cloud : %f", (ros::Time::now()-tcur).toSec());
tcur = ros::Time::now();
// Let's do a downsampling before doing cluster extraction
pcl::VoxelGrid<pcl::PointXYZ> vg;
vg.setInputCloud (cloud_filtered);
vg.setLeafSize (0.01f, 0.01f, 0.01f);
vg.filter (*cloud_downsampled);
ROS_DEBUG("finished downsampling : %f", (ros::Time::now()-tcur).toSec());
tcur = ros::Time::now();
ROS_DEBUG("Begin extraction filtering");
// build a 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_downsampled);
ROS_DEBUG("done with KdTree initialization : %f",
(ros::Time::now()-tcur).toSec());
tcur = ros::Time::now();
// create a vector for storing the indices of the clusters
std::vector<pcl::PointIndices> cluster_indices;
// setup extraction:
pcl::EuclideanClusterExtraction<pcl::PointXYZ> ec;
ec.setClusterTolerance (0.02); // cm
ec.setMinClusterSize (50);
ec.setMaxClusterSize (3000);
ec.setSearchMethod (tree);
ec.setInputCloud (cloud_downsampled);
// now we can perform cluster extraction
ec.extract (cluster_indices);
ROS_DEBUG("finished extraction : %f", (ros::Time::now()-tcur).toSec());
tcur = ros::Time::now();
// run through the indices, create new clouds, and then publish them
int j=1;
int number_clusters=0;
geometry_msgs::PointStamped pt;
puppeteer_msgs::Robots robots;
std::vector<int> num;
for (std::vector<pcl::PointIndices>::const_iterator
it=cluster_indices.begin();
it!=cluster_indices.end (); ++it)
{
number_clusters = (int) cluster_indices.size();
ROS_DEBUG("Number of clusters found: %d",number_clusters);
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_downsampled->points[*pit]);
}
cloud_cluster->width = cloud_cluster->points.size ();
cloud_cluster->height = 1;
cloud_cluster->is_dense = true;
// convert to rosmsg and publish:
ROS_DEBUG("Publishing extracted cloud");
pcl::toROSMsg(*cloud_cluster, *ros_cloud_filtered);
ros_cloud_filtered->header.frame_id =
"/oriented_optimization_frame";
if(j < MAX_CLUSTERS+1)
cloud_pub[j].publish(ros_cloud_filtered);
j++;
// compute centroid and add to Robots:
pcl::compute3DCentroid(*cloud_cluster, centroid);
pt.point.x = centroid(0);
pt.point.y = centroid(1);
pt.point.z = centroid(2);
pt.header.stamp = tstamp;
pt.header.frame_id = "/oriented_optimization_frame";
robots.robots.push_back(pt);
// add number of points in cluster to num:
num.push_back(cloud_cluster->points.size());
}
ROS_DEBUG("finished creating and publishing clusters : %f",
(ros::Time::now()-tcur).toSec());
tcur = ros::Time::now();
if (number_clusters > number_robots)
{
ROS_WARN("Number of clusters found is greater "
"than the number of robots");
// pop minimum cluster count
remove_least_likely(&robots, &num);
}
robots.header.stamp = tstamp;
robots.header.frame_id = "/oriented_optimization_frame";
robots.number = number_clusters;
robots_pub.publish(robots);
ROS_DEBUG("removed extra clusters, and published : %f",
(ros::Time::now()-tcur).toSec());
tcur = ros::Time::now();
ros::Duration d = ros::Time::now() - start_time;
ROS_DEBUG("End of cloudcb; time elapsed = %f (%f Hz)",
d.toSec(), 1/d.toSec());
}
/*
* Apply a the region growing algorithm.
* Params[in/out]: the inliers detected by Ransac, the output clusters found by RG
* return the number of clusters
*/
int Segmentation::regionGrowing(std::vector < pcl::PointCloud<pcl::PointXYZRGBA>::Ptr > &inliers, std::vector < pcl::PointCloud<pcl::PointXYZRGBA>::Ptr > &clusters)
{
// // REGION GROWING
// if (inliers.size() > 0)
// {
// for(unsigned int i = 0; i < inliers.size(); i++)
// {
// pcl::search::Search<pcl::PointXYZRGBA>::Ptr tree = boost::shared_ptr<pcl::search::Search<pcl::PointXYZRGBA> > (new pcl::search::KdTree<pcl::PointXYZRGBA>);
// pcl::PointCloud <pcl::Normal>::Ptr normals (new pcl::PointCloud <pcl::Normal>);
// pcl::NormalEstimation<pcl::PointXYZRGBA, pcl::Normal> normal_estimator;
// normal_estimator.setSearchMethod (tree);
// normal_estimator.setInputCloud (inliers[i]);
// normal_estimator.setKSearch (50);
// normal_estimator.compute (*normals);
// pcl::RegionGrowing<pcl::PointXYZRGBA, pcl::Normal> reg;
// reg.setMinClusterSize (1000);//1000
// reg.setMaxClusterSize (100000000);
// reg.setSearchMethod (tree);
// reg.setNumberOfNeighbours (30);
// reg.setInputCloud (inliers[i]);
// //reg.setIndices (indices);
// reg.setInputNormals (normals);
// reg.setSmoothnessThreshold (7.0 / 180.0 * M_PI); //4.0 / 6
// reg.setCurvatureThreshold (4.0); //4.0
// std::vector <pcl::PointIndices> clustersIndices;
// reg.extract (clustersIndices);
// //pcl::PointCloud <pcl::PointXYZRGBA>::Ptr cloud_cluster;
// for (std::vector<pcl::PointIndices>::const_iterator it = clustersIndices.begin (); it != clustersIndices.end (); ++it)
// {
// pcl::PointCloud<pcl::PointXYZRGBA>::Ptr cloud_cluster (new pcl::PointCloud<pcl::PointXYZRGBA>);
// for (std::vector<int>::const_iterator pit = it->indices.begin (); pit != it->indices.end (); ++pit)
// {
// cloud_cluster->points.push_back (inliers[i]->points[*pit]);
// }
// cloud_cluster->width = cloud_cluster->points.size ();
// cloud_cluster->height = 1;
// cloud_cluster->is_dense = true;
// clusters.push_back(cloud_cluster);
// }
// }
// return clusters.size();
// }
// else
// {
// std::cerr << "No planes detected" << std::endl;
// return -1;
// }
// REGION GROWING
if (inliers.size() > 0)
{
for(unsigned int i = 0; i < inliers.size(); i++)
{
pcl::search::KdTree<pcl::PointXYZRGBA>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZRGBA>);
tree->setInputCloud (inliers[i]);
std::vector<pcl::PointIndices> cluster_indices;
pcl::EuclideanClusterExtraction<pcl::PointXYZRGBA> ec;
ec.setClusterTolerance (0.02); // 2cm
ec.setMinClusterSize (1000);
ec.setMaxClusterSize (100000000);
ec.setSearchMethod (tree);
ec.setInputCloud (inliers[i]);
ec.extract (cluster_indices);
// pcl::search::Search<pcl::PointXYZRGBA>::Ptr tree = boost::shared_ptr<pcl::search::Search<pcl::PointXYZRGBA> > (new pcl::search::KdTree<pcl::PointXYZRGBA>);
// pcl::PointCloud <pcl::Normal>::Ptr normals (new pcl::PointCloud <pcl::Normal>);
// pcl::NormalEstimation<pcl::PointXYZRGBA, pcl::Normal> normal_estimator;
// normal_estimator.setSearchMethod (tree);
// normal_estimator.setInputCloud (inliers[i]);
// normal_estimator.setKSearch (50);
// normal_estimator.compute (*normals);
// pcl::RegionGrowing<pcl::PointXYZRGBA, pcl::Normal> reg;
// reg.setMinClusterSize (1000);//1000
// reg.setMaxClusterSize (100000000);
// reg.setSearchMethod (tree);
// reg.setNumberOfNeighbours (30);
// reg.setInputCloud (inliers[i]);
// //reg.setIndices (indices);
// reg.setInputNormals (normals);
// reg.setSmoothnessThreshold (7.0 / 180.0 * M_PI); //4.0 / 6
// reg.setCurvatureThreshold (4.0); //4.0
// std::vector <pcl::PointIndices> clustersIndices;
// reg.extract (clustersIndices);
// //pcl::PointCloud <pcl::PointXYZRGBA>::Ptr cloud_cluster;
// for (std::vector<pcl::PointIndices>::const_iterator it = clustersIndices.begin (); it != clustersIndices.end (); ++it)
// {
for (std::vector<pcl::PointIndices>::const_iterator it = cluster_indices.begin (); it != cluster_indices.end (); ++it)
{
pcl::PointCloud<pcl::PointXYZRGBA>::Ptr cloud_cluster (new pcl::PointCloud<pcl::PointXYZRGBA>);
for (std::vector<int>::const_iterator pit = it->indices.begin (); pit != it->indices.end (); ++pit)
{
cloud_cluster->points.push_back (inliers[i]->points[*pit]);
}
cloud_cluster->width = cloud_cluster->points.size ();
cloud_cluster->height = 1;
cloud_cluster->is_dense = true;
clusters.push_back(cloud_cluster);
}
// pcl::PointCloud<pcl::PointXYZRGBA>::Ptr cloud_cluster (new pcl::PointCloud<pcl::PointXYZRGBA>);
// for (std::vector<int>::const_iterator pit = it->indices.begin (); pit != it->indices.end (); ++pit)
// {
// cloud_cluster->points.push_back (inliers[i]->points[*pit]);
// }
// cloud_cluster->width = cloud_cluster->points.size ();
// cloud_cluster->height = 1;
// cloud_cluster->is_dense = true;
// clusters.push_back(cloud_cluster);
// }
}
return clusters.size();
}
else
{
std::cerr << "No planes detected" << std::endl;
return -1;
}
}
/**
*
* @brief Publish the data based on set up parameters.
*
*/
bool publishData() {
/*
* Get frame from camera
*/
status = BTAgetFrame(btaHandle, &frame,3000); //3000;
if (status != BTA_StatusOk) {
std::cout << "\t Could transfer data: " << status << "\n";
return 0;
}
/*
* Obtain XYZcoordinates from 3Dcamera
*/
float_t *xCoordinates, *yCoordinates, *zCoordinates;
BTA_DataFormat dataFormat;
BTA_Unit unit;
uint16_t xRes, yRes;
status = BTAgetXYZcoordinates(frame, (void **)&xCoordinates, (void **)&yCoordinates,
(void **)&zCoordinates, &dataFormat, &unit, &xRes, &yRes);
if (status != BTA_StatusOk) {
std::cout << "\t Could get cartesian coordinates: " << status << "\n";
return 0;
}
/*
* Creating the pointcloud
*/
PointCloud::Ptr cloud_raw (new PointCloud);
cloud_raw->header.frame_id = "map";
cloud_raw->height = 1;
cloud_raw->width = xRes*yRes;
for (size_t i = 0; i < xRes*yRes; ++i) {
pcl::PointXYZ temp_point;
temp_point.x = xCoordinates[i];
temp_point.y = yCoordinates[i];
temp_point.z = zCoordinates[i];
cloud_raw->points.push_back(temp_point);
}
/*
* Downsampling a PointCloud using a VoxelGrid filter
*/
PointCloud::Ptr cloud_filtered (new PointCloud);
pcl::VoxelGrid<pcl::PointXYZ> sor;
sor.setInputCloud (cloud_raw);
sor.setLeafSize (VOXEL_GRID_LEAF_SIZE, VOXEL_GRID_LEAF_SIZE, VOXEL_GRID_LEAF_SIZE);
sor.filter (*cloud_filtered);
/*
* Removing outliers using a StatisticalOutlierRemoval filter
*/
PointCloud::Ptr cloud_filtered_without_outliers (new PointCloud);
pcl::StatisticalOutlierRemoval<pcl::PointXYZ> sor_stat;
sor_stat.setInputCloud (cloud_filtered);
sor_stat.setMeanK (STAT_FILTER_MEAN_K);
sor_stat.setStddevMulThresh (STAT_FILTER_DEV_MUL_THRESH);
sor_stat.filter (*cloud_filtered_without_outliers);
/*
* Euclidean Cluster Extraction
*/
// 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_without_outliers);
pcl::EuclideanClusterExtraction<pcl::PointXYZ> ec;
ec.setClusterTolerance (EUCL_CLUSTER_EXTRAC_TOLERANCE);
ec.setMinClusterSize (EUCL_CLUSTER_EXTRAC_MIN_CLUSTER_SIZE);
ec.setMaxClusterSize (cloud_filtered_without_outliers->size());
ec.setSearchMethod (tree);
ec.setInputCloud (cloud_filtered_without_outliers);
std::vector<pcl::PointIndices> cluster_indices;
ec.extract (cluster_indices);
std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr > vec_cluters;
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_filtered_without_outliers->points[*pit]);
}
vec_cluters.push_back(cloud_cluster);
}
/*
* ConvexHull for every cluster
*/
/*
std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr > vec_convex_hulls;
for (int i = 0; i < vec_cluters.size(); ++i) {
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);
pcl::ConvexHull<pcl::PointXYZ> hull;
hull.setInputCloud (vec_cluters[i]);
hull.reconstruct (*cloud);
vec_convex_hulls.push_back(cloud);
}
*/
/*
* Bounding box for every cluster
*/
obst_min.clear();
obst_max.clear();
for (int i = 0; i < vec_cluters.size(); ++i) {
obst_min.push_back(pcl::PointXYZ());
obst_max.push_back(pcl::PointXYZ());
pcl::getMinMax3D(*vec_cluters[i], obst_min[i], obst_max[i]);
}
/*
* Visualization of convex hulls
*/
visualizer->clearWindow();
for (int i = 0; i < vec_cluters.size(); ++i) {
//visualizer->addPointCloud(vec_cluters[i],i);
//visualizer->addConvexMesh(vec_cluters[i],i);
visualizer->addBox(obst_min[i],obst_max[i],i);
}
visualizer->refresh();
status = BTAfreeFrame(&frame);
if (status != BTA_StatusOk) {
std::cout << "\t Could not free frame: " << status << "\n";
return 0;
}
return 1;
}
void
cloud_cb (const sensor_msgs::PointCloud2ConstPtr& input)
{
// Convert the sensor_msgs/PointCloud2 data to pcl/PointCloud
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>), cloud_f (new pcl::PointCloud<pcl::PointXYZ>);
pcl::fromROSMsg (*input, *cloud);
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);
pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients);
pcl::PointIndices::Ptr inliers (new pcl::PointIndices);
pcl::SACSegmentation<pcl::PointXYZ> seg;
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_plane (new pcl::PointCloud<pcl::PointXYZ>() );
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)
{
seg.setInputCloud(cloud_filtered);
seg.segment(*inliers, *coefficients);
if(inliers -> indices.size() == 0)
break;
pcl::ExtractIndices<pcl::PointXYZ> extract;
extract.setInputCloud(cloud_filtered);
extract.setIndices(inliers);
extract.setNegative(false);
extract.filter(*cloud_plane);
extract.setNegative(true);
extract.filter(*cloud_f);
*cloud_filtered = *cloud_f;
}
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);
ec.setMinClusterSize(100);
ec.setMaxClusterSize(25000);
ec.setSearchMethod(tree);
ec.setInputCloud(cloud_filtered);
ec.extract(cluster_indices);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_cluster (new pcl::PointCloud<pcl::PointXYZ>);
for (std::vector<pcl::PointIndices>::const_iterator it = cluster_indices.begin (); it != cluster_indices.end (); ++it)
{
for (std::vector<int>::const_iterator pit = it->indices.begin (); pit != it->indices.end (); ++pit)
cloud_cluster->points.push_back (cloud_filtered->points[*pit]);
}
sensor_msgs::PointCloud2 output;
pcl::toROSMsg(*cloud_cluster, output);
pub.publish (output);
}
int
main (int argc, char** argv)
{
// Read in the cloud data
pcl::PCDReader reader;
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>), cloud_f (new pcl::PointCloud<pcl::PointXYZ>);
reader.read ("table_scene_lms400.pcd", *cloud);
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 (100);
ec.setMaxClusterSize (25000);
ec.setSearchMethod (tree);
ec.setInputCloud (cloud_filtered);
ec.extract (cluster_indices);
int j = 0;
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_filtered->points[*pit]); //*
cloud_cluster->width = cloud_cluster->points.size ();
cloud_cluster->height = 1;
cloud_cluster->is_dense = true;
// Visualization removed noise
printf("\ncloud_cluster\n");
pcl::visualization::PCLVisualizer viewer1 ("cloud_cluster");
pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> cloud_cluster_handler (cloud_cluster, 0, 0, 0); // Where 255,255,255 are R,G,B colors
viewer1.addPointCloud (cloud_cluster, cloud_cluster_handler, "cloud_cluster"); // We add the point cloud to the viewer and pass the color handler
//viewer.addCoordinateSystem (1.0, "cloud", 0);
viewer1.setBackgroundColor(0.95, 0.95, 0.95, 0); // Setting background to a dark grey
viewer1.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "cloud_cluster");
//viewer.setPosition(800, 400); // Setting visualiser window position
while (!viewer1.wasStopped ()) { // Display the visualiser untill 'q' key is pressed
viewer1.spinOnce ();
}
std::cout << "PointCloud representing the Cluster: " << cloud_cluster->points.size () << " data points." << std::endl;
std::stringstream ss;
ss << "cloud_cluster_" << j << ".pcd";
writer.write<pcl::PointXYZ> (ss.str (), *cloud_cluster, false); //*
j++;
}
return (0);
}
void cloud_cb_ (const pcl::PointCloud<pcl::PointXYZ>::ConstPtr &cloud) {
if (!viewer.wasStopped()) {
// viewer.showCloud (cloud);
// writer.write<pcl::PointXYZ> ("kinect_cloud.pcd", *cloud, false); //*
// 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;
}
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_f (new pcl::PointCloud<pcl::PointXYZ>);
// Extract the planar inliers from the input cloud
pcl::ExtractIndices<pcl::PointXYZ> extract;
extract.setInputCloud (cloud_filtered);
extract.setIndices (inliers);
extract.setNegative (false);
// Write the planar inliers to disk
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 (100);
ec.setMaxClusterSize (25000);
ec.setSearchMethod (tree);
ec.setInputCloud (cloud_filtered);
ec.extract (cluster_indices);
int j = 0;
std::vector<pcl::PointIndices>::const_iterator it = cluster_indices.begin ();
// 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_filtered->points[*pit]); //*
cloud_cluster->width = cloud_cluster->points.size ();
cloud_cluster->height = 1;
cloud_cluster->is_dense = true;
// std::cout << "PointCloud representing the Cluster: " << cloud_cluster->points.size () << " data points." << std::endl;
// std::stringstream ss;
// ss << "cloud_cluster_" << j << ".pcd";
// writer.write<pcl::PointXYZ> (ss.str (), *cloud_cluster, false); //*
// j++;
viewer.showCloud (cloud_cluster);
}
}
}
int main (int argc, char** argv)
{
//---------------------------------------------------------------------------------------------------
//-- Initialization stuff
//---------------------------------------------------------------------------------------------------
//-- Command-line arguments
float ransac_threshold = 0.02;
float hsv_s_threshold = 0.30;
float hsv_v_threshold = 0.35;
//-- Show usage
if (pcl::console::find_switch(argc, argv, "-h") || pcl::console::find_switch(argc, argv, "--help"))
{
show_usage(argv[0]);
return 0;
}
if (pcl::console::find_switch(argc, argv, "--ransac-threshold"))
pcl::console::parse_argument(argc, argv, "--ransac-threshold", ransac_threshold);
else
{
std::cerr << "RANSAC theshold not specified, using default value..." << std::endl;
}
if (pcl::console::find_switch(argc, argv, "--hsv-s-threshold"))
pcl::console::parse_argument(argc, argv, "--hsv-s-threshold", hsv_s_threshold);
else
{
std::cerr << "Saturation theshold not specified, using default value..." << std::endl;
}
if (pcl::console::find_switch(argc, argv, "--hsv-v-threshold"))
pcl::console::parse_argument(argc, argv, "--hsv-v-threshold", hsv_v_threshold);
else
{
std::cerr << "Value theshold not specified, using default value..." << std::endl;
}
//-- Get point cloud file from arguments
std::vector<int> filenames;
bool file_is_pcd = false;
filenames = pcl::console::parse_file_extension_argument(argc, argv, ".ply");
if (filenames.size() != 1)
{
filenames = pcl::console::parse_file_extension_argument(argc, argv, ".pcd");
if (filenames.size() != 1)
{
show_usage(argv[0]);
return -1;
}
file_is_pcd = true;
}
//-- Load point cloud data
pcl::PointCloud<pcl::PointXYZ>::Ptr source_cloud(new pcl::PointCloud<pcl::PointXYZ>);
if (file_is_pcd)
{
if (pcl::io::loadPCDFile(argv[filenames[0]], *source_cloud) < 0)
{
std::cout << "Error loading point cloud " << argv[filenames[0]] << std::endl << std::endl;
show_usage(argv[0]);
return -1;
}
}
else
{
if (pcl::io::loadPLYFile(argv[filenames[0]], *source_cloud) < 0)
{
std::cout << "Error loading point cloud " << argv[filenames[0]] << std::endl << std::endl;
show_usage(argv[0]);
return -1;
}
}
//-- Load point cloud data (with color)
pcl::PointCloud<pcl::PointXYZRGB>::Ptr source_cloud_color(new pcl::PointCloud<pcl::PointXYZRGB>);
if (file_is_pcd)
{
if (pcl::io::loadPCDFile(argv[filenames[0]], *source_cloud_color) < 0)
{
std::cout << "Error loading colored point cloud " << argv[filenames[0]] << std::endl << std::endl;
show_usage(argv[0]);
return -1;
}
}
else
{
if (pcl::io::loadPLYFile(argv[filenames[0]], *source_cloud_color) < 0)
{
std::cout << "Error loading colored point cloud " << argv[filenames[0]] << std::endl << std::endl;
show_usage(argv[0]);
return -1;
}
}
//-- Print arguments to user
std::cout << "Selected arguments: " << std::endl
<< "\tRANSAC threshold: " << ransac_threshold << std::endl
<< "\tColor point threshold: " << hsv_s_threshold << std::endl
<< "\tColor region threshold: " << hsv_v_threshold << std::endl;
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered(new pcl::PointCloud<pcl::PointXYZ>);
//--------------------------------------------------------------------------------------------------------
//-- Program does actual work from here
//--------------------------------------------------------------------------------------------------------
Debug debug;
debug.setAutoShow(false);
debug.setEnabled(false);
debug.setEnabled(true);
debug.plotPointCloud<pcl::PointXYZRGB>(source_cloud_color, Debug::COLOR_ORIGINAL);
debug.show("Original with color");
//-- Downsample the dataset prior to plane detection (using a leaf size of 1cm)
//-----------------------------------------------------------------------------------
pcl::VoxelGrid<pcl::PointXYZ> voxel_grid;
voxel_grid.setInputCloud(source_cloud);
voxel_grid.setLeafSize(0.01f, 0.01f, 0.01f);
voxel_grid.filter(*cloud_filtered);
std::cout << "Initially PointCloud has: " << source_cloud->points.size () << " data points." << std::endl;
std::cout << "PointCloud after filtering has: " << cloud_filtered->points.size () << " data points." << std::endl;
//-- Detect all possible planes
//-----------------------------------------------------------------------------------
std::vector<pcl::ModelCoefficientsPtr> all_planes;
pcl::SACSegmentation<pcl::PointXYZ> ransac_segmentation;
ransac_segmentation.setOptimizeCoefficients(true);
ransac_segmentation.setModelType(pcl::SACMODEL_PLANE);
ransac_segmentation.setMethodType(pcl::SAC_RANSAC);
ransac_segmentation.setDistanceThreshold(ransac_threshold);
pcl::PointIndices::Ptr inliers(new pcl::PointIndices);
pcl::ModelCoefficients::Ptr current_plane(new pcl::ModelCoefficients);
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
ransac_segmentation.setInputCloud(cloud_filtered);
ransac_segmentation.segment(*inliers, *current_plane);
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::PointCloud<pcl::PointXYZ>::Ptr cloud_f(new pcl::PointCloud<pcl::PointXYZ>);
pcl::ExtractIndices<pcl::PointXYZ> extract;
extract.setInputCloud(cloud_filtered);
extract.setIndices(inliers);
extract.setNegative(false);
// Remove the planar inliers, extract the rest
extract.setNegative(true);
extract.filter(*cloud_f);
*cloud_filtered = *cloud_f;
//-- Save plane
pcl::ModelCoefficients::Ptr copy_current_plane(new pcl::ModelCoefficients);
*copy_current_plane = *current_plane;
all_planes.push_back(copy_current_plane);
//-- Debug stuff
debug.setEnabled(false);
debug.plotPlane(*current_plane, Debug::COLOR_BLUE);
debug.plotPointCloud<pcl::PointXYZ>(cloud_filtered, Debug::COLOR_RED);
debug.show("Plane segmentation");
}
//-- Filter planes to obtain garment plane
//-----------------------------------------------------------------------------------
pcl::ModelCoefficients::Ptr garment_plane(new pcl::ModelCoefficients);
float min_height = FLT_MAX;
pcl::PointXYZ garment_projected_center;
for(int i = 0; i < all_planes.size(); i++)
{
//-- Check orientation
Eigen::Vector3f normal_vector(all_planes[i]->values[0],
all_planes[i]->values[1],
all_planes[i]->values[2]);
normal_vector.normalize();
Eigen::Vector3f good_orientation(0, -1, -1);
good_orientation.normalize();
std::cout << "Checking vector with dot product: " << std::abs(normal_vector.dot(good_orientation)) << std::endl;
if (std::abs(normal_vector.dot(good_orientation)) >= 0.9)
{
//-- Check "height" (height is defined in the local frame of reference in the yz direction)
//-- With this frame, it is approximately equal to the norm of the vector OO' (being O the
//-- center of the old frame and O' the projection of that center onto the plane).
//-- Project center point onto given plane:
pcl::PointCloud<pcl::PointXYZ>::Ptr center_to_be_projected_cloud(new pcl::PointCloud<pcl::PointXYZ>);
center_to_be_projected_cloud->points.push_back(pcl::PointXYZ(0,0,0));
pcl::PointCloud<pcl::PointXYZ>::Ptr center_projected_cloud(new pcl::PointCloud<pcl::PointXYZ>);
pcl::ProjectInliers<pcl::PointXYZ> project_inliners;
project_inliners.setModelType(pcl::SACMODEL_PLANE);
project_inliners.setInputCloud(center_to_be_projected_cloud);
project_inliners.setModelCoefficients(all_planes[i]);
project_inliners.filter(*center_projected_cloud);
pcl::PointXYZ projected_center = center_projected_cloud->points[0];
Eigen::Vector3f projected_center_vector(projected_center.x, projected_center.y, projected_center.z);
float height = projected_center_vector.norm();
if (height < min_height)
{
min_height = height;
*garment_plane = *all_planes[i];
garment_projected_center = projected_center;
}
}
}
if (!(min_height < FLT_MAX))
{
std::cerr << "Garment plane not found!" << std::endl;
return -3;
}
else
{
std::cout << "Found closest plane with h=" << min_height << std::endl;
//-- Debug stuff
debug.setEnabled(true);
debug.plotPlane(*garment_plane, Debug::COLOR_BLUE);
debug.plotPointCloud<pcl::PointXYZ>(source_cloud, Debug::COLOR_RED);
debug.show("Garment plane");
}
//-- Reorient cloud to origin (with color point cloud)
//-----------------------------------------------------------------------------------
//-- Translating to center
//pcl::PointCloud<pcl::PointXYZRGB>::Ptr centered_cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
Eigen::Affine3f translation_transform = Eigen::Affine3f::Identity();
translation_transform.translation() << -garment_projected_center.x, -garment_projected_center.y, -garment_projected_center.z;
//pcl::transformPointCloud(*source_cloud_color, *centered_cloud, translation_transform);
//-- Orient using the plane normal
pcl::PointCloud<pcl::PointXYZRGB>::Ptr oriented_cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
Eigen::Vector3f normal_vector(garment_plane->values[0], garment_plane->values[1], garment_plane->values[2]);
//-- Check normal vector orientation
if (normal_vector.dot(Eigen::Vector3f::UnitZ()) >= 0 && normal_vector.dot(Eigen::Vector3f::UnitY()) >= 0)
normal_vector = -normal_vector;
Eigen::Quaternionf rotation_quaternion = Eigen::Quaternionf().setFromTwoVectors(normal_vector, Eigen::Vector3f::UnitZ());
//pcl::transformPointCloud(*centered_cloud, *oriented_cloud, Eigen::Vector3f(0,0,0), rotation_quaternion);
Eigen::Transform<float, 3, Eigen::Affine> t(rotation_quaternion * translation_transform);
pcl::transformPointCloud(*source_cloud_color, *oriented_cloud, t);
//-- Save to file
record_transformation(argv[filenames[0]]+std::string("-transform1.txt"), translation_transform, rotation_quaternion);
debug.setEnabled(true);
debug.plotPointCloud<pcl::PointXYZRGB>(oriented_cloud, Debug::COLOR_GREEN);
debug.show("Oriented");
//-- Filter points under the garment table
//-----------------------------------------------------------------------------------
pcl::PointCloud<pcl::PointXYZRGB>::Ptr garment_table_cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::PassThrough<pcl::PointXYZRGB> passthrough_filter;
passthrough_filter.setInputCloud(oriented_cloud);
passthrough_filter.setFilterFieldName("z");
passthrough_filter.setFilterLimits(-ransac_threshold/2.0f, FLT_MAX);
passthrough_filter.setFilterLimitsNegative(false);
passthrough_filter.filter(*garment_table_cloud);
debug.setEnabled(true);
debug.plotPointCloud<pcl::PointXYZRGB>(garment_table_cloud, Debug::COLOR_GREEN);
debug.show("Table cloud (filtered)");
//-- Color segmentation of the garment
//-----------------------------------------------------------------------------------
//-- HSV thresholding
pcl::PointCloud<pcl::PointXYZHSV>::Ptr hsv_cloud(new pcl::PointCloud<pcl::PointXYZHSV>);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr filtered_garment_cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::PointCloudXYZRGBtoXYZHSV(*garment_table_cloud, *hsv_cloud);
for (int i = 0; i < hsv_cloud->points.size(); i++)
{
if (isnan(hsv_cloud->points[i].x) || isnan(hsv_cloud->points[i].y || isnan(hsv_cloud->points[i].z)))
continue;
if (hsv_cloud->points[i].s > hsv_s_threshold && hsv_cloud->points[i].v > hsv_v_threshold)
filtered_garment_cloud->push_back(garment_table_cloud->points[i]);
}
debug.setEnabled(true);
debug.plotPointCloud<pcl::PointXYZRGB>(filtered_garment_cloud, Debug::COLOR_GREEN);
debug.show("Garment cloud");
//-- Euclidean Clustering of the resultant cloud
pcl::search::KdTree<pcl::PointXYZRGB>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZRGB>);
tree->setInputCloud(filtered_garment_cloud);
std::vector<pcl::PointIndices> cluster_indices;
pcl::EuclideanClusterExtraction<pcl::PointXYZRGB> euclidean_custering;
euclidean_custering.setClusterTolerance(0.005);
euclidean_custering.setMinClusterSize(100);
euclidean_custering.setSearchMethod(tree);
euclidean_custering.setInputCloud(filtered_garment_cloud);
euclidean_custering.extract(cluster_indices);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr largest_color_cluster(new pcl::PointCloud<pcl::PointXYZRGB>);
int largest_cluster_size = 0;
for (auto it = cluster_indices.begin (); it != cluster_indices.end (); ++it)
{
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_cluster (new pcl::PointCloud<pcl::PointXYZRGB>);
for (auto pit = it->indices.begin (); pit != it->indices.end (); ++pit)
cloud_cluster->points.push_back(filtered_garment_cloud->points[*pit]);
cloud_cluster->width = cloud_cluster->points.size ();
cloud_cluster->height = 1;
cloud_cluster->is_dense = true;
std::cout << "Found cluster of " << cloud_cluster->points.size() << " points." << std::endl;
if (cloud_cluster->points.size() > largest_cluster_size)
{
largest_cluster_size = cloud_cluster->points.size();
*largest_color_cluster = *cloud_cluster;
}
}
debug.setEnabled(true);
debug.plotPointCloud<pcl::PointXYZRGB>(largest_color_cluster, Debug::COLOR_GREEN);
debug.show("Filtered garment cloud");
//-- Centering the point cloud before saving it
//-----------------------------------------------------------------------------------
//-- Find bounding box
pcl::MomentOfInertiaEstimation<pcl::PointXYZRGB> feature_extractor;
pcl::PointXYZRGB min_point_AABB, max_point_AABB;
pcl::PointXYZRGB min_point_OBB, max_point_OBB;
pcl::PointXYZRGB position_OBB;
Eigen::Matrix3f rotational_matrix_OBB;
feature_extractor.setInputCloud(largest_color_cluster);
feature_extractor.compute();
feature_extractor.getAABB(min_point_AABB, max_point_AABB);
feature_extractor.getOBB(min_point_OBB, max_point_OBB, position_OBB, rotational_matrix_OBB);
//-- Translating to center
pcl::PointCloud<pcl::PointXYZRGB>::Ptr centered_garment_cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
Eigen::Affine3f garment_translation_transform = Eigen::Affine3f::Identity();
garment_translation_transform.translation() << -position_OBB.x, -position_OBB.y, -position_OBB.z;
pcl::transformPointCloud(*largest_color_cluster, *centered_garment_cloud, garment_translation_transform);
//-- Orient using the principal axes of the bounding box
pcl::PointCloud<pcl::PointXYZRGB>::Ptr oriented_garment_cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
Eigen::Vector3f principal_axis_x(max_point_OBB.x - min_point_OBB.x, 0, 0);
Eigen::Quaternionf garment_rotation_quaternion = Eigen::Quaternionf().setFromTwoVectors(principal_axis_x, Eigen::Vector3f::UnitX()); //-- This transformation is wrong (I guess)
Eigen::Transform<float, 3, Eigen::Affine> t2 = Eigen::Transform<float, 3, Eigen::Affine>::Identity();
t2.rotate(rotational_matrix_OBB.inverse());
//pcl::transformPointCloud(*centered_garment_cloud, *oriented_garment_cloud, Eigen::Vector3f(0,0,0), garment_rotation_quaternion);
pcl::transformPointCloud(*centered_garment_cloud, *oriented_garment_cloud, t2);
//-- Save to file
record_transformation(argv[filenames[0]]+std::string("-transform2.txt"), garment_translation_transform, Eigen::Quaternionf(t2.rotation()));
debug.setEnabled(true);
debug.plotPointCloud<pcl::PointXYZRGB>(oriented_garment_cloud, Debug::COLOR_GREEN);
debug.plotBoundingBox(min_point_OBB, max_point_OBB, position_OBB, rotational_matrix_OBB, Debug::COLOR_YELLOW);
debug.show("Oriented garment patch");
//-- Save point cloud in file to process it in Python
pcl::io::savePCDFileBinary(argv[filenames[0]]+std::string("-output.pcd"), *oriented_garment_cloud);
return 0;
}
void ClusterExtraction::processCloud(float plane_tolerance)
{
ros::Time stamp = ros::Time::now();
if(!pcl_data)
{
ROS_INFO("No xtion_camera data.");
sleep(1);
return;
}
//pcl::PointCloud<PoinT>::Ptr _cloud;// (new pcl::PointCloud<PoinT>);
sensor_msgs::PointCloud2ConstPtr _temp_cloud_ = pcl_data;
pcl::PointCloud<PoinT>::Ptr _cloud (new pcl::PointCloud<PoinT>);
pcl::fromROSMsg(*_temp_cloud_, *_cloud);
pcl::VoxelGrid<PoinT> vg_filter;
pcl::PointCloud<PoinT>::Ptr cloud_filtered (new pcl::PointCloud<PoinT>);
vg_filter.setInputCloud (_cloud);
vg_filter.setLeafSize (0.01f, 0.01f, 0.01f);
vg_filter.filter (*cloud_filtered);
_cloud = cloud_filtered;
/**********************************************
* NEW BULL
*********************************************/
//////////////////////////////////////////////////////////////////////
// Cluster Extraction
//////////////////////////////////////////////////////////////////////
// Findout the points that are more than 1.25 m away.
pcl::PointIndices::Ptr out_of_range_points (new pcl::PointIndices);
unsigned int i = 0;
BOOST_FOREACH(const PoinT& pt, _cloud->points)
{
if(sqrt( (pt.x*pt.x) + (pt.y*pt.y) + (pt.z*pt.z) ) > 1.5 || isnan (pt.x) || isnan (pt.y) || isnan (pt.z) ||
isinf (pt.x) || isinf (pt.y) || isinf (pt.z) )
out_of_range_points->indices.push_back(i);
i++;
}
pcl::ExtractIndices<PoinT> extract;
pcl::PointCloud<PoinT>::Ptr cloud (new pcl::PointCloud<PoinT>);
// Perform the extraction of these points (indices).
extract.setInputCloud(_cloud);
extract.setIndices(out_of_range_points);
extract.setNegative (true);
extract.filter (*cloud);
// Prepare plane segmentation.
pcl::SACSegmentation<PoinT> seg;
pcl::PointIndices::Ptr inliers (new pcl::PointIndices);
pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients);
pcl::PointCloud<PoinT>::Ptr cloud_plane; // This door is opened elsewhere.
pcl::PointCloud<PoinT>::Ptr cloud_f (new pcl::PointCloud<PoinT> ());
seg.setOptimizeCoefficients (true);
seg.setModelType (pcl::SACMODEL_PLANE);
seg.setMethodType (pcl::SAC_RANSAC);
seg.setMaxIterations (1000);
seg.setDistanceThreshold (0.02);
// Remove the planes.
i = 0;
int nr_points = (int) cloud->points.size();
tf::StampedTransform base_link_to_openni;
try
{
tf_listener->waitForTransform("base_link", cloud->header.frame_id, ros::Time(0), ros::Duration(1));
//tf_listener->transformPoint("base_link", plane_normal, _plane_normal);
tf_listener->lookupTransform("base_link", cloud->header.frame_id, ros::Time(0), base_link_to_openni);
}
catch(tf::TransformException& ex)
{
ROS_INFO("COCKED UP POINT INFO! Why: %s", ex.what());
}
// We do this here so that we can put in an empty cluster, if we see no table in the first place.
doro_msgs::ClusterArray __clusters;
while (cloud->points.size () > 0.5 * nr_points)
{
seg.setInputCloud (cloud);
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
extract.setInputCloud (cloud);
extract.setIndices (inliers);
extract.setNegative (false);
extract.filter (*cloud_f);
// Is this a parallel to ground plane? If yes, save it.
tf::Vector3 plane_normal (coefficients->values[0], coefficients->values[1], coefficients->values[2]);
tf::Vector3 _plane_normal = base_link_to_openni*plane_normal;
// What's the angle between this vector and the actual z axis? cos_inverse ( j component )...
tf::Vector3 normal (_plane_normal.x(), _plane_normal.y(), _plane_normal.z());
normal = normal.normalized();
//std::cout<<"x: "<<normal.x()<<"\t";
//std::cout<<"y: "<<normal.y()<<"\t";
//std::cout<<"z: "<<normal.z()<<"\t";
if(acos (normal.z()) < plane_tolerance)
{
cloud_plane = pcl::PointCloud<PoinT>::Ptr(new pcl::PointCloud<PoinT>);
*cloud_plane = *cloud_f;
}
extract.setNegative (true);
extract.filter (*cloud_f);
*cloud = *cloud_f;
}
if(!cloud_plane)
{
ROS_INFO("No table or table-like object could be seen. Can't extract...");
//clusters_pub.publish(__clusters);
//sleep(1);
return;
}
//ROS_INFO("Table seen.");
//table_cloud_pub.publish(cloud_plane);
//////////////////////////////////////////////////////////////////////
/**
* COMPUTE THE CENTROID OF THE PLANE AND PUBLISH IT.
*/
//////////////////////////////////////////////////////////////////////
Eigen::Vector4f plane_cen;
// REMOVE COMMENTS WITH REAL ROBOT!!!
pcl::compute3DCentroid(*cloud_plane, plane_cen);
//std::cout<< plane_cen;
tf::Vector3 plane_centroid (plane_cen[0], plane_cen[1], plane_cen[2]);
tf::Vector3 _plane_centroid = base_link_to_openni*plane_centroid;
geometry_msgs::PointStamped _plane_centroid_ROSMsg;
_plane_centroid_ROSMsg.header.frame_id = "base_link";
_plane_centroid_ROSMsg.header.stamp = stamp;
_plane_centroid_ROSMsg.point.x = _plane_centroid.x();
_plane_centroid_ROSMsg.point.y = _plane_centroid.y();
_plane_centroid_ROSMsg.point.z = _plane_centroid.z();
// Publish the centroid.
table_position_pub.publish(_plane_centroid_ROSMsg);
pcl::search::KdTree<PoinT>::Ptr tree (new pcl::search::KdTree<PoinT>);
if(cloud->points.size() == 0)
{
clusters_pub.publish(__clusters);
return;
}
tree->setInputCloud (cloud);
std::vector<pcl::PointIndices> cluster_indices;
pcl::EuclideanClusterExtraction<PoinT> ec;
ec.setClusterTolerance (0.02); // 2cm
ec.setMinClusterSize (20);
ec.setMaxClusterSize (25000);
ec.setSearchMethod (tree);
ec.setInputCloud (cloud);
ec.extract (cluster_indices);
//ROS_INFO("WIDTH: %d, HEIGHT: %d\n", _cloud->width, _cloud->height);
//ROS_INFO("GOOD TILL HERE!");
int j = 0;
for (std::vector<pcl::PointIndices>::const_iterator it = cluster_indices.begin (); it != cluster_indices.end (); ++it)
{
pcl::PointCloud<PoinT>::Ptr cloud_cluster (new pcl::PointCloud<PoinT>);
cloud_cluster->header.frame_id = cloud->header.frame_id;
long int color_r, color_g, color_b;
uint8_t mean_r, mean_g, mean_b;
for (std::vector<int>::const_iterator pit = it->indices.begin (); pit != it->indices.end (); pit++)
{
cloud_cluster->points.push_back (cloud->points[*pit]);
/* ***************** */
/* COLOR COMPUTATION */
/* ***************** */
color_r += cloud->points[*pit].r;
color_g += cloud->points[*pit].g;
color_b += cloud->points[*pit].b;
}
geometry_msgs::Point a, b;
std::vector <double> cluster_dims = getClusterDimensions(cloud_cluster, a, b);
mean_r = (uint8_t) (color_r / cloud_cluster->points.size ());
mean_g = (uint8_t) (color_g / cloud_cluster->points.size ());
mean_b = (uint8_t) (color_b / cloud_cluster->points.size ());
cloud_cluster->width = cloud_cluster->points.size ();
cloud_cluster->height = 1;
cloud_cluster->is_dense = true;
cloud_cluster->header.frame_id = cloud->header.frame_id;
Eigen::Vector4f cluster_cen;
pcl::compute3DCentroid(*cloud_cluster, cluster_cen);
tf::Vector3 cluster_centroid (cluster_cen[0], cluster_cen[1], cluster_cen[2]);
tf::Vector3 _cluster_centroid = base_link_to_openni*cluster_centroid;
geometry_msgs::PointStamped _cluster_centroid_ROSMsg;
_cluster_centroid_ROSMsg.header.frame_id = "base_link";
_cluster_centroid_ROSMsg.header.stamp = stamp;
_cluster_centroid_ROSMsg.point.x = _cluster_centroid.x();
_cluster_centroid_ROSMsg.point.y = _cluster_centroid.y();
_cluster_centroid_ROSMsg.point.z = _cluster_centroid.z();
if(DIST(cluster_centroid,plane_centroid) < 0.6 && _cluster_centroid.z() > _plane_centroid.z())
{
doro_msgs::Cluster __cluster;
__cluster.centroid = _cluster_centroid_ROSMsg;
// Push cluster dimentions. Viewed width, breadth and height
__cluster.cluster_size = cluster_dims;
__cluster.a = a;
__cluster.b = b;
// Push colors
__cluster.color.push_back(mean_r);
__cluster.color.push_back(mean_g);
__cluster.color.push_back(mean_b);
__clusters.clusters.push_back (__cluster);
j++;
}
}
clusters_pub.publish(__clusters);
/////////////// RUBBISH ENDS ////////////////
//if(pcl_data)
// pcl_data.reset();
}