bool TabletopSegmentor<PointT>::processCloud(const PointCloudConstPtr &_cloud ) {
// PCL objects
KdTreePtr normals_tree_, clusters_tree_;
pcl::VoxelGrid<PointT> grid_, grid_objects_;
pcl::PassThrough<PointT> pass_;
pcl::NormalEstimation<PointT, pcl::Normal> n3d_;
pcl::SACSegmentationFromNormals<PointT, pcl::Normal> seg_;
pcl::ProjectInliers<PointT> proj_;
pcl::ConvexHull<PointT> hull_;
pcl::ExtractPolygonalPrismData<PointT> prism_;
pcl::EuclideanClusterExtraction<PointT> pcl_cluster_;
PointCloudPtr table_hull_ptr (new PointCloud);
/************ STEP 0: Parameter Settings **************/
// Filtering parameters
grid_.setLeafSize (plane_detection_voxel_size_, plane_detection_voxel_size_, plane_detection_voxel_size_);
grid_objects_.setLeafSize (clustering_voxel_size_, clustering_voxel_size_, clustering_voxel_size_);
grid_.setFilterFieldName ("z");
grid_.setFilterLimits (z_filter_min_, z_filter_max_);
grid_.setDownsampleAllData (false);
grid_objects_.setDownsampleAllData (true);
normals_tree_ = boost::make_shared<pcl::search::KdTree<PointT> > ();
clusters_tree_ = boost::make_shared<pcl::search::KdTree<PointT> > ();
// Normal estimation parameters
n3d_.setKSearch (10);
n3d_.setSearchMethod (normals_tree_);
// Table model fitting parameters
seg_.setDistanceThreshold (0.01);
seg_.setMaxIterations (10000);
seg_.setNormalDistanceWeight (0.1);
seg_.setOptimizeCoefficients (true);
seg_.setModelType (pcl::SACMODEL_PLANE);
//seg_.setModelType (pcl::SACMODEL_NORMAL_PLANE);
//seg_.setModelType( pcl::SACMODEL_PERPENDICULAR_PLANE );
//seg_.setEpsAngle( 45*M_PI/180.0 );
//seg_.setAxis( Eigen::Vector3f(0,1,0) );
seg_.setMethodType (pcl::SAC_RANSAC);
seg_.setProbability (0.99);
//proj_.setModelType (pcl::SACMODEL_NORMAL_PLANE);
proj_.setModelType (pcl::SACMODEL_PERPENDICULAR_PLANE);
// Clustering parameters
pcl_cluster_.setClusterTolerance (cluster_distance_);
pcl_cluster_.setMinClusterSize (min_cluster_size_);
pcl_cluster_.setSearchMethod (clusters_tree_);
/******** Step 1 : Filter, remove NaNs and downsample ***********/
// Filter in X, Y and Z directions: output= cloud_filtered_ptr
pass_.setInputCloud (_cloud);
pass_.setFilterFieldName ("z");
pass_.setFilterLimits (z_filter_min_, z_filter_max_);
PointCloudPtr z_cloud_filtered_ptr (new PointCloud);
pass_.filter (*z_cloud_filtered_ptr);
pass_.setInputCloud (z_cloud_filtered_ptr);
pass_.setFilterFieldName ("y");
pass_.setFilterLimits (y_filter_min_, y_filter_max_);
PointCloudPtr y_cloud_filtered_ptr (new PointCloud);
pass_.filter (*y_cloud_filtered_ptr);
pass_.setInputCloud (y_cloud_filtered_ptr);
pass_.setFilterFieldName ("x");
pass_.setFilterLimits (x_filter_min_, x_filter_max_);
PointCloudPtr cloud_filtered_ptr (new PointCloud);
pass_.filter (*cloud_filtered_ptr);
// Check that the points filtered at least are of a minimum size
if (cloud_filtered_ptr->points.size() < (unsigned int)min_cluster_size_) {
printf("\t [ERROR] Filtered cloud only has %d points. \n",
(int)cloud_filtered_ptr->points.size() );
return false;
}
pcl::io::savePCDFile( "filtered_cloud.pcd", *cloud_filtered_ptr, true );
// Downsample the filtered cloud: output = cloud_downsampled_ptr
PointCloudPtr cloud_downsampled_ptr (new PointCloud);
grid_.setInputCloud (cloud_filtered_ptr);
grid_.filter (*cloud_downsampled_ptr);
if (cloud_downsampled_ptr->points.size() < (unsigned int)min_cluster_size_) {
printf( "\t [ERROR] Downsampled cloud only has %d points \n",
(int)cloud_downsampled_ptr->points.size() );
return false;
}
printf( "\t Downsampled cloud only has %d points / %d. \n",
(int)cloud_downsampled_ptr->points.size(), (int)cloud_filtered_ptr->points.size() );
/***************** Step 2 : Estimate normals ******************/
pcl::PointCloud<pcl::Normal>::Ptr cloud_normals_ptr (new pcl::PointCloud<pcl::Normal>);
n3d_.setInputCloud (cloud_downsampled_ptr);
n3d_.compute (*cloud_normals_ptr);
/************* Step 3 : Perform planar segmentation, **********/
/** if table is not given, otherwise use given table */
Eigen::Matrix4d table_plane_trans;
Eigen::Matrix4d table_plane_trans_flat;
pcl::PointIndices::Ptr table_inliers_ptr (new pcl::PointIndices);
pcl::ModelCoefficients::Ptr table_coefficients_ptr (new pcl::ModelCoefficients);
seg_.setInputCloud (cloud_downsampled_ptr);
seg_.setInputNormals (cloud_normals_ptr);
seg_.segment( *table_inliers_ptr,
*table_coefficients_ptr );
// Check the coefficients and inliers are above threshold values
if (table_coefficients_ptr->values.size () <=3 ) {
printf( "\t [ERROR] Failed to detect table in scan \n" );
return false;
}
if ( table_inliers_ptr->indices.size() < (unsigned int)inlier_threshold_) {
printf( "\t [ERROR] Plane detection has %d below min thresh of %d points \n",
(int)table_inliers_ptr->indices.size(),
inlier_threshold_ );
return false;
}
pcl::ExtractIndices<PointT> extract;
PointCloudPtr tablePointsPtr( new PointCloud );
extract.setInputCloud( cloud_downsampled_ptr );
extract.setIndices( table_inliers_ptr );
extract.setNegative(false);
extract.filter( *tablePointsPtr );
int na = tablePointsPtr->points.size();
if( na > 0 ) {
for( int a = 0; a < na; ++a ) {
tablePointsPtr->points[a].r = 255;
tablePointsPtr->points[a].g = 255;
tablePointsPtr->points[a].b = 255;
tablePointsPtr->points[a].a = 255;
}
pcl::io::savePCDFileASCII( "table_points.pcd", *tablePointsPtr );
}
/********** Step 4 : Project the table inliers on the table *********/
PointCloudPtr table_projected_ptr (new PointCloud);
proj_.setInputCloud (cloud_downsampled_ptr);
proj_.setIndices (table_inliers_ptr);
proj_.setModelCoefficients (table_coefficients_ptr);
proj_.filter (*table_projected_ptr);
// Get the table transformation w.r.t. camera
table_plane_trans = getPlaneTransform (*table_coefficients_ptr, up_direction_, false);
mTableTf.matrix() = table_plane_trans;
// ---[ Estimate the convex hull (not in table frame)
hull_.setInputCloud (table_projected_ptr);
hull_.reconstruct (*table_hull_ptr);
// Save points in the hull with some points down
mTable_Points.points.resize(0);
for( int i = 0; i < table_hull_ptr->points.size(); ++i ) {
mTable_Points.points.push_back(table_hull_ptr->points[i]);
}
mTable_Points.width = 1; mTable_Points.height = mTable_Points.points.size();
// Store table's plane coefficients (after calling getPlaneTransform! because here we check if a sign change is needed)
mTableCoeffs.resize(4);
for( int i = 0; i < 4; ++i ) { mTableCoeffs[i] = table_coefficients_ptr->values[i]; }
/******* Step 5 : Get the objects on top of the table ******/
pcl::PointIndices cloud_object_indices;
prism_.setInputCloud (cloud_filtered_ptr);
prism_.setInputPlanarHull (table_hull_ptr);
prism_.setHeightLimits (table_obj_height_filter_min_, table_obj_height_filter_max_);
prism_.segment (cloud_object_indices);
PointCloudPtr cloud_objects_ptr (new PointCloud);
pcl::ExtractIndices<PointT> extract_object_indices;
extract_object_indices.setInputCloud (cloud_filtered_ptr);
extract_object_indices.setIndices (boost::make_shared<const pcl::PointIndices> (cloud_object_indices));
extract_object_indices.filter (*cloud_objects_ptr);
if (cloud_objects_ptr->points.empty ()) {
std::cout<<"\t [ERROR] No object points on table" << std::endl;
return false;
}
pcl::io::savePCDFile( "cloudObjects.pcd", *cloud_objects_ptr, true );
// Downsample the points
PointCloudPtr cloud_objects_downsampled_ptr (new PointCloud);
grid_objects_.setInputCloud (cloud_objects_ptr);
grid_objects_.filter (*cloud_objects_downsampled_ptr);
printf("Object clouds from %ld to %ld \n", cloud_objects_ptr->points.size(),
cloud_objects_downsampled_ptr->points.size() );
// Step 6: Split the objects into Euclidean clusters
std::vector<pcl::PointIndices> clusters2;
pcl_cluster_.setInputCloud (cloud_objects_downsampled_ptr);
pcl_cluster_.extract (clusters2);
mClusterInds.resize( clusters2.size() );
for( int i = 0; i < clusters2.size(); ++i ) {
mClusterInds[i] = clusters2[i];
printf("Cluster %d size: %ld \n", i, clusters2[i].indices.size() );
}
mClusters.resize( clusters2.size() );
int i = 0;
for( std::vector<pcl::PointIndices>::const_iterator it =
clusters2.begin (); it != clusters2.end (); ++it ) {
PointCloud cloud_cluster;
for (std::vector<int>::const_iterator pit = it->indices.begin (); pit != it->indices.end (); ++pit) {
cloud_cluster.points.push_back (cloud_objects_downsampled_ptr->points[*pit]);
}
cloud_cluster.width = cloud_cluster.points.size ();
cloud_cluster.height = 1;
cloud_cluster.is_dense = true;
mClusters[i] = cloud_cluster;
i++;
}
return true;
}
void TabletopSegmentor::processCloud(pcl::PointCloud<Point>::Ptr &cloud_ptr,
TabletopSegmentation::Response &response)
{
//ROS_INFO("Starting process on new cloud in frame %s", cloud_ptr->header.frame_id.c_str());
// PCL objects
KdTreePtr normals_tree_, clusters_tree_;
pcl::VoxelGrid<Point> grid_, grid_objects_;
pcl::PassThrough<Point> pass_;
pcl::NormalEstimation<Point, pcl::Normal> n3d_;
pcl::SACSegmentationFromNormals<Point, pcl::Normal> seg_;
pcl::ProjectInliers<Point> proj_;
pcl::ConvexHull<Point> hull_;
pcl::ExtractPolygonalPrismData<Point> prism_;
pcl::EuclideanClusterExtraction<Point> pcl_cluster_;
// Filtering parameters
grid_.setLeafSize (plane_detection_voxel_size_, plane_detection_voxel_size_, plane_detection_voxel_size_);
grid_objects_.setLeafSize (clustering_voxel_size_, clustering_voxel_size_, clustering_voxel_size_);
grid_.setFilterFieldName ("z");
grid_.setFilterLimits (z_filter_min_, z_filter_max_);
grid_.setDownsampleAllData (false);
grid_objects_.setDownsampleAllData (false);
normals_tree_ = boost::make_shared<pcl::search::KdTree<Point> > ();
clusters_tree_ = boost::make_shared<pcl::search::KdTree<Point> > ();
// Normal estimation parameters
n3d_.setKSearch (10);
n3d_.setSearchMethod (normals_tree_);
// Table model fitting parameters
seg_.setDistanceThreshold (0.05);
seg_.setMaxIterations (10000);
seg_.setNormalDistanceWeight (0.1);
seg_.setOptimizeCoefficients (true);
seg_.setModelType (pcl::SACMODEL_NORMAL_PLANE);
seg_.setMethodType (pcl::SAC_RANSAC);
seg_.setProbability (0.99);
proj_.setModelType (pcl::SACMODEL_PLANE);
// Clustering parameters
pcl_cluster_.setClusterTolerance (cluster_distance_);
pcl_cluster_.setMinClusterSize (min_cluster_size_);
pcl_cluster_.setSearchMethod (clusters_tree_);
// Step 1 : Filter, remove NaNs and downsample
pass_.setInputCloud (cloud_ptr);
pass_.setFilterFieldName ("z");
pass_.setFilterLimits (z_filter_min_, z_filter_max_);
pcl::PointCloud<Point>::Ptr z_cloud_filtered_ptr (new pcl::PointCloud<Point>);
pass_.filter (*z_cloud_filtered_ptr);
pass_.setInputCloud (z_cloud_filtered_ptr);
pass_.setFilterFieldName ("y");
pass_.setFilterLimits (y_filter_min_, y_filter_max_);
pcl::PointCloud<Point>::Ptr y_cloud_filtered_ptr (new pcl::PointCloud<Point>);
pass_.filter (*y_cloud_filtered_ptr);
pass_.setInputCloud (y_cloud_filtered_ptr);
pass_.setFilterFieldName ("x");
pass_.setFilterLimits (x_filter_min_, x_filter_max_);
pcl::PointCloud<Point>::Ptr cloud_filtered_ptr (new pcl::PointCloud<Point>);
pass_.filter (*cloud_filtered_ptr);
//ROS_INFO("Step 1 done");
if (cloud_filtered_ptr->points.size() < (unsigned int)min_cluster_size_)
{
//ROS_INFO("Filtered cloud only has %d points", (int)cloud_filtered_ptr->points.size());
response.result = response.NO_TABLE;
return;
}
pcl::PointCloud<Point>::Ptr cloud_downsampled_ptr (new pcl::PointCloud<Point>);
grid_.setInputCloud (cloud_filtered_ptr);
grid_.filter (*cloud_downsampled_ptr);
if (cloud_downsampled_ptr->points.size() < (unsigned int)min_cluster_size_)
{
//ROS_INFO("Downsampled cloud only has %d points", (int)cloud_downsampled_ptr->points.size());
response.result = response.NO_TABLE;
return;
}
// Step 2 : Estimate normals
pcl::PointCloud<pcl::Normal>::Ptr cloud_normals_ptr (new pcl::PointCloud<pcl::Normal>);
n3d_.setInputCloud (cloud_downsampled_ptr);
n3d_.compute (*cloud_normals_ptr);
//ROS_INFO("Step 2 done");
// Step 3 : Perform planar segmentation
pcl::PointIndices::Ptr table_inliers_ptr (new pcl::PointIndices);
pcl::ModelCoefficients::Ptr table_coefficients_ptr (new pcl::ModelCoefficients);
seg_.setInputCloud (cloud_downsampled_ptr);
seg_.setInputNormals (cloud_normals_ptr);
seg_.segment (*table_inliers_ptr, *table_coefficients_ptr);
if (table_coefficients_ptr->values.size () <=3)
{
//ROS_INFO("Failed to detect table in scan");
response.result = response.NO_TABLE;
return;
}
if ( table_inliers_ptr->indices.size() < (unsigned int)inlier_threshold_)
{
//ROS_INFO("Plane detection has %d inliers, below min threshold of %d", (int)table_inliers_ptr->indices.size(),inlier_threshold_);
response.result = response.NO_TABLE;
return;
}
//ROS_INFO ("[TableObjectDetector::input_callback] Model found with %d inliers: [%f %f %f %f].", (int)table_inliers_ptr->indices.size (), table_coefficients_ptr->values[0], table_coefficients_ptr->values[1], table_coefficients_ptr->values[2], table_coefficients_ptr->values[3]);
//ROS_INFO("Step 3 done");
// Step 4 : Project the table inliers on the table
pcl::PointCloud<Point>::Ptr table_projected_ptr (new pcl::PointCloud<Point>);
proj_.setInputCloud (cloud_downsampled_ptr);
proj_.setIndices (table_inliers_ptr);
proj_.setModelCoefficients (table_coefficients_ptr);
proj_.filter (*table_projected_ptr);
//ROS_INFO("Step 4 done");
sensor_msgs::PointCloud table_points;
sensor_msgs::PointCloud table_hull_points;
tf::Transform table_plane_trans = getPlaneTransform (*table_coefficients_ptr, up_direction_, false);
tf::Transform table_plane_trans_flat;
// ---[ Estimate the convex hull (not in table frame)
pcl::PointCloud<Point>::Ptr table_hull_ptr (new pcl::PointCloud<Point>);
hull_.setInputCloud (table_projected_ptr);
hull_.reconstruct (*table_hull_ptr);
if(!flatten_table_)
{
// --- [ Take the points projected on the table and transform them into the PointCloud message
// while also transforming them into the table's coordinate system
if (!getPlanePoints<Point> (*table_projected_ptr, table_plane_trans, table_points))
{
response.result = response.OTHER_ERROR;
return;
}
// ---[ Create the table message
response.table = getTable<sensor_msgs::PointCloud>(cloud_ptr->header, table_plane_trans, table_points);
// ---[ Convert the convex hull points to table frame
if (!getPlanePoints<Point> (*table_hull_ptr, table_plane_trans, table_hull_points))
{
response.result = response.OTHER_ERROR;
return;
}
}
if(flatten_table_)
{
// if flattening the table, find the center of the convex hull and move the table frame there
table_plane_trans_flat = getPlaneTransform (*table_coefficients_ptr, up_direction_, flatten_table_);
tf::Vector3 flat_table_pos;
double avg_x, avg_y, avg_z;
avg_x = avg_y = avg_z = 0;
for (size_t i=0; i<table_projected_ptr->points.size(); i++)
{
avg_x += table_projected_ptr->points[i].x;
avg_y += table_projected_ptr->points[i].y;
avg_z += table_projected_ptr->points[i].z;
}
avg_x /= table_projected_ptr->points.size();
avg_y /= table_projected_ptr->points.size();
avg_z /= table_projected_ptr->points.size();
//ROS_INFO("average x,y,z = (%.5f, %.5f, %.5f)", avg_x, avg_y, avg_z);
// place the new table frame in the center of the convex hull
flat_table_pos[0] = avg_x;
flat_table_pos[1] = avg_y;
flat_table_pos[2] = avg_z;
table_plane_trans_flat.setOrigin(flat_table_pos);
// shift the non-flat table frame to the center of the convex hull as well
table_plane_trans.setOrigin(flat_table_pos);
// --- [ Take the points projected on the table and transform them into the PointCloud message
// while also transforming them into the flat table's coordinate system
sensor_msgs::PointCloud flat_table_points;
if (!getPlanePoints<Point> (*table_projected_ptr, table_plane_trans_flat, flat_table_points))
{
response.result = response.OTHER_ERROR;
return;
}
// ---[ Create the table message
response.table = getTable<sensor_msgs::PointCloud>(cloud_ptr->header, table_plane_trans_flat, flat_table_points);
// ---[ Convert the convex hull points to flat table frame
if (!getPlanePoints<Point> (*table_hull_ptr, table_plane_trans_flat, table_hull_points))
{
response.result = response.OTHER_ERROR;
return;
}
}
//ROS_INFO("Table computed");
response.result = response.SUCCESS;
// ---[ Add the convex hull as a triangle mesh to the Table message
// addConvexHullTable<sensor_msgs::PointCloud>(response.table, table_hull_points, flatten_table_);
// ---[ Get the objects on top of the (non-flat) table
pcl::PointIndices cloud_object_indices;
prism_.setInputCloud (cloud_filtered_ptr);
prism_.setInputPlanarHull (table_hull_ptr);
//ROS_INFO("Using table prism: %f to %f", table_z_filter_min_, table_z_filter_max_);
prism_.setHeightLimits (table_z_filter_min_, table_z_filter_max_);
prism_.segment (cloud_object_indices);
pcl::PointCloud<Point>::Ptr cloud_objects_ptr (new pcl::PointCloud<Point>);
pcl::ExtractIndices<Point> extract_object_indices;
extract_object_indices.setInputCloud (cloud_filtered_ptr);
extract_object_indices.setIndices (boost::make_shared<const pcl::PointIndices> (cloud_object_indices));
extract_object_indices.filter (*cloud_objects_ptr);
//ROS_INFO (" Number of object point candidates: %d.", (int)cloud_objects_ptr->points.size ());
if (cloud_objects_ptr->points.empty ())
{
//ROS_INFO("No objects on table");
return;
}
// ---[ Downsample the points
pcl::PointCloud<Point>::Ptr cloud_objects_downsampled_ptr (new pcl::PointCloud<Point>);
grid_objects_.setInputCloud (cloud_objects_ptr);
grid_objects_.filter (*cloud_objects_downsampled_ptr);
// ---[ Split the objects into Euclidean clusters
std::vector<pcl::PointIndices> clusters2;
pcl_cluster_.setInputCloud (cloud_objects_ptr);
//pcl_cluster_.setInputCloud (cloud_objects_downsampled_ptr);
pcl_cluster_.extract (clusters2);
//:ROS_INFO ("Number of clusters found matching the given constraints: %d.", (int)clusters2.size ());
// ---[ Convert clusters into the PointCloud message
std::vector<sensor_msgs::PointCloud2> clusters;
getClusters<Point> (*cloud_objects_ptr, clusters2, clusters);
//ROS_INFO("Clusters converted");
response.clusters = clusters;
// publishClusterMarkers(clusters, cloud.header);
}