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();
}
/** \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
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);
}
}