/**
* Find object candidates by clustering the ramining point cloud.
* The minimum number of point forming an object candidate and
* the maximum object size is configurable.
* This helps to filter noise.
*/
int findBoxes(
TheiaCloudPtr inCloud,
std::vector<Box> & outBoxVect
){
int errorCode = 0;
size_t numbPoints = inCloud->points.size();
if(!numbPoints){
std::cout << "Error in " << __FUNCTION__ << std::endl;
std::cout << "Cloud is empty" << std::endl;
return -1;
}
EuclideanClusterExtraction<TheiaPoint> extractor;
extractor.setClusterTolerance(config.objectSize);
extractor.setMinClusterSize(config.minClusterSize);
extractor.setInputCloud(inCloud);
std::vector<PointIndices> clusterVect;
extractor.extract(clusterVect);
size_t numbClusters = clusterVect.size();
for(size_t i = 0; i < numbClusters; i++){
PointIndices & cluster = clusterVect[i];
Box box;
errorCode = clusterToBox(inCloud, cluster, box);
if(errorCode) return errorCode;
outBoxVect.push_back(box);
}
return errorCode;
}
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);
}
/** \brief Given a plane, and the set of inlier indices representing it,
* segment out the object of intererest supported by it.
*
* \param[in] picked_idx the index of a point on the object
* \param[in] cloud the full point cloud dataset
* \param[in] plane_indices a set of indices representing the plane supporting the object of interest
* \param[out] object the segmented resultant object
*/
void
segmentObject (int picked_idx,
const typename PointCloud<PointT>::ConstPtr &cloud,
const PointIndices::Ptr &plane_indices,
PointCloud<PointT> &object)
{
typename PointCloud<PointT>::Ptr plane_hull (new PointCloud<PointT>);
// Compute the convex hull of the plane
ConvexHull<PointT> chull;
chull.setDimension (2);
chull.setInputCloud (cloud);
chull.setIndices (plane_indices);
chull.reconstruct (*plane_hull);
// Remove the plane indices from the data
typename PointCloud<PointT>::Ptr plane (new PointCloud<PointT>);
ExtractIndices<PointT> extract (true);
extract.setInputCloud (cloud);
extract.setIndices (plane_indices);
extract.setNegative (false);
extract.filter (*plane);
PointIndices::Ptr indices_but_the_plane (new PointIndices);
extract.getRemovedIndices (*indices_but_the_plane);
// Extract all clusters above the hull
PointIndices::Ptr points_above_plane (new PointIndices);
ExtractPolygonalPrismData<PointT> exppd;
exppd.setInputCloud (cloud);
exppd.setIndices (indices_but_the_plane);
exppd.setInputPlanarHull (plane_hull);
exppd.setViewPoint (cloud->points[picked_idx].x, cloud->points[picked_idx].y, cloud->points[picked_idx].z);
exppd.setHeightLimits (0.001, 0.5); // up to half a meter
exppd.segment (*points_above_plane);
vector<PointIndices> euclidean_label_indices;
// Prefer a faster method if the cloud is organized, over EuclidanClusterExtraction
if (cloud_->isOrganized ())
{
// Use an organized clustering segmentation to extract the individual clusters
typename EuclideanClusterComparator<PointT, Label>::Ptr euclidean_cluster_comparator (new EuclideanClusterComparator<PointT, Label>);
euclidean_cluster_comparator->setInputCloud (cloud);
euclidean_cluster_comparator->setDistanceThreshold (0.03f, false);
// Set the entire scene to false, and the inliers of the objects located on top of the plane to true
Label l; l.label = 0;
PointCloud<Label>::Ptr scene (new PointCloud<Label> (cloud->width, cloud->height, l));
// Mask the objects that we want to split into clusters
for (const int &index : points_above_plane->indices)
scene->points[index].label = 1;
euclidean_cluster_comparator->setLabels (scene);
boost::shared_ptr<std::set<uint32_t> > exclude_labels = boost::make_shared<std::set<uint32_t> > ();
exclude_labels->insert (0);
euclidean_cluster_comparator->setExcludeLabels (exclude_labels);
OrganizedConnectedComponentSegmentation<PointT, Label> euclidean_segmentation (euclidean_cluster_comparator);
euclidean_segmentation.setInputCloud (cloud);
PointCloud<Label> euclidean_labels;
euclidean_segmentation.segment (euclidean_labels, euclidean_label_indices);
}
else
{
print_highlight (stderr, "Extracting individual clusters from the points above the reference plane ");
TicToc tt; tt.tic ();
EuclideanClusterExtraction<PointT> ec;
ec.setClusterTolerance (0.02); // 2cm
ec.setMinClusterSize (100);
ec.setSearchMethod (search_);
ec.setInputCloud (cloud);
ec.setIndices (points_above_plane);
ec.extract (euclidean_label_indices);
print_info ("[done, "); print_value ("%g", tt.toc ()); print_info (" ms : "); print_value ("%lu", euclidean_label_indices.size ()); print_info (" clusters]\n");
}
// For each cluster found
bool cluster_found = false;
for (const auto &euclidean_label_index : euclidean_label_indices)
{
if (cluster_found)
break;
// Check if the point that we picked belongs to it
for (size_t j = 0; j < euclidean_label_index.indices.size (); ++j)
{
if (picked_idx != euclidean_label_index.indices[j])
continue;
copyPointCloud (*cloud, euclidean_label_index.indices, object);
cluster_found = true;
break;
}
}
}
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();
}
void segment(PointCloud<PointXYZRGB> cloud)
{
CloudT object_cloud = get_object_points(cloud);
objects_pub.publish(object_cloud);
// Estimate point normals
// PointCloud<Normal> cloud_normals;
// KdTreeANN<PointXYZRGB>::Ptr tree = boost::make_shared<KdTreeANN<PointXYZRGB> >();
// NormalEstimation<PointXYZRGB, Normal> ne;
// ne.setSearchMethod(tree);
// ne.setInputCloud(boost::make_shared<PointCloud<PointXYZRGB> >(object_cloud));
// ne.setKSearch(50);
// ne.compute(cloud_normals);
// Extract clusters of points (i.e., objects)
EuclideanClusterExtraction<PointXYZRGB> clustering;
KdTreeANN<PointXYZRGB>::Ptr cluster_tree = boost::make_shared<KdTreeANN<PointXYZRGB> >();
vector<PointIndices> clusters;
clustering.setInputCloud(boost::make_shared<PointCloud<PointXYZRGB> >(object_cloud));
clustering.setClusterTolerance(0.05); // ?
clustering.setMinClusterSize(300);
clustering.setSearchMethod(cluster_tree); // Not sure if this should be ANN, or FLANN, or if it matters
clustering.extract(clusters);
// cout << (cloud_normals.height * cloud_normals.width) << " " << (object_cloud.height * object_cloud.width) << endl;
cout << "FOUND " << clusters.size() << " OBJECTS" << endl;
for (uint i = 0; i < clusters.size(); i++)
{
cout << "CLUSTER " << i << " has " << clusters[i].indices.size() << " points" << endl;
PointCloud<PointXYZRGB> cluster_points;
ExtractIndices<PointXYZRGB> extract_cluster;
extract_cluster.setInputCloud(boost::make_shared<PointCloud<PointXYZRGB> >(object_cloud));
extract_cluster.setIndices(boost::make_shared<PointIndices>(clusters[i]));
extract_cluster.filter(cluster_points);
cluster_pub.publish(cluster_points);
// Estimate point normals
PointCloud<Normal> cluster_normals;
KdTreeANN<PointT>::Ptr tree = boost::make_shared<KdTreeANN<PointT> >();
// Estimate point normals
NormalEstimation<PointT, Normal> ne;
ne.setSearchMethod(tree);
ne.setInputCloud(boost::make_shared<pcl::PointCloud<PointT> >(cluster_points));
ne.setKSearch(10);
ne.compute(cluster_normals);
// NormalEstimation<PointT, Normal> ne;
// ne.setSearchMethod(tree);
// ne.setInputCloud(boost::make_shared<CloudT>(cluster_points));
// ne.setKSearch(50);
// ne.compute(cluster_normals);
// cout << cluster_points.height * cluster_points.width << " " << cluster_normals.height * cluster_normals.width
// << endl;
// Obtain the plane inliers and coefficients
ModelCoefficients coefficients_plane;
PointIndices inliers_plane;
SACSegmentationFromNormals<PointT, Normal> seg_plane;
seg_plane.setOptimizeCoefficients(true);
seg_plane.setMethodType(SAC_RANSAC);
seg_plane.setModelType(SACMODEL_NORMAL_PLANE);
seg_plane.setDistanceThreshold(0.05);
seg_plane.setInputCloud(boost::make_shared<CloudT>(cluster_points));
seg_plane.setInputNormals(boost::make_shared<PointCloud<Normal> >(cluster_normals));
seg_plane.segment(inliers_plane, coefficients_plane);
// cout << "Plane coefficients: " << coefficients_plane << endl;
cout << "PLANE INLIERS: " << inliers_plane.indices.size() << endl;
// Obtain the Sphere inliers and coefficients
ModelCoefficients coefficients_sphere;
PointIndices inliers_sphere;
SACSegmentation<PointXYZRGB> seg_sphere;
seg_sphere.setOptimizeCoefficients(true);
seg_sphere.setMethodType(SAC_RANSAC);
seg_sphere.setModelType(SACMODEL_SPHERE);
seg_sphere.setRadiusLimits(0.01, 0.1);
seg_sphere.setDistanceThreshold(0.005);
seg_sphere.setInputCloud(boost::make_shared<CloudT>(cluster_points));
seg_sphere.segment(inliers_sphere, coefficients_sphere);
// cout << "Sphere coefficients: " << coefficients_sphere << endl;
cout << "SPHERE INLIERS: " << inliers_sphere.indices.size() << endl;
ModelCoefficients coefficients_cylinder;
PointIndices inliers_cylinder;
SACSegmentationFromNormals<PointT, Normal> seg_cylinder;
seg_cylinder.setOptimizeCoefficients(true);
seg_cylinder.setModelType(SACMODEL_CYLINDER);
seg_cylinder.setMethodType(SAC_RANSAC);
seg_cylinder.setNormalDistanceWeight(0.1);
seg_cylinder.setMaxIterations(1000);
seg_cylinder.setDistanceThreshold(0.05);
seg_cylinder.setRadiusLimits(0.01, 0.1);
seg_cylinder.setInputCloud(boost::make_shared<CloudT>(cluster_points));
seg_cylinder.setInputNormals(boost::make_shared<PointCloud<Normal> >(cluster_normals));
seg_cylinder.segment(inliers_cylinder, coefficients_cylinder);
cout << "CYLINDER INLIERS: " << inliers_cylinder.indices.size() << endl;
cout << "Cylinder coefficients: " << coefficients_cylinder << endl;
cout << endl;
if (!inliers_sphere.indices.empty())
{
visualization_msgs::Marker marker;
marker.header.frame_id = coefficients_sphere.header.frame_id;
marker.header.stamp = ros::Time::now();
marker.ns = "basic_shapes" + boost::lexical_cast<string>(i);
marker.id = 0;
marker.type = visualization_msgs::Marker::SPHERE;
marker.action = visualization_msgs::Marker::ADD;
marker.pose.position.x = coefficients_sphere.values[0];
marker.pose.position.y = coefficients_sphere.values[1];
marker.pose.position.z = coefficients_sphere.values[2];
marker.pose.orientation.x = 0.0;
marker.pose.orientation.y = 0.0;
marker.pose.orientation.z = 0.0;
marker.pose.orientation.w = 1.0;
marker.scale.x = coefficients_sphere.values[3] * 2;
marker.scale.y = coefficients_sphere.values[3] * 2;
marker.scale.z = coefficients_sphere.values[3] * 2;
marker.color.r = 0.0f;
marker.color.g = 1.0f;
marker.color.b = 0.0f;
marker.color.a = 1.0;
marker.lifetime = ros::Duration();
marker_pub.publish(marker);
}
if (!inliers_cylinder.indices.empty())
{
// TODO: The cylinder coefficients are not as straightforward, need to do some
// math to pull out the location and angle (from axis orientation and point on axis)
// visualization_msgs::Marker marker;
// marker.header.frame_id = coefficients_sphere.header.frame_id;
// marker.header.stamp = ros::Time::now();
// marker.ns = "basic_shapes" + boost::lexical_cast<string>(i);
// marker.id = 0;
// marker.type = visualization_msgs::Marker::SPHERE;
// marker.action = visualization_msgs::Marker::ADD;
// marker.pose.position.x = coefficients_sphere.values[0];
// marker.pose.position.y = coefficients_sphere.values[1];
// marker.pose.position.z = coefficients_sphere.values[2];
// marker.pose.orientation.x = 0.0;
// marker.pose.orientation.y = 0.0;
// marker.pose.orientation.z = 0.0;
// marker.pose.orientation.w = 1.0;
// marker.scale.x = coefficients_sphere.values[3] * 2;
// marker.scale.y = coefficients_sphere.values[3] * 2;
// marker.scale.z = coefficients_sphere.values[3] * 2;
// marker.color.r = 0.0f;
// marker.color.g = 1.0f;
// marker.color.b = 0.0f;
// marker.color.a = 1.0;
// marker.lifetime = ros::Duration();
// marker_pub.publish(marker);
}
}
}
void
compute (const sensor_msgs::PointCloud2::ConstPtr &input, sensor_msgs::PointCloud2 &output,
int max_iterations = 1000, double threshold = 0.05, bool negative = false)
{
// Convert data to PointCloud<T>
PointCloud<PointXYZ>::Ptr xyz (new PointCloud<PointXYZ>);
fromROSMsg (*input, *xyz);
// Estimate
TicToc tt;
print_highlight (stderr, "Computing ");
tt.tic ();
// Refine the plane indices
typedef SampleConsensusModelPlane<PointXYZ>::Ptr SampleConsensusModelPlanePtr;
SampleConsensusModelPlanePtr model (new SampleConsensusModelPlane<PointXYZ> (xyz));
RandomSampleConsensus<PointXYZ> sac (model, threshold);
sac.setMaxIterations (max_iterations);
bool res = sac.computeModel ();
vector<int> inliers;
sac.getInliers (inliers);
Eigen::VectorXf coefficients;
sac.getModelCoefficients (coefficients);
if (!res || inliers.empty ())
{
PCL_ERROR ("No planar model found. Relax thresholds and continue.\n");
return;
}
sac.refineModel (2, 50);
sac.getInliers (inliers);
sac.getModelCoefficients (coefficients);
print_info ("[done, "); print_value ("%g", tt.toc ()); print_info (" ms, plane has : "); print_value ("%zu", inliers.size ()); print_info (" points]\n");
print_info ("Model coefficients: [");
print_value ("%g %g %g %g", coefficients[0], coefficients[1], coefficients[2], coefficients[3]); print_info ("]\n");
// Instead of returning the planar model as a set of inliers, return the outliers, but perform a cluster segmentation first
if (negative)
{
// Remove the plane indices from the data
PointIndices::Ptr everything_but_the_plane (new PointIndices);
std::vector<int> indices_fullset (xyz->size ());
for (int p_it = 0; p_it < static_cast<int> (indices_fullset.size ()); ++p_it)
indices_fullset[p_it] = p_it;
std::sort (inliers.begin (), inliers.end ());
set_difference (indices_fullset.begin (), indices_fullset.end (),
inliers.begin (), inliers.end (),
inserter (everything_but_the_plane->indices, everything_but_the_plane->indices.begin ()));
// Extract largest cluster minus the plane
vector<PointIndices> cluster_indices;
EuclideanClusterExtraction<PointXYZ> ec;
ec.setClusterTolerance (0.02); // 2cm
ec.setMinClusterSize (100);
ec.setInputCloud (xyz);
ec.setIndices (everything_but_the_plane);
ec.extract (cluster_indices);
// Convert data back
copyPointCloud (*input, cluster_indices[0].indices, output);
}
else
{
// Convert data back
PointCloud<PointXYZ> output_inliers;
copyPointCloud (*input, inliers, output);
}
}
// 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;
}