/** \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;
}
}
}
foreach (const CloudComposerItem* input_item, input_data)
{
QList <CloudComposerItem*> normals_list = input_item->getChildren (CloudComposerItem::NORMALS_ITEM);
//Get the normals cloud, we just use the first normals that were found if there are more than one
pcl::PointCloud<pcl::Normal>::ConstPtr input_normals = normals_list.value(0)->data(ItemDataRole::CLOUD_TEMPLATED).value <pcl::PointCloud<pcl::Normal>::ConstPtr> ();
QVariant variant = input_item->data (ItemDataRole::CLOUD_TEMPLATED);
typename PointCloud <PointT>::Ptr input_cloud = variant.value <typename PointCloud<PointT>::Ptr> ();
pcl::OrganizedMultiPlaneSegmentation<PointT, pcl::Normal, pcl::Label> mps;
mps.setMinInliers (min_inliers);
mps.setAngularThreshold (pcl::deg2rad (angular_threshold)); // convert to radians
mps.setDistanceThreshold (distance_threshold);
mps.setInputNormals (input_normals);
mps.setInputCloud (input_cloud);
std::vector<pcl::PlanarRegion<PointT>, Eigen::aligned_allocator<pcl::PlanarRegion<PointT> > > regions;
std::vector<pcl::ModelCoefficients> model_coefficients;
std::vector<pcl::PointIndices> inlier_indices;
pcl::PointCloud<pcl::Label>::Ptr labels (new pcl::PointCloud<pcl::Label>);
std::vector<pcl::PointIndices> label_indices;
std::vector<pcl::PointIndices> boundary_indices;
mps.segmentAndRefine (regions, model_coefficients, inlier_indices, labels, label_indices, boundary_indices);
std::vector<bool> plane_labels;
plane_labels.resize (label_indices.size (), false);
for (size_t i = 0; i < label_indices.size (); i++)
{
if (label_indices[i].indices.size () > min_plane_size)
{
plane_labels[i] = true;
}
}
typename PointCloud<PointT>::CloudVectorType clusters;
typename EuclideanClusterComparator<PointT, pcl::Normal, pcl::Label>::Ptr euclidean_cluster_comparator = boost::make_shared <EuclideanClusterComparator<PointT, pcl::Normal, pcl::Label> > ();
euclidean_cluster_comparator->setInputCloud (input_cloud);
euclidean_cluster_comparator->setLabels (labels);
euclidean_cluster_comparator->setExcludeLabels (plane_labels);
euclidean_cluster_comparator->setDistanceThreshold (cluster_distance_threshold, false);
pcl::PointCloud<pcl::Label> euclidean_labels;
std::vector<pcl::PointIndices> euclidean_label_indices;
pcl::OrganizedConnectedComponentSegmentation<PointT,pcl::Label> euclidean_segmentation (euclidean_cluster_comparator);
euclidean_segmentation.setInputCloud (input_cloud);
euclidean_segmentation.segment (euclidean_labels, euclidean_label_indices);
pcl::IndicesPtr extracted_indices (new std::vector<int> ());
for (size_t i = 0; i < euclidean_label_indices.size (); i++)
{
if (euclidean_label_indices[i].indices.size () >= min_cluster_size)
{
typename PointCloud<PointT>::Ptr cluster = boost::make_shared <PointCloud<PointT> >();
pcl::copyPointCloud (*input_cloud,euclidean_label_indices[i].indices,*cluster);
qDebug () << "Found cluster with size " << cluster->width;
QString name = input_item->text () + tr ("- Clstr %1").arg (i);
CloudItem* new_cloud_item = CloudItem::createCloudItemFromTemplate<PointT>(name,cluster);
output.append (new_cloud_item);
extracted_indices->insert (extracted_indices->end (), euclidean_label_indices[i].indices.begin (), euclidean_label_indices[i].indices.end ());
}
}
for (size_t i = 0; i < label_indices.size (); i++)
{
if (label_indices[i].indices.size () >= min_plane_size)
{
typename PointCloud<PointT>::Ptr plane = boost::make_shared <PointCloud<PointT> >();
pcl::copyPointCloud (*input_cloud,label_indices[i].indices,*plane);
qDebug () << "Found plane with size " << plane->width;
QString name = input_item->text () + tr ("- Plane %1").arg (i);
CloudItem* new_cloud_item = CloudItem::createCloudItemFromTemplate<PointT>(name,plane);
output.append (new_cloud_item);
extracted_indices->insert (extracted_indices->end (), label_indices[i].indices.begin (), label_indices[i].indices.end ());
}
}
typename PointCloud<PointT>::Ptr leftovers = boost::make_shared <PointCloud<PointT> >();
if (extracted_indices->size () == 0)
pcl::copyPointCloud (*input_cloud,*leftovers);
else
{
pcl::ExtractIndices<PointT> filter;
filter.setInputCloud (input_cloud);
filter.setIndices (extracted_indices);
filter.setNegative (true);
filter.filter (*leftovers);
}
CloudItem* leftover_cloud_item = CloudItem::createCloudItemFromTemplate<PointT>(input_item->text(),leftovers);
output.append (leftover_cloud_item);
}
