bool EuclideanClustering::serviceCallback(
jsk_recognition_msgs::EuclideanSegment::Request &req,
jsk_recognition_msgs::EuclideanSegment::Response &res)
{
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);
pcl::fromROSMsg(req.input, *cloud);
#if ( PCL_MAJOR_VERSION >= 1 && PCL_MINOR_VERSION >= 5 )
pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ>);
tree = boost::make_shared< pcl::search::KdTree<pcl::PointXYZ> > ();
#else
pcl::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::KdTreeFLANN<pcl::PointXYZ>);
tree = boost::make_shared<pcl::KdTreeFLANN<pcl::PointXYZ> > ();
#endif
vector<pcl::PointIndices> cluster_indices;
EuclideanClusterExtraction<pcl::PointXYZ> impl;
double tor;
if ( req.tolerance < 0) {
tor = tolerance;
} else {
tor = req.tolerance;
}
impl.setClusterTolerance (tor);
impl.setMinClusterSize (minsize_);
impl.setMaxClusterSize (maxsize_);
impl.setSearchMethod (tree);
impl.setInputCloud (cloud);
impl.extract (cluster_indices);
res.output.resize( cluster_indices.size() );
#if ( PCL_MAJOR_VERSION >= 1 && PCL_MINOR_VERSION >= 7 )
pcl::PCLPointCloud2::Ptr pcl_cloud(new pcl::PCLPointCloud2);
pcl_conversions::toPCL(req.input, *pcl_cloud);
pcl::ExtractIndices<pcl::PCLPointCloud2> ex;
ex.setInputCloud(pcl_cloud);
#else
pcl::ExtractIndices<sensor_msgs::PointCloud2> ex;
ex.setInputCloud ( boost::make_shared< sensor_msgs::PointCloud2 > (req.input) );
#endif
for ( size_t i = 0; i < cluster_indices.size(); i++ ) {
ex.setIndices ( boost::make_shared< pcl::PointIndices > (cluster_indices[i]) );
ex.setNegative ( false );
#if ( PCL_MAJOR_VERSION >= 1 && PCL_MINOR_VERSION >= 7 )
pcl::PCLPointCloud2 output_cloud;
ex.filter ( output_cloud );
pcl_conversions::fromPCL(output_cloud, res.output[i]);
#else
ex.filter ( res.output[i] );
#endif
}
return true;
}
static void clustering(PointCloudPtr cloudPCLInput, vector<PointIndices> &vecSegmentIndices, double dClusterTolerance, unsigned int unMinClusterSize, unsigned int unMaxClusterSize)
{
vecSegmentIndices.clear();
typedef pcl::search::KdTree<PointT> KdTree;
typedef typename KdTree::Ptr KdTreePtr;
EuclideanClusterExtraction<PointT> cluster;
KdTreePtr cluster_tree = boost::make_shared<pcl::search::KdTree<PointT> > ();
cluster.setInputCloud(cloudPCLInput);
cluster.setClusterTolerance(dClusterTolerance);
cluster.setMinClusterSize(unMinClusterSize);
cluster.setMaxClusterSize(unMaxClusterSize);
cluster.setSearchMethod(cluster_tree);
cluster.extract(vecSegmentIndices);
}
void EuclideanClustering::extract(
const sensor_msgs::PointCloud2ConstPtr &input)
{
boost::mutex::scoped_lock lock(mutex_);
vital_checker_->poke();
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud
(new pcl::PointCloud<pcl::PointXYZ>);
pcl::fromROSMsg(*input, *cloud);
std::vector<pcl::PointIndices> cluster_indices;
// list up indices which are not NaN to use
// organized pointcloud
pcl::PointIndices::Ptr nonnan_indices (new pcl::PointIndices);
for (size_t i = 0; i < cloud->points.size(); i++) {
pcl::PointXYZ p = cloud->points[i];
if (!isnan(p.x) && !isnan(p.y) && !isnan(p.z)) {
nonnan_indices->indices.push_back(i);
}
}
if (nonnan_indices->indices.size() == 0) {
// if input points is 0, publish empty data as result
jsk_recognition_msgs::ClusterPointIndices result;
result.header = input->header;
result_pub_.publish(result);
// do nothing and return it
jsk_recognition_msgs::Int32Stamped::Ptr cluster_num_msg (new jsk_recognition_msgs::Int32Stamped);
cluster_num_msg->header = input->header;
cluster_num_msg->data = 0;
cluster_num_pub_.publish(cluster_num_msg);
return;
}
EuclideanClusterExtraction<pcl::PointXYZ> impl;
{
jsk_topic_tools::ScopedTimer timer = kdtree_acc_.scopedTimer();
#if ( PCL_MAJOR_VERSION >= 1 && PCL_MINOR_VERSION >= 5 )
pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ>);
tree = boost::make_shared< pcl::search::KdTree<pcl::PointXYZ> > ();
#else
pcl::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::KdTreeFLANN<pcl::PointXYZ>);
tree = boost::make_shared<pcl::KdTreeFLANN<pcl::PointXYZ> > ();
#endif
tree->setInputCloud (cloud);
impl.setClusterTolerance (tolerance);
impl.setMinClusterSize (minsize_);
impl.setMaxClusterSize (maxsize_);
impl.setSearchMethod (tree);
impl.setIndices(nonnan_indices);
impl.setInputCloud (cloud);
}
{
jsk_topic_tools::ScopedTimer timer = segmentation_acc_.scopedTimer();
impl.extract (cluster_indices);
}
// Publish result indices
jsk_recognition_msgs::ClusterPointIndices result;
result.cluster_indices.resize(cluster_indices.size());
cluster_counter_.add(cluster_indices.size());
result.header = input->header;
if (cogs_.size() != 0 && cogs_.size() == cluster_indices.size()) {
// tracking the labels
//ROS_INFO("computing distance matrix");
// compute distance matrix
// D[i][j] --> distance between the i-th previous cluster
// and the current j-th cluster
Vector4fVector new_cogs;
computeCentroidsOfClusters(new_cogs, cloud, cluster_indices);
double D[cogs_.size() * new_cogs.size()];
computeDistanceMatrix(D, cogs_, new_cogs);
std::vector<int> pivot_table = buildLabelTrackingPivotTable(D, cogs_, new_cogs, label_tracking_tolerance);
if (pivot_table.size() != 0) {
cluster_indices = pivotClusterIndices(pivot_table, cluster_indices);
}
}
Vector4fVector tmp_cogs;
computeCentroidsOfClusters(tmp_cogs, cloud, cluster_indices); // NB: not efficient
cogs_ = tmp_cogs;
for (size_t i = 0; i < cluster_indices.size(); i++) {
#if ROS_VERSION_MINIMUM(1, 10, 0)
// hydro and later
result.cluster_indices[i].header
= pcl_conversions::fromPCL(cluster_indices[i].header);
#else
// groovy
result.cluster_indices[i].header = cluster_indices[i].header;
#endif
result.cluster_indices[i].indices = cluster_indices[i].indices;
}
result_pub_.publish(result);
jsk_recognition_msgs::Int32Stamped::Ptr cluster_num_msg (new jsk_recognition_msgs::Int32Stamped);
cluster_num_msg->header = input->header;
cluster_num_msg->data = cluster_indices.size();
cluster_num_pub_.publish(cluster_num_msg);
diagnostic_updater_->update();
}
// call Euclidean Cluster Extraction (ref: http://www.pointclouds.org/documentation/tutorials/cluster_extraction.php)
bool clusterize(ClusterSegmentation::Request &req, ClusterSegmentation::Response &res){
// get data and convert in useful format considering also default
PCLCloudPtr cloud = pcm::PCManager::cloudForRosMsg( req.cloud);
double tolerance, minClusterSizeRate, maxClusterSizeRate;
int minInputSize;
nh_ptr->param(srvm::PARAM_NAME_CLUSTER_TOLERANCE,
tolerance, CLUSTER_TOLERANCE_DEFAULT);
nh_ptr->param(srvm::PARAM_NAME_CLUSTER_MIN_RATE,
minClusterSizeRate, CLUSTER_MIN_RATE_DEFAULT);
nh_ptr->param(srvm::PARAM_NAME_CLUSTER_MAX_RATE,
maxClusterSizeRate, CLUSTER_MAX_RATE_DEFAULT);
nh_ptr->param(srvm::PARAM_NAME_CLUSTER_TOLERANCE,
minInputSize, CLUSTER_MIN_INPUT_SIZE_DEFAULT);
if( cloud->points.size() >= minInputSize){ // skip if input cloud is too small
// Creating the KdTree object for the search method of the extraction
search::KdTree< PointXYZ>::Ptr tree (new search::KdTree< PointXYZ>);
tree->setInputCloud ( cloud);
// compute clusters
vector< PointIndices> cluster_indices;
EuclideanClusterExtraction< PointXYZ> ec;
ec.setClusterTolerance(tolerance); // in meters
ec.setMinClusterSize( round(cloud->points.size () * minClusterSizeRate)); // percentage
ec.setMaxClusterSize( round(cloud->points.size () * maxClusterSizeRate));
//ROS_ERROR( "%d %f %f", cloud->points.size(), cloud->points.size () * minClusterSizeRate, cloud->points.size () * maxClusterSizeRate);
ec.setSearchMethod( tree);
ec.setInputCloud( cloud);
ec.extract( cluster_indices);
// for all the cluster
for ( vector< pcl::PointIndices>::const_iterator it = cluster_indices.begin (); it != cluster_indices.end (); ++it){
// build inliers output
vector< int>* inlier (new std::vector< int>);
PCLCloudPtr cloudExtraxted (new PCLCloud);
// build centroid output
float xSumm = 0, ySumm = 0, zSumm = 0;
int cnt = 1;
// for all the point of a cluster (create a new point cloud for every clusters)
for ( vector<int>::const_iterator pit = it->indices.begin (); pit != it->indices.end (); pit++){
// set inlier
inlier->push_back( *pit);
// set a point of the cloud
PointXYZ* p ( new PointXYZ( cloud->points[*pit].x, cloud->points[*pit].y, cloud->points[*pit].z));
cloudExtraxted->push_back( *p);
// save summ to compute avarage
xSumm += cloud->points[*pit].x;
ySumm += cloud->points[*pit].y;
zSumm += cloud->points[*pit].z;
cnt++;
}
// create returning object
InliersCluster* cluster ( new InliersCluster);
cluster->inliers = *inlier;
cluster->cloud = PCManager::cloudToRosMsg( cloudExtraxted);
// compute and assign to output the cluster centroid
cluster->x_centroid = xSumm / cnt;
cluster->y_centroid = ySumm / cnt;
cluster->z_centroid = zSumm / cnt;
// add to returning values
res.cluster_objs.push_back( *cluster);
}
}
return true;
}
int main(int argc, char** argv)
{
if (argc < 2)
{
cout << "Input a PCD file name...\n";
return 0;
}
PointCloud<PointXYZ>::Ptr cloud(new PointCloud<PointXYZ>), cloud_f(new PointCloud<PointXYZ>);
PCDReader reader;
reader.read(argv[1], *cloud);
cout << "PointCloud before filtering has: " << cloud->points.size() << " data points.\n";
PointCloud<PointXYZ>::Ptr cloud_filtered(new PointCloud<PointXYZ>);
VoxelGrid<PointXYZ> vg;
vg.setInputCloud(cloud);
vg.setLeafSize(0.01f, 0.01f, 0.01f);
vg.filter(*cloud_filtered);
cout << "PointCloud after filtering has: " << cloud_filtered->points.size() << " data points.\n";
SACSegmentation<PointXYZ> seg;
PointIndices::Ptr inliers(new PointIndices);
PointCloud<PointXYZ>::Ptr cloud_plane(new PointCloud<PointXYZ>);
ModelCoefficients::Ptr coefficients(new ModelCoefficients);
seg.setOptimizeCoefficients(true);
seg.setModelType(SACMODEL_PLANE);
seg.setMethodType(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)
{
cout << "Coud not estimate a planar model for the given dataset.\n";
break;
}
ExtractIndices<PointXYZ> extract;
extract.setInputCloud(cloud_filtered);
extract.setIndices(inliers);
extract.setNegative(false);
extract.filter(*cloud_plane);
cout << "PointCloud representing the planar component has: " << cloud_filtered->points.size() << " data points.\n";
extract.setNegative(true);
extract.filter(*cloud_f);
cloud_filtered->swap(*cloud_f);
}
search::KdTree<PointXYZ>::Ptr kdtree(new search::KdTree<PointXYZ>);
kdtree->setInputCloud(cloud_filtered);
vector<PointIndices> cluster_indices;
EuclideanClusterExtraction<PointXYZ> ece;
ece.setClusterTolerance(0.02);
ece.setMinClusterSize(100);
ece.setMaxClusterSize(25000);
ece.setSearchMethod(kdtree);
ece.setInputCloud(cloud_filtered);
ece.extract(cluster_indices);
PCDWriter writer;
int j = 0;
for (vector<PointIndices>::const_iterator it=cluster_indices.begin(); it != cluster_indices.end(); ++it)
{
PointCloud<PointXYZ>::Ptr cluster_cloud(new PointCloud<PointXYZ>);
for (vector<int>::const_iterator pit=it->indices.begin(); pit != it->indices.end(); ++pit)
cluster_cloud->points.push_back(cloud_filtered->points[*pit]);
cluster_cloud->width = cluster_cloud->points.size();
cluster_cloud->height = 1;
cluster_cloud->is_dense = true;
cout << "PointCloud representing a cluster has: " << cluster_cloud->points.size() << " data points.\n";
stringstream ss;
ss << "cloud_cluster_" << j << ".pcd";
writer.write<PointXYZ>(ss.str(), *cluster_cloud, false);
j++;
}
return 0;
}
