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); }