void
cloud_callback (const CloudConstPtr& cloud)
{
FPS_CALC ("cloud callback");
boost::mutex::scoped_lock lock (cloud_mutex_);
cloud_ = cloud;
search_.setInputCloud (cloud);
// Subsequent frames are segmented by "tracking" the parameters of the previous frame
// We do this by using the estimated inliers from previous frames in the current frame,
// and refining the coefficients
if (!first_frame_)
{
if (!plane_indices_ || plane_indices_->indices.empty () || !search_.getInputCloud ())
{
PCL_ERROR ("Lost tracking. Select the object again to continue.\n");
first_frame_ = true;
return;
}
SACSegmentation<PointT> seg;
seg.setOptimizeCoefficients (true);
seg.setModelType (SACMODEL_PLANE);
seg.setMethodType (SAC_RANSAC);
seg.setMaxIterations (1000);
seg.setDistanceThreshold (0.01);
seg.setInputCloud (search_.getInputCloud ());
seg.setIndices (plane_indices_);
ModelCoefficients coefficients;
PointIndices inliers;
seg.segment (inliers, coefficients);
if (inliers.indices.empty ())
{
PCL_ERROR ("No planar model found. Select the object again to continue.\n");
first_frame_ = true;
return;
}
// Visualize the object in 3D...
CloudPtr plane_inliers (new Cloud);
pcl::copyPointCloud (*search_.getInputCloud (), inliers.indices, *plane_inliers);
if (plane_inliers->points.empty ())
{
PCL_ERROR ("No planar model found. Select the object again to continue.\n");
first_frame_ = true;
return;
}
else
{
plane_.reset (new Cloud);
// Compute the convex hull of the plane
ConvexHull<PointT> chull;
chull.setDimension (2);
chull.setInputCloud (plane_inliers);
chull.reconstruct (*plane_);
}
}
}
void
segment (const PointT &picked_point,
int picked_idx,
PlanarRegion<PointT> ®ion,
typename PointCloud<PointT>::Ptr &object)
{
object.reset ();
// Segment out all planes
vector<ModelCoefficients> model_coefficients;
vector<PointIndices> inlier_indices, boundary_indices;
vector<PlanarRegion<PointT>, Eigen::aligned_allocator<PlanarRegion<PointT> > > regions;
// Prefer a faster method if the cloud is organized, over RANSAC
if (cloud_->isOrganized ())
{
print_highlight (stderr, "Estimating normals ");
TicToc tt; tt.tic ();
// Estimate normals
PointCloud<Normal>::Ptr normal_cloud (new PointCloud<Normal>);
estimateNormals (cloud_, *normal_cloud);
print_info ("[done, "); print_value ("%g", tt.toc ()); print_info (" ms : "); print_value ("%lu", normal_cloud->size ()); print_info (" points]\n");
OrganizedMultiPlaneSegmentation<PointT, Normal, Label> mps;
mps.setMinInliers (1000);
mps.setAngularThreshold (deg2rad (3.0)); // 3 degrees
mps.setDistanceThreshold (0.03); // 2 cm
mps.setMaximumCurvature (0.001); // a small curvature
mps.setProjectPoints (true);
mps.setComparator (plane_comparator_);
mps.setInputNormals (normal_cloud);
mps.setInputCloud (cloud_);
// Use one of the overloaded segmentAndRefine calls to get all the information that we want out
PointCloud<Label>::Ptr labels (new PointCloud<Label>);
vector<PointIndices> label_indices;
mps.segmentAndRefine (regions, model_coefficients, inlier_indices, labels, label_indices, boundary_indices);
}
else
{
SACSegmentation<PointT> seg;
seg.setOptimizeCoefficients (true);
seg.setModelType (SACMODEL_PLANE);
seg.setMethodType (SAC_RANSAC);
seg.setMaxIterations (10000);
seg.setDistanceThreshold (0.005);
// Copy XYZ and Normals to a new cloud
typename PointCloud<PointT>::Ptr cloud_segmented (new PointCloud<PointT> (*cloud_));
typename PointCloud<PointT>::Ptr cloud_remaining (new PointCloud<PointT>);
ModelCoefficients coefficients;
ExtractIndices<PointT> extract;
PointIndices::Ptr inliers (new PointIndices ());
// Up until 30% of the original cloud is left
int i = 1;
while (double (cloud_segmented->size ()) > 0.3 * double (cloud_->size ()))
{
seg.setInputCloud (cloud_segmented);
print_highlight (stderr, "Searching for the largest plane (%2.0d) ", i++);
TicToc tt; tt.tic ();
seg.segment (*inliers, coefficients);
print_info ("[done, "); print_value ("%g", tt.toc ()); print_info (" ms : "); print_value ("%lu", inliers->indices.size ()); print_info (" points]\n");
// No datasets could be found anymore
if (inliers->indices.empty ())
break;
// Save this plane
PlanarRegion<PointT> region;
region.setCoefficients (coefficients);
regions.push_back (region);
inlier_indices.push_back (*inliers);
model_coefficients.push_back (coefficients);
// Extract the outliers
extract.setInputCloud (cloud_segmented);
extract.setIndices (inliers);
extract.setNegative (true);
extract.filter (*cloud_remaining);
cloud_segmented.swap (cloud_remaining);
}
}
print_highlight ("Number of planar regions detected: %lu for a cloud of %lu points\n", regions.size (), cloud_->size ());
double max_dist = numeric_limits<double>::max ();
// Compute the distances from all the planar regions to the picked point, and select the closest region
int idx = -1;
for (size_t i = 0; i < regions.size (); ++i)
{
double dist = pointToPlaneDistance (picked_point, regions[i].getCoefficients ());
if (dist < max_dist)
{
max_dist = dist;
idx = static_cast<int> (i);
}
}
// Get the plane that holds the object of interest
if (idx != -1)
{
plane_indices_.reset (new PointIndices (inlier_indices[idx]));
if (cloud_->isOrganized ())
{
approximatePolygon (regions[idx], region, 0.01f, false, true);
print_highlight ("Planar region: %lu points initial, %lu points after refinement.\n", regions[idx].getContour ().size (), region.getContour ().size ());
}
else
{
// Save the current region
region = regions[idx];
print_highlight (stderr, "Obtaining the boundary points for the region ");
TicToc tt; tt.tic ();
// Project the inliers to obtain a better hull
typename PointCloud<PointT>::Ptr cloud_projected (new PointCloud<PointT>);
ModelCoefficients::Ptr coefficients (new ModelCoefficients (model_coefficients[idx]));
ProjectInliers<PointT> proj;
proj.setModelType (SACMODEL_PLANE);
proj.setInputCloud (cloud_);
proj.setIndices (plane_indices_);
proj.setModelCoefficients (coefficients);
proj.filter (*cloud_projected);
// Compute the boundary points as a ConvexHull
ConvexHull<PointT> chull;
chull.setDimension (2);
chull.setInputCloud (cloud_projected);
PointCloud<PointT> plane_hull;
chull.reconstruct (plane_hull);
region.setContour (plane_hull);
print_info ("[done, "); print_value ("%g", tt.toc ()); print_info (" ms : "); print_value ("%lu", plane_hull.size ()); print_info (" points]\n");
}
}
// Segment the object of interest
if (plane_indices_ && !plane_indices_->indices.empty ())
{
plane_.reset (new PointCloud<PointT>);
copyPointCloud (*cloud_, plane_indices_->indices, *plane_);
object.reset (new PointCloud<PointT>);
segmentObject (picked_idx, cloud_, plane_indices_, *object);
}
}
/** \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;
}
}
}
/** \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[in] plane_boundary_indices a set of indices representing the boundary of the plane
* \param[out] object the segmented resultant object
*/
void
segmentObject (int picked_idx,
const CloudConstPtr &cloud,
const PointIndices::Ptr &plane_indices,
const PointIndices::Ptr &plane_boundary_indices,
Cloud &object)
{
CloudPtr plane_hull (new Cloud);
// Compute the convex hull of the plane
ConvexHull<PointT> chull;
chull.setDimension (2);
chull.setInputCloud (cloud);
chull.setIndices (plane_boundary_indices);
chull.reconstruct (*plane_hull);
// Remove the plane indices from the data
PointIndices::Ptr everything_but_the_plane (new PointIndices);
if (indices_fullset_.size () != cloud->points.size ())
{
indices_fullset_.resize (cloud->points.size ());
for (int p_it = 0; p_it < static_cast<int> (indices_fullset_.size ()); ++p_it)
indices_fullset_[p_it] = p_it;
}
std::vector<int> indices_subset = plane_indices->indices;
std::sort (indices_subset.begin (), indices_subset.end ());
set_difference (indices_fullset_.begin (), indices_fullset_.end (),
indices_subset.begin (), indices_subset.end (),
inserter (everything_but_the_plane->indices, everything_but_the_plane->indices.begin ()));
// Extract all clusters above the hull
PointIndices::Ptr points_above_plane (new PointIndices);
ExtractPolygonalPrismData<PointT> exppd;
exppd.setInputCloud (cloud);
exppd.setInputPlanarHull (plane_hull);
exppd.setIndices (everything_but_the_plane);
exppd.setHeightLimits (0.0, 0.5); // up to half a meter
exppd.segment (*points_above_plane);
// Use an organized clustering segmentation to extract the individual clusters
EuclideanClusterComparator<PointT, Normal, Label>::Ptr euclidean_cluster_comparator (new EuclideanClusterComparator<PointT, Normal, 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 (int i = 0; i < static_cast<int> (points_above_plane->indices.size ()); ++i)
scene->points[points_above_plane->indices[i]].label = 1;
euclidean_cluster_comparator->setLabels (scene);
vector<bool> exclude_labels (2); exclude_labels[0] = true; exclude_labels[1] = false;
euclidean_cluster_comparator->setExcludeLabels (exclude_labels);
OrganizedConnectedComponentSegmentation<PointT, Label> euclidean_segmentation (euclidean_cluster_comparator);
euclidean_segmentation.setInputCloud (cloud);
PointCloud<Label> euclidean_labels;
vector<PointIndices> euclidean_label_indices;
euclidean_segmentation.segment (euclidean_labels, euclidean_label_indices);
// For each cluster found
bool cluster_found = false;
for (size_t i = 0; i < euclidean_label_indices.size (); i++)
{
if (cluster_found)
break;
// Check if the point that we picked belongs to it
for (size_t j = 0; j < euclidean_label_indices[i].indices.size (); ++j)
{
if (picked_idx != euclidean_label_indices[i].indices[j])
continue;
//pcl::PointCloud<PointT> cluster;
pcl::copyPointCloud (*cloud, euclidean_label_indices[i].indices, object);
cluster_found = true;
break;
//object_indices = euclidean_label_indices[i].indices;
//clusters.push_back (cluster);
}
}
}