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;
}
int
process(const ecto::tendrils& inputs, const ecto::tendrils& outputs)
{
// Get the origin of the plane
cv::Point origin;
cv::Matx33f K, R;
cv::Vec3f T;
if (*do_center_)
origin = cv::Point(masks_->cols/2, masks_->rows/2);
else {
if (!K_ || K_->empty() || !R_in_ || R_in_->empty() || !T_in_ || T_in_->empty()) {
*found_ = false;
return ecto::OK;
}
K = *K_;
R = *R_in_;
T = *T_in_;
cv::Vec3f T = K*T;
origin = cv::Point(T(0)/T(2), T(1)/T(2));
}
// Go over the plane masks and simply count the occurrence of each mask
std::vector<int> occurrences(256, 0);
for(int y = std::max(0, origin.y - *window_size_); y < std::min(masks_->rows, origin.y + *window_size_); ++y) {
uchar *mask = masks_->ptr<uchar>(y) + std::max(0, origin.x - *window_size_);
uchar *mask_end = masks_->ptr<uchar>(y) + std::min(masks_->cols, origin.x + *window_size_);
for(; mask != mask_end; ++mask)
if (*mask != 255)
++occurrences[*mask];
}
// Find the most common plane
int best_index = -1;
int best_count = 0;
for(size_t i = 0; i < 255; ++i) {
if (occurrences[i] > best_count) {
best_index = i;
best_count = occurrences[i];
}
}
// Convert the plane coefficients to R,t
cv::Matx33f rotation;
cv::Vec3f translation;
if (best_index >= 0) {
*coeffs_ = (*planes_)[best_index];
float a = (*coeffs_)[0], b = (*coeffs_)[1], c = (*coeffs_)[2], d = (*coeffs_)[3];
// Deal with translation
if (*do_center_) {
getPlaneTransform(*coeffs_, rotation, translation);
// Make sure T_ points to the center of the image
translation = cv::Vec3f(0,0,-d/c);
} else {
// Have T_ point to the origin. Find alpha such that alpha*Kinv*origin is on the plane
cv::Matx33f K_inv = K.inv();
cv::Vec3f origin_inv = cv::Mat(K_inv * cv::Vec3f(origin.x, origin.y, 1));
float alpha = -d/(a*origin_inv(0) + b*origin_inv(1) + c*origin_inv(2));
translation = alpha*origin_inv;
if (translation(2) < 0)
translation = -translation;
// Make the rotation fit to the plane (but as close as possible to the current estimate
// Get the Z axis
cv::Vec3f N(a, b, c);
N = N/cv::norm(N);
// Get the X, Y axes
cv::Vec3f vecX(R(0,0), R(1,0), R(2,0));
cv::Vec3f vecY(R(0,1), R(1,1), R(2,1));
// Project them onto the plane
vecX = vecX - vecX.dot(N)*N;
vecY = vecY - vecY.dot(N)*N;
vecX = vecX/cv::norm(vecX);
vecY = vecY/cv::norm(vecY);
// Get the median
cv::Vec3f median = vecX + vecY;
median = median/cv::norm(median);
// Get a new basis
cv::Vec3f vecYtmp = vecY - median.dot(vecY)*median;
cv::Vec3f vecXtmp = vecX - median.dot(vecX)*median;
vecYtmp = vecYtmp/cv::norm(vecYtmp);
vecXtmp = vecXtmp/cv::norm(vecXtmp);
// Get the rectified X/Y axes
cv::Vec3f vecXnew = median + vecXtmp;
cv::Vec3f vecYnew = median + vecYtmp;
vecXnew = vecXnew/cv::norm(vecXnew);
vecYnew = vecYnew/cv::norm(vecYnew);
// Fill in the matrix
rotation = cv::Matx33f(vecXnew(0), vecYnew(0), N(0), vecXnew(1), vecYnew(1), N(1), vecXnew(2), vecYnew(2), N(2));
}
*R_out_ = cv::Mat(rotation);
*T_out_ = cv::Mat(translation);
*found_ = true;
} else
*found_ = false;
return ecto::OK;
}
/** Compute the pose of the table plane
* @param inputs
* @param outputs
* @return
*/
int
process(const tendrils& inputs, const tendrils& outputs)
{
std::vector<tabletop_object_detector::TabletopObjectRecognizer<pcl::PointXYZ>::TabletopResult > results;
// Process each table
pose_results_->clear();
std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> clusters_merged;
// Map to store the transformation for each cluster (table_index)
std::map<pcl::PointCloud<pcl::PointXYZ>::Ptr, size_t> cluster_table;
std::vector<cv::Vec3f> translations(clusters_->size());
std::vector<cv::Matx33f> rotations(clusters_->size());
for (size_t table_index = 0; table_index < clusters_->size(); ++table_index)
{
getPlaneTransform((*table_coefficients_)[table_index], rotations[table_index], translations[table_index]);
// Make the clusters be in the table frame
size_t n_clusters = (*clusters_)[table_index].size();
std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> clusters(n_clusters);
cv::Matx33f Rinv = rotations[table_index].t();
cv::Vec3f Tinv = -Rinv*translations[table_index];
for (size_t cluster_index = 0; cluster_index < n_clusters; ++cluster_index)
{
clusters[cluster_index] = pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>());
for(size_t i = 0; i < (*clusters_)[table_index][cluster_index].size(); ++i)
{
cv::Vec3f res = Rinv*(*clusters_)[table_index][cluster_index][i] + Tinv;
clusters[cluster_index]->push_back(pcl::PointXYZ(res[0], res[1], res[2]));
}
cluster_table.insert(std::pair<pcl::PointCloud<pcl::PointXYZ>::Ptr, size_t>(clusters[cluster_index], table_index));
}
clusters_merged.insert(clusters_merged.end(), clusters.begin(), clusters.end());
}
object_recognizer_.objectDetection(clusters_merged, confidence_cutoff_, perform_fit_merge_, results);
for (size_t i = 0; i < results.size(); ++i)
{
const tabletop_object_detector::TabletopObjectRecognizer<pcl::PointXYZ>::TabletopResult & result = results[i];
const size_t table_index = cluster_table[result.cloud_];
PoseResult pose_result;
// Add the object id
std::stringstream ss;
ss << result.object_id_;
pose_result.set_object_id(db_, ss.str());
// Add the pose
const geometry_msgs::Pose &pose = result.pose_;
cv::Vec3f T(pose.position.x, pose.position.y, pose.position.z);
Eigen::Quaternionf quat(pose.orientation.w, pose.orientation.x, pose.orientation.y, pose.orientation.z);
cv::Vec3f new_T = rotations[table_index] * T + translations[table_index];
pose_result.set_T(cv::Mat(new_T));
pose_result.set_R(quat);
cv::Mat R = cv::Mat(rotations[table_index] * pose_result.R<cv::Matx33f>());
pose_result.set_R(R);
pose_result.set_confidence(result.confidence_);
// Add the cluster of points
std::vector<sensor_msgs::PointCloud2ConstPtr> ros_clouds (1);
sensor_msgs::PointCloud2Ptr cluster_cloud (new sensor_msgs::PointCloud2());
#if PCL_VERSION_COMPARE(>=,1,7,0)
::pcl::PCLPointCloud2 pcd_tmp;
::pcl::toPCLPointCloud2(*result.cloud_, pcd_tmp);
pcl_conversions::fromPCL(pcd_tmp, *cluster_cloud);
#else
pcl::toROSMsg(*result.cloud_, *cluster_cloud);
#endif
ros_clouds[0] = cluster_cloud;
pose_result.set_clouds(ros_clouds);
pose_results_->push_back(pose_result);
}
return ecto::OK;
}
std::vector<PointCloud> TableClusterDetector::findTableClusters(const sensor_msgs::PointCloud2 &scene)
{
std::vector<PointCloud> clusters;
// Convert the dataset
PointCloud cloud; PointCloud::Ptr cloudPtr;
pcl::fromROSMsg (scene, cloud);
cloudPtr.reset(new PointCloud(cloud));
// Remove NaNs
PointCloud cloud_filtered;
pass_.setInputCloud (cloudPtr);
pass_.filter (cloud_filtered);
cloudPtr.reset(new PointCloud(cloud_filtered));
// Downsample
PointCloud cloud_downsampled;
grid_.setInputCloud (cloudPtr);
grid_.filter (cloud_downsampled);
cloudPtr.reset(new PointCloud(cloud_downsampled));
if ((int)cloud_filtered.points.size() < k_)
{
ROS_WARN("Filtering returned %zd points! Skipping.", cloud_filtered.points.size());
return clusters;
}
// Estimate the point normals
pcl::PointCloud<pcl::Normal> cloud_normals;pcl::PointCloud<pcl::Normal>::Ptr cloud_normalsPtr;
// add this if normal estimation is inaccurate
//n3d_.setSearchSurface (cloud_);
n3d_.setInputCloud (cloudPtr);
n3d_.compute (cloud_normals);
cloud_normalsPtr.reset(new pcl::PointCloud<pcl::Normal>(cloud_normals));
ROS_INFO ("[TableObjectDetector] %d normals estimated.", (int)cloud_normals.points.size ());
// ---[ Perform segmentation
pcl::PointIndices table_inliers; pcl::PointIndices::Ptr table_inliersPtr;
pcl::ModelCoefficients table_coefficients; pcl::ModelCoefficients::Ptr table_coefficientsPtr;
seg_.setInputCloud (cloudPtr);
seg_.setInputNormals (cloud_normalsPtr);
seg_.segment (table_inliers, table_coefficients);
table_inliersPtr = boost::make_shared<pcl::PointIndices>(table_inliers);
table_coefficientsPtr = boost::make_shared<pcl::ModelCoefficients>(table_coefficients);
if (table_coefficients.values.size () > 3)
ROS_INFO ("[TableObjectDetector::input_callback] Model found with %d inliers: [%f %f %f %f].", (int)table_inliers.indices.size (),
table_coefficients.values[0], table_coefficients.values[1], table_coefficients.values[2], table_coefficients.values[3]);
if (table_inliers.indices.size () == 0)
return clusters;
// ---[ Extract the table
PointCloud table_projected; PointCloud::Ptr table_projectedPtr;
proj_.setInputCloud (cloudPtr);
proj_.setIndices (table_inliersPtr);
proj_.setModelCoefficients (table_coefficientsPtr);
proj_.filter (table_projected);
table_projectedPtr.reset (new PointCloud(table_projected));
ROS_INFO ("[TableObjectDetector::input_callback] Number of projected inliers: %d.", (int)table_projected.points.size ());
sensor_msgs::PointCloud table_points;
tf::Transform table_plane_trans = getPlaneTransform (*table_coefficientsPtr, -1.0);
std::string base_frame_id = scene.header.frame_id;
ROS_INFO("sending table transform");
ros::Rate r(10);
for (int i=0; i < 10; i++) {
tf_pub_.sendTransform(tf::StampedTransform(table_plane_trans, ros::Time::now(),
base_frame_id, "table"));
r.sleep();
}
//takes the points projected on the table and transforms them into the PointCloud message
//while also transforming them into the table's coordinate system
getPlanePoints<Point> (table_projected, table_plane_trans, table_points);
ROS_INFO("Table computed");
table_ = computeTable<sensor_msgs::PointCloud>(cloud.header, table_plane_trans, table_points);
publishTable(table_);
// ---[ Estimate the convex hull
PointCloud table_hull; PointCloud::Ptr table_hullPtr;
hull_.setInputCloud (table_projectedPtr);
hull_.reconstruct (table_hull);
table_hullPtr.reset (new PointCloud(table_hull));
// ---[ Get the objects on top of the table
pcl::PointIndices cloud_object_indices; pcl::PointIndices::Ptr cloud_object_indicesPtr;
prism_.setInputCloud (cloudPtr);
prism_.setInputPlanarHull (table_hullPtr);
prism_.segment (cloud_object_indices);
cloud_object_indicesPtr = boost::make_shared<pcl::PointIndices>(cloud_object_indices);
ROS_INFO ("[TableObjectDetector::input_callback] Number of object point indices: %d.", (int)cloud_object_indices.indices.size ());
PointCloud cloud_objects; PointCloud::Ptr cloud_objectsPtr;
pcl::ExtractIndices<Point> extract_object_indices;
extract_object_indices.setInputCloud (cloudPtr);
extract_object_indices.setIndices (cloud_object_indicesPtr);
extract_object_indices.filter (cloud_objects);
cloudPtr.reset(new PointCloud(cloud_objects));
ROS_INFO ("[TableObjectDetector::input_callback] Number of object point candidates: %d.", (int)cloud_objects.points.size ());
if (cloud_objects.points.size () == 0)
return clusters;
// ---[ Downsample the points
PointCloud cloud_objects_downsampled;PointCloud::Ptr cloud_objects_downsampledPtr;
grid_objects_.setInputCloud (cloudPtr);
grid_objects_.filter (cloud_objects_downsampled);
cloudPtr.reset (new PointCloud(cloud_objects_downsampled));
ROS_INFO ("[TableObjectDetector::input_callback] Number of object point candidates left after downsampling: %d.", (int)cloud_objects_downsampled.points.size ());
// ---[ Split the objects into Euclidean clusters
std::vector<pcl::PointIndices> clustersIndices;
cluster_.setInputCloud (cloudPtr);
cluster_.extract (clustersIndices);
ROS_INFO ("[TableObjectDetector::input_callback] Number of clusters found matching the given constraints: %d.", (int)clustersIndices.size ());
AffineTransform table_trans_eig;
cse481::tfToEigen(table_plane_trans, table_trans_eig);
table_trans_eig = table_trans_eig.inverse().eval();
PointCloud cloud_in_table_frame;
pcl::transformPointCloud(cloud_objects_downsampled, cloud_in_table_frame, table_trans_eig);
// Clouds are now in table frame
BOOST_FOREACH(pcl::PointIndices indices, clustersIndices) {
PointCloud clusterCloud;
pcl::copyPointCloud(cloud_in_table_frame, indices, clusterCloud);
clusters.push_back(clusterCloud);
}
bool TableDetector::detectTable(const sensor_msgs::PointCloud2 &cloud, tabletop_object_detector::Table& table)
{
ROS_INFO("Starting process on new cloud");
ROS_INFO("In frame %s", cloud.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");
pass_.setFilterFieldName ("z");
pass_.setFilterLimits (z_filter_min_, z_filter_max_);
grid_.setFilterLimits (z_filter_min_, z_filter_max_);
grid_.setDownsampleAllData (false);
grid_objects_.setDownsampleAllData (false);
normals_tree_ = boost::make_shared<pcl::KdTreeFLANN<Point> > ();
clusters_tree_ = boost::make_shared<pcl::KdTreeFLANN<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);
// Consider only objects in a given layer above the table
prism_.setHeightLimits (table_z_filter_min_, table_z_filter_max_);
// 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
pcl::PointCloud<Point> cloud_t;
pcl::fromROSMsg (cloud, cloud_t);
pcl::PointCloud<Point>::ConstPtr cloud_ptr = boost::make_shared<const pcl::PointCloud<Point> > (cloud_t);
pcl::PointCloud<Point> cloud_filtered;
pass_.setInputCloud (cloud_ptr);
pass_.filter (cloud_filtered);
pcl::PointCloud<Point>::ConstPtr cloud_filtered_ptr =
boost::make_shared<const pcl::PointCloud<Point> > (cloud_filtered);
ROS_INFO("Step 1 done");
if (cloud_filtered.points.size() < (unsigned int)min_cluster_size_)
{
ROS_INFO("Filtered cloud only has %d points", (int)cloud_filtered.points.size());
return false;
}
pcl::PointCloud<Point> cloud_downsampled;
grid_.setInputCloud (cloud_filtered_ptr);
grid_.filter (cloud_downsampled);
pcl::PointCloud<Point>::ConstPtr cloud_downsampled_ptr =
boost::make_shared<const pcl::PointCloud<Point> > (cloud_downsampled);
if (cloud_downsampled.points.size() < (unsigned int)min_cluster_size_)
{
ROS_INFO("Downsampled cloud only has %d points", (int)cloud_downsampled.points.size());
return false;
}
// Step 2 : Estimate normals
pcl::PointCloud<pcl::Normal> cloud_normals;
n3d_.setInputCloud (cloud_downsampled_ptr);
n3d_.compute (cloud_normals);
pcl::PointCloud<pcl::Normal>::ConstPtr cloud_normals_ptr =
boost::make_shared<const pcl::PointCloud<pcl::Normal> > (cloud_normals);
ROS_INFO("Step 2 done");
// Step 3 : Perform planar segmentation
pcl::PointIndices table_inliers; pcl::ModelCoefficients table_coefficients;
seg_.setInputCloud (cloud_downsampled_ptr);
seg_.setInputNormals (cloud_normals_ptr);
seg_.segment (table_inliers, table_coefficients);
pcl::PointIndices::ConstPtr table_inliers_ptr = boost::make_shared<const pcl::PointIndices> (table_inliers);
pcl::ModelCoefficients::ConstPtr table_coefficients_ptr =
boost::make_shared<const pcl::ModelCoefficients> (table_coefficients);
if (table_coefficients.values.size () <=3)
{
ROS_INFO("Failed to detect table in scan");
return false;
}
if ( table_inliers.indices.size() < (unsigned int)inlier_threshold_)
{
ROS_INFO("Plane detection has %d inliers, below min threshold of %d", (int)table_inliers.indices.size(),
inlier_threshold_);
return false;
}
ROS_INFO ("[TableObjectDetector::input_callback] Model found with %d inliers: [%f %f %f %f].",
(int)table_inliers.indices.size (),
table_coefficients.values[0], table_coefficients.values[1],
table_coefficients.values[2], table_coefficients.values[3]);
ROS_INFO("Step 3 done");
// Step 4 : Project the table inliers on the table
pcl::PointCloud<Point> table_projected;
proj_.setInputCloud (cloud_downsampled_ptr);
proj_.setIndices (table_inliers_ptr);
proj_.setModelCoefficients (table_coefficients_ptr);
proj_.filter (table_projected);
pcl::PointCloud<Point>::ConstPtr table_projected_ptr =
boost::make_shared<const pcl::PointCloud<Point> > (table_projected);
ROS_INFO("Step 4 done");
sensor_msgs::PointCloud table_points;
tf::Transform table_plane_trans = getPlaneTransform (table_coefficients, up_direction_);
//takes the points projected on the table and transforms them into the PointCloud message
//while also transforming them into the table's coordinate system
if (!getPlanePoints(table_projected, table_plane_trans, table_points))
{
return false;
}
ROS_INFO("Table computed");
table = getTable(cloud.header, table_plane_trans, table_points);
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);
}
