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;
}
/* Compute the corner points of the biggest planar in the area (based on depth image)
* Input : Depth image (CV_16U), and rgb image, which is used for display
* Output : vector of planes, where each plane is represented by 4 Corner points on the planar surface
*/
vector<vector<PointXYZI> > PlanarExtractor::getPlanar(Mat &depth, Mat rgb) {
vector<vector<PointXYZI> > allCorners;
PointCloud<PointXYZI>::Ptr cloud_inliers(new PointCloud<PointXYZI>);
PointCloud<PointXYZI>::Ptr cloud_projection(new PointCloud<PointXYZI>);
PointIndices::Ptr inliers(new PointIndices);
ModelCoefficients::Ptr coefficients(new ModelCoefficients);
vector<Point> positions;
// *Extract Point cloud from depth image*
PointCloud<PointXYZI>::Ptr cloud = getPointCloud(depth, positions);
for (int plannr = 1;; plannr++) {
// *Compute the planar*
#if USE_NORMAL_PLANE
PointCloud<Normal>::Ptr normals_out(new PointCloud<Normal>);
NormalEstimationOMP<PointXYZI, Normal> norm_est;
norm_est.setSearchMethod(search::KdTree<PointXYZI>::Ptr(new search::KdTree<PointXYZI>));
norm_est.setRadiusSearch(radius);
norm_est.setInputCloud(cloud);
norm_est.compute(*normals_out);
SACSegmentationFromNormals<PointXYZI, Normal> segNormal;
segNormal.setOptimizeCoefficients(true);
segNormal.setModelType(SACMODEL_NORMAL_PLANE);
segNormal.setNormalDistanceWeight(distanceWeight);
segNormal.setMethodType(SAC_RANSAC);
segNormal.setMaxIterations(maxIterations);
segNormal.setDistanceThreshold(distanceThreshold);
segNormal.setInputCloud(cloud);
segNormal.setInputNormals(normals_out);
segNormal.segment(*inliers, *coefficients);
#else
SACSegmentation<PointXYZI> seg;
seg.setOptimizeCoefficients(true);
seg.setModelType(SACMODEL_PLANE);
seg.setMethodType(SAC_RANSAC);
seg.setDistanceThreshold(params.distanceThreshold); // WARNING : non normalized kinect input data, possible max: 4096 (needs verfification), how does this threshold compare to other devices???
seg.setInputCloud(cloud);
seg.segment(*inliers, *coefficients);
#endif
ExtractIndices<PointXYZI> extract_inliers;
extract_inliers.setInputCloud(cloud);
extract_inliers.setIndices(inliers);
extract_inliers.setNegative(false);
extract_inliers.filter(*cloud_inliers);
const size_t numInliers = inliers->indices.size();
if (numInliers < MIN_INLIERS) break;
// *Place inlier overlay over rgb image*
const int channel = plannr % 3;
for (int i = 0, iend = numInliers; i < iend; i++) {
Point &p = positions[inliers->indices[i]];
for (int x = p.x, xend = min(int(p.x + stride), rgb.cols - 1); x < xend; x++) {
for (int y = p.y, yend = min(int(p.y + stride), rgb.rows - 1); y < yend; y++) {
Vec3b &col = rgb.at<Vec3b>(y, x);
col[channel] = (col[channel] + 255) / 2;
}
}
}
float a = coefficients->values[0];
float b = coefficients->values[1];
float c = coefficients->values[2];
Eigen::Vector3f x_axis(b / sqrt(a * a + b * b), -a / sqrt(a * a + b * b), 0);
Eigen::Vector3f y_direction(a, b, c);
Eigen::Affine3f rotation = getTransFromUnitVectorsXY(x_axis, y_direction);
for (int x = 0; x < 3; x++) // The rotation actually flattens the XZ axis, instead of XY, since using XY variables is much more convienient, swap the rotation of the Y and Z axis around
swap(rotation(1, x), rotation(2, x));
PointCloud<PointXYZI>::Ptr rotated(new PointCloud<PointXYZI>);
transformPointCloud(*cloud_inliers, *rotated, rotation);
#if 0
// viewing purposes:: view rotated inlier plane
for (int i = 0; i < 10; i++) {
cout << rotated->points[i] << endl;
}
visualization::CloudViewer viewer("Simple Cloud Viewer");
viewer.showCloud(rotated);
while (!viewer.wasStopped()) sleep(500);
#endif
vector<PointXYZI> rect1(4); // Corner points in projection-space
vector<PointXYZI> rect2(4); // Corner points in projection-space, but rotated 45 degrees
for (int i = 0; i < 4; i++) { // Initialize corner points to the first point
rect1[i] = rotated->points[0];
rect2[i] = rotate45(rotated->points[0]);
}
// *Compute corner points of planar*
// Do estimation of rectangle, in order to really compute the corners compute for each point the point that has the LARGEST distance (farthest neighbor?). Keep track how many times each point has been listed as farthest, and create histogram.
// For estimation, make 2 rectangles, first extract the 4 extrema (top/right/bottom/left), do the same but then rotate 45 degrees. Take the rectangle with the largest surface area.
for (size_t i = 1; i < numInliers; i++) { // start from 1, since 0 is already set
PointXYZI p = rotated->points[i];
PointXYZI p45 = rotate45(p);
if (p.y > rect1[0].y) rect1[0] = p; // TOP
if (p.x > rect1[1].x) rect1[1] = p; // RIGHT
if (p.y < rect1[2].y) rect1[2] = p; // BOTTOM
if (p.x < rect1[3].x) rect1[3] = p; // LEFT
if (p45.y > rect2[0].y) rect2[0] = p45; // TOP-LEFT
if (p45.x > rect2[1].x) rect2[1] = p45; // TOP-RIGHT
if (p45.y < rect2[2].y) rect2[2] = p45; // BOTTOM-RIGHT
if (p45.x < rect2[3].x) rect2[3] = p45; // BOTTOM-LEFT
}
// Measure rectangle surface area for both suggestions, and use the largest one
double area1 = getArea(rect1);
double area2 = getArea(rect2);
vector<PointXYZI> corners = fetchFromIndex<PointXYZI>(area1 > area2 ? rect1 : rect2, cloud->points);
vector<Point> cvCorners = fetchFromIndex<Point>(corners, positions);
allCorners.push_back(corners);
// compute rectangle
Point prev = cvCorners.back();
for (int i = 0; i < 4; i++) {
Point &p = cvCorners[i];
circle(rgb, p, 5, cvScalar(0, 0, 255), -1);
line(rgb, prev, p, cvScalar(0, 0, 255), 1);
prev = p;
}
removeIndexes(cloud->points, inliers->indices);
cloud->width = cloud->points.size();
cout << "Corners: " << corners[0] << " " << corners[1] << " " << corners[2] << " " << corners[3] << std::endl;
imshow("planar", rgb);
if (waitKey() == 27) break;
}
return allCorners;
}