int convex_plane(eusfloat_t *src, int ssize,
eusfloat_t *coeff, eusfloat_t *ret) {
typedef PointXYZ Point;
PointCloud<Point>::Ptr cloud_projected (new PointCloud<Point>);
PointCloud<Point>::Ptr cloud_filtered (new PointCloud<Point>);
floatvector2pointcloud(src, ssize, 1, *cloud_filtered);
ModelCoefficients::Ptr coefficients (new ModelCoefficients);
coefficients->values.resize(4);
coefficients->values[0] = coeff[0];
coefficients->values[1] = coeff[1];
coefficients->values[2] = coeff[2];
coefficients->values[3] = coeff[3] / 1000.0;
// Project the model inliers
ProjectInliers<Point> proj;
proj.setModelType (SACMODEL_PLANE);
proj.setInputCloud (cloud_filtered);
proj.setModelCoefficients (coefficients);
proj.filter (*cloud_projected);
// Create a Concave Hull representation of the projected inliers
PointCloud<Point>::Ptr cloud_hull (new PointCloud<Point>);
ConvexHull<Point> chull;
chull.setInputCloud (cloud_projected);
//chull.setAlpha (alpha);
chull.reconstruct (*cloud_hull);
for(int i = 0; i < cloud_hull->points.size(); i++) {
*ret++ = cloud_hull->points[i].x * 1000.0;
*ret++ = cloud_hull->points[i].y * 1000.0;
*ret++ = cloud_hull->points[i].z * 1000.0;
}
return cloud_hull->points.size();
}
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);
}
}
// Returns the points that are above the floor, but not the floor itself
CloudT get_object_points(CloudT cloud)
{
// PHASE 1: DATA CLEANUP
////////////////////////////////////////////////////////
// Filter out NaNs from data (this is necessary now) ...
PassThrough<PointXYZRGB> nan_remover;
nan_remover.setInputCloud(boost::make_shared<PointCloud<PointXYZRGB> >(cloud));
nan_remover.setFilterFieldName("z");
nan_remover.setFilterLimits(0.0, 10.0);
nan_remover.filter(cloud);
// Filter out statistical outliers
StatisticalOutlierRemoval<PointXYZRGB> statistical_filter;
statistical_filter.setInputCloud(boost::make_shared<PointCloud<PointXYZRGB> >(cloud));
statistical_filter.setMeanK(50);
statistical_filter.setStddevMulThresh(1.0);
statistical_filter.filter(cloud);
// Downsample to 1cm Voxel Grid
pcl::VoxelGrid<PointT> downsampler;
downsampler.setInputCloud(boost::make_shared<CloudT>(cloud));
downsampler.setLeafSize(0.005, 0.005, 0.005);
downsampler.filter(cloud);
ROS_INFO("PointCloud after filtering: %d data points.", cloud.width * cloud.height);
// PHASE 2: FIND THE GROUND PLANE
/////////////////////////////////////////////////////////
// Step 3: Find the ground plane using RANSAC
ModelCoefficients ground_coefficients;
PointIndices ground_indices;
SACSegmentation<PointXYZRGB> ground_finder;
ground_finder.setOptimizeCoefficients(true);
ground_finder.setModelType(SACMODEL_PLANE);
ground_finder.setMethodType(SAC_RANSAC);
ground_finder.setDistanceThreshold(0.015);
ground_finder.setInputCloud(boost::make_shared<PointCloud<PointXYZRGB> >(cloud));
ground_finder.segment(ground_indices, ground_coefficients);
// PHASE 3: EXTRACT ONLY POINTS ABOVE THE GROUND PLANE
/////////////////////////////////////////////////////////
// Step 3a. Extract the ground plane inliers
CloudT ground_points;
ExtractIndices<PointXYZRGB> extractor;
extractor.setInputCloud(boost::make_shared<PointCloud<PointXYZRGB> >(cloud));
extractor.setIndices(boost::make_shared<PointIndices>(ground_indices));
extractor.filter(ground_points);
// Step 3b. Extract the ground plane outliers
CloudT object_points;
ExtractIndices<PointXYZRGB> outlier_extractor;
outlier_extractor.setInputCloud(boost::make_shared<PointCloud<PointXYZRGB> >(cloud));
outlier_extractor.setIndices(boost::make_shared<PointIndices>(ground_indices));
outlier_extractor.setNegative(true);
outlier_extractor.filter(object_points);
// Step 3c. Project the ground inliers
PointCloud<PointXYZRGB> cloud_projected;
ProjectInliers<PointXYZRGB> proj;
proj.setModelType(SACMODEL_PLANE);
proj.setInputCloud(boost::make_shared<PointCloud<PointXYZRGB> >(ground_points));
proj.setModelCoefficients(boost::make_shared<ModelCoefficients>(ground_coefficients));
proj.filter(cloud_projected);
// Step 3d. Create a Convex Hull representation of the projected inliers
PointCloud<PointXYZRGB> ground_hull;
ConvexHull2D<PointXYZRGB, PointXYZRGB> chull;
chull.setInputCloud(boost::make_shared<PointCloud<PointXYZRGB> >(cloud_projected));
chull.reconstruct(ground_hull);
ROS_INFO ("Convex hull has: %d data points.", (int) ground_hull.points.size ());
hull_pub.publish(ground_hull);
// Step 3e. Extract only those outliers that lie above the ground plane's convex hull
PointIndices object_indices;
ExtractPolygonalPrismData<PointT> hull_limiter;
hull_limiter.setInputCloud(boost::make_shared<CloudT>(object_points));
hull_limiter.setInputPlanarHull(boost::make_shared<CloudT>(ground_hull));
hull_limiter.setHeightLimits(0, 0.3);
hull_limiter.segment(object_indices);
ExtractIndices<PointT> object_extractor;
object_extractor.setInputCloud(boost::make_shared<CloudT>(object_points));
object_extractor.setIndices(boost::make_shared<PointIndices>(object_indices));
object_extractor.filter(object_points);
return object_points;
}