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
determinePlaneNormal (PointCloud<PointXYZ>::Ptr& p, Eigen::Vector3f& normal)
{
SACSegmentation<PointXYZ> seg;
seg.setOptimizeCoefficients (true);
seg.setModelType (SACMODEL_PLANE);
seg.setMethodType (SAC_RANSAC);
seg.setMaxIterations (50);
seg.setDistanceThreshold (0.01);
seg.setInputCloud (p);
ModelCoefficients coefficients_plane;
pcl::PointIndices inliers_plane;
seg.segment (inliers_plane, coefficients_plane);
normal (0) = coefficients_plane.values[0];
normal (1) = coefficients_plane.values[1];
normal (2) = coefficients_plane.values[2];
}
PointCloud<PointXYZ>::Ptr PCLTools::segmentPlanar(PointCloud<PointXYZ>::Ptr cloud, bool negative) {
// Create the segmentation object for the planar model and set all the parameters
PointCloud<PointXYZ>::Ptr cloud_f(new PointCloud<PointXYZ>);
SACSegmentation<PointXYZ> seg;
PointIndices::Ptr inliers(new PointIndices);
ModelCoefficients::Ptr coefficients(new ModelCoefficients);
PointCloud<PointXYZ>::Ptr cloud_plane(new PointCloud<PointXYZ> ());
seg.setOptimizeCoefficients(true);
seg.setModelType(pcl::SACMODEL_PLANE);
seg.setMethodType(pcl::SAC_RANSAC);
seg.setMaxIterations(100);
seg.setDistanceThreshold(0.02);
// Segment the largest planar component from the remaining cloud
seg.setInputCloud(cloud);
seg.segment(*inliers, *coefficients);
if (inliers->indices.size () == 0) {
cerr << "Could not estimate a planar model for the given dataset." << endl;
return cloud_f;
}
// Extract the planar inliers from the input cloud
ExtractIndices<PointXYZ> extract;
extract.setInputCloud(cloud);
extract.setIndices(inliers);
extract.setNegative(false);
// Get the points associated with the planar surface
extract.filter(*cloud_plane);
// Remove the planar inliers, extract the rest
extract.setNegative(negative);
extract.filter(*cloud_f);
return cloud_f;
}
TEST (SACSegmentation, Segmentation)
{
ModelCoefficients::Ptr sphere_coefficients (new ModelCoefficients);
PointIndices::Ptr inliers (new PointIndices);
SACSegmentation<PointXYZ> seg;
seg.setOptimizeCoefficients (true);
seg.setModelType (SACMODEL_SPHERE);
seg.setMethodType (SAC_RANSAC);
seg.setMaxIterations (10000);
seg.setDistanceThreshold (0.01);
seg.setRadiusLimits (0.03, 0.07); // true radius is 0.05
seg.setInputCloud (cloud_);
seg.segment (*inliers, *sphere_coefficients);
EXPECT_NEAR (sphere_coefficients->values[0], 0.998776, 1e-2);
EXPECT_NEAR (sphere_coefficients->values[1], 0.752023, 1e-2);
EXPECT_NEAR (sphere_coefficients->values[2], 1.24558, 1e-2);
EXPECT_NEAR (sphere_coefficients->values[3], 0.0536238, 1e-2);
EXPECT_NEAR (static_cast<int> (inliers->indices.size ()), 3516, 10);
}
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);
}
}
/**
* This function tries to find all planes in a point cloud by applying
* a model-based RANSAC algorithm.
*
* Heavily inspired by
* http://www.pointclouds.org/documentation/tutorials/extract_indices.php
*/
void filterPlanes(
TheiaCloudPtr inCloud,
TheiaCloudPtr outPlanes,
TheiaCloudPtr outObjects
){
TheiaCloudPtr workingCloudPtr(new TheiaCloud(*inCloud));
size_t origCloudSize = workingCloudPtr->points.size();
size_t minPlanePoints = config.minPercentage * origCloudSize;
if(origCloudSize < 3){
ROS_INFO("Input cloud for findPlanes is too small");
ROS_INFO("Check cropping and rescaling parameters");
return;
}
TheiaCloudPtr allPlanesCloudPtr(new TheiaCloud());
PointIndices::Ptr inliers(new PointIndices());
ModelCoefficients::Ptr coefficients(new ModelCoefficients());
// prepare plane segmentation
SACSegmentation<TheiaPoint> seg;
seg.setModelType(SACMODEL_PLANE);
seg.setMethodType(SAC_RANSAC);
seg.setMaxIterations(config.numbIterations);
seg.setDistanceThreshold(config.planeDistThresh);
seg.setOptimizeCoefficients(config.planeOptimize);
// prepare extractor
ExtractIndices<TheiaPoint> extract;
while(workingCloudPtr->points.size() >= 4){
seg.setInputCloud(workingCloudPtr);
seg.segment(*inliers, *coefficients);
/**
* Stop the search for planes when the found plane
* is too small (in terms of points).
*/
size_t numbInliers = inliers->indices.size();
if(!numbInliers) break;
if(numbInliers < minPlanePoints) break;
// extract plane points
TheiaCloudPtr planeCloudPtr(new TheiaCloud());
extract.setInputCloud(workingCloudPtr);
extract.setIndices(inliers);
extract.setNegative(false);
extract.filter(*planeCloudPtr);
*allPlanesCloudPtr += *planeCloudPtr;
/**
* Remove plane from input cloud
*/
TheiaCloudPtr remainingCloudPtr(new TheiaCloud());
extract.setNegative(true);
extract.filter(*remainingCloudPtr);
/**
* Continue search
* But only use remaining point cloud
*/
workingCloudPtr->swap(*remainingCloudPtr);
}
*outPlanes = TheiaCloud(*allPlanesCloudPtr);
*outObjects = TheiaCloud(*workingCloudPtr);
}
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;
}