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 Point picking callback. This gets called when the user selects
* a 3D point on screen (in the PCLVisualizer window) using Shift+click.
*
* \param[in] event the event that triggered the call
*/
void
pp_callback (const visualization::PointPickingEvent& event, void*)
{
// Check to see if we got a valid point. Early exit.
int idx = event.getPointIndex ();
if (idx == -1)
return;
vector<int> indices (1);
vector<float> distances (1);
// Get the point that was picked
PointT picked_pt;
event.getPoint (picked_pt.x, picked_pt.y, picked_pt.z);
print_info (stderr, "Picked point with index %d, and coordinates %f, %f, %f.\n", idx, picked_pt.x, picked_pt.y, picked_pt.z);
// Add a sphere to it in the PCLVisualizer window
stringstream ss;
ss << "sphere_" << idx;
cloud_viewer_->addSphere (picked_pt, 0.01, 1.0, 0.0, 0.0, ss.str ());
// Because VTK/OpenGL stores data without NaN, we lose the 1-1 correspondence, so we must search for the real point
search_->nearestKSearch (picked_pt, 1, indices, distances);
// Add some marker to the image
if (image_viewer_)
{
// Get the [u, v] in pixel coordinates for the ImageViewer. Remember that 0,0 is bottom left.
uint32_t width = search_->getInputCloud ()->width,
height = search_->getInputCloud ()->height;
int v = height - indices[0] / width,
u = indices[0] % width;
image_viewer_->addCircle (u, v, 5, 1.0, 0.0, 0.0, "circles", 1.0);
image_viewer_->addFilledRectangle (u-5, u+5, v-5, v+5, 0.0, 1.0, 0.0, "boxes", 0.5);
image_viewer_->markPoint (u, v, visualization::red_color, visualization::blue_color, 10);
}
// Segment the region that we're interested in, by employing a two step process:
// * first, segment all the planes in the scene, and find the one closest to our picked point
// * then, use euclidean clustering to find the object that we clicked on and return it
PlanarRegion<PointT> region;
segment (picked_pt, indices[0], region, object_);
// If no region could be determined, exit
if (region.getContour ().empty ())
{
PCL_ERROR ("No planar region detected. Please select another point or relax the thresholds and continue.\n");
return;
}
// Else, draw it on screen
else
{
cloud_viewer_->addPolygon (region, 0.0, 0.0, 1.0, "region");
cloud_viewer_->setShapeRenderingProperties (visualization::PCL_VISUALIZER_LINE_WIDTH, 10, "region");
// Draw in image space
if (image_viewer_)
{
image_viewer_->addPlanarPolygon (search_->getInputCloud (), region, 0.0, 0.0, 1.0, "refined_region", 1.0);
}
}
// If no object could be determined, exit
if (!object_)
{
PCL_ERROR ("No object detected. Please select another point or relax the thresholds and continue.\n");
return;
}
else
{
// Visualize the object in 3D...
visualization::PointCloudColorHandlerCustom<PointT> red (object_, 255, 0, 0);
if (!cloud_viewer_->updatePointCloud (object_, red, "object"))
cloud_viewer_->addPointCloud (object_, red, "object");
// ...and 2D
if (image_viewer_)
{
image_viewer_->removeLayer ("object");
image_viewer_->addMask (search_->getInputCloud (), *object_, "object");
}
// ...and 2D
if (image_viewer_)
image_viewer_->addRectangle (search_->getInputCloud (), *object_);
}
}
/** \brief Point picking callback. This gets called when the user selects
* a 3D point on screen (in the PCLVisualizer window) using Shift+click.
*
* \param[in] event the event that triggered the call
*/
void
pp_callback (const visualization::PointPickingEvent& event, void*)
{
// Check to see if we got a valid point. Early exit.
int idx = event.getPointIndex ();
if (idx == -1)
return;
vector<int> indices (1);
vector<float> distances (1);
// Use mutices to make sure we get the right cloud
boost::mutex::scoped_lock lock1 (cloud_mutex_);
// Get the point that was picked
PointT picked_pt;
event.getPoint (picked_pt.x, picked_pt.y, picked_pt.z);
// Add a sphere to it in the PCLVisualizer window
stringstream ss;
ss << "sphere_" << idx;
cloud_viewer_.addSphere (picked_pt, 0.01, 1.0, 0.0, 0.0, ss.str ());
// Check to see if we have access to the actual cloud data. Use the previously built search object.
if (!search_.isValid ())
return;
// Because VTK/OpenGL stores data without NaN, we lose the 1-1 correspondence, so we must search for the real point
search_.nearestKSearch (picked_pt, 1, indices, distances);
// Get the [u, v] in pixel coordinates for the ImageViewer. Remember that 0,0 is bottom left.
uint32_t width = search_.getInputCloud ()->width,
height = search_.getInputCloud ()->height;
int v = height - indices[0] / width,
u = indices[0] % width;
// Add some marker to the image
image_viewer_.addCircle (u, v, 5, 1.0, 0.0, 0.0, "circles", 1.0);
image_viewer_.addFilledRectangle (u-5, u+5, v-5, v+5, 0.0, 1.0, 0.0, "boxes", 0.5);
image_viewer_.markPoint (u, v, visualization::red_color, visualization::blue_color, 10);
// Segment the region that we're interested in, by employing a two step process:
// * first, segment all the planes in the scene, and find the one closest to our picked point
// * then, use euclidean clustering to find the object that we clicked on and return it
PlanarRegion<PointT> region;
CloudPtr object;
PointIndices region_indices;
segment (picked_pt, indices[0], region, region_indices, object);
// If no region could be determined, exit
if (region.getContour ().empty ())
{
PCL_ERROR ("No planar region detected. Please select another point or relax the thresholds and continue.\n");
return;
}
// Else, draw it on screen
else
{
//cloud_viewer_.addPolygon (region, 1.0, 0.0, 0.0, "region");
//cloud_viewer_.setShapeRenderingProperties (visualization::PCL_VISUALIZER_LINE_WIDTH, 10, "region");
PlanarRegion<PointT> refined_region;
pcl::approximatePolygon (region, refined_region, 0.01, false, true);
PCL_INFO ("Planar region: %zu points initial, %zu points after refinement.\n", region.getContour ().size (), refined_region.getContour ().size ());
cloud_viewer_.addPolygon (refined_region, 0.0, 0.0, 1.0, "refined_region");
cloud_viewer_.setShapeRenderingProperties (visualization::PCL_VISUALIZER_LINE_WIDTH, 10, "refined_region");
// Draw in image space
image_viewer_.addPlanarPolygon (search_.getInputCloud (), refined_region, 0.0, 0.0, 1.0, "refined_region", 1.0);
}
// If no object could be determined, exit
if (!object)
{
PCL_ERROR ("No object detected. Please select another point or relax the thresholds and continue.\n");
return;
}
else
{
// Visualize the object in 3D...
visualization::PointCloudColorHandlerCustom<PointT> red (object, 255, 0, 0);
if (!cloud_viewer_.updatePointCloud (object, red, "object"))
cloud_viewer_.addPointCloud (object, red, "object");
// ...and 2D
image_viewer_.removeLayer ("object");
image_viewer_.addMask (search_.getInputCloud (), *object, "object");
// Compute the min/max of the object
PointT min_pt, max_pt;
getMinMax3D (*object, min_pt, max_pt);
stringstream ss;
ss << "cube_" << idx;
// Visualize the bounding box in 3D...
cloud_viewer_.addCube (min_pt.x, max_pt.x, min_pt.y, max_pt.y, min_pt.z, max_pt.z, 0.0, 1.0, 0.0, ss.str ());
cloud_viewer_.setShapeRenderingProperties (visualization::PCL_VISUALIZER_LINE_WIDTH, 10, ss.str ());
// ...and 2D
image_viewer_.addRectangle (search_.getInputCloud (), *object);
}
}