// Align a collection of object templates to a sample point cloud
void cloud_cb( const sensor_msgs::PointCloud2ConstPtr& input )
{
if( controllerState != 1 )
return;
//--- Convert Incoming Cloud --- //
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud( new pcl::PointCloud<pcl::PointXYZ> );
pcl::fromROSMsg( *input, *cloud );
// --- Z-Filter And Downsample Cloud --- //
// Preprocess the cloud by removing distant points
pcl::PassThrough<pcl::PointXYZ> pass_z;
pass_z.setInputCloud( cloud );
pass_z.setFilterFieldName( "z" );
pass_z.setFilterLimits( 0, 1.75 );
pass_z.filter( *cloud );
pcl::PassThrough<pcl::PointXYZ> pass_y;
pass_y.setInputCloud( cloud );
pass_y.setFilterFieldName("y");
pass_y.setFilterLimits( -0.5, 0.2 );
pass_y.filter( *cloud );
pcl::PassThrough<pcl::PointXYZ> pass_x;
pass_x.setInputCloud( cloud );
pass_x.setFilterFieldName("x");
pass_x.setFilterLimits( -0.5, 0.5 );
pass_x.filter( *cloud );
// It is possible to not have any points after z-filtering (ex. if we are looking up).
// Just bail if there is nothing left.
if( cloud->points.size() == 0 )
return;
//visualize( cloud, visualizer_o_Ptr );
// --- Calculate Scene Normals --- //
pcl::PointCloud<pcl::Normal>::Ptr pSceneNormals( new pcl::PointCloud<pcl::Normal>() );
pcl::NormalEstimationOMP<pcl::PointXYZ, pcl::Normal> normEst;
normEst.setKSearch(10);
normEst.setInputCloud( cloud );
normEst.compute( *pSceneNormals );
// --- Get Rid Of Floor --- //
pcl::PointIndices::Ptr inliers_plane( new pcl::PointIndices );
pcl::ModelCoefficients::Ptr coefficients_plane( new pcl::ModelCoefficients );
pcl::SACSegmentationFromNormals<pcl::PointXYZ, pcl::Normal> seg1;
seg1.setOptimizeCoefficients( true );
seg1.setModelType( pcl::SACMODEL_NORMAL_PLANE );
seg1.setNormalDistanceWeight( 0.05 );
seg1.setMethodType( pcl::SAC_RANSAC );
seg1.setMaxIterations( 100 );
seg1.setDistanceThreshold( 0.075 );
seg1.setInputCloud( cloud );
seg1.setInputNormals( pSceneNormals );
// Obtain the plane inliers and coefficients
seg1.segment( *inliers_plane, *coefficients_plane );
// Extract the planar inliers from the input cloud
pcl::ExtractIndices<pcl::PointXYZ> extract;
extract.setInputCloud( cloud );
extract.setIndices( inliers_plane );
extract.setNegative( false );
// Write the planar inliers to disk
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_plane( new pcl::PointCloud<pcl::PointXYZ> );
extract.filter( *cloud_plane );
// Remove the planar inliers, extract the rest
pcl::PointCloud<pcl::PointXYZ>::Ptr filteredScene( new pcl::PointCloud<pcl::PointXYZ> );
extract.setNegative( true );
extract.filter( *filteredScene );
pcl::ExtractIndices<pcl::Normal> extract_normals;
pcl::PointCloud<pcl::Normal>::Ptr filteredSceneNormals( new pcl::PointCloud<pcl::Normal> );
extract_normals.setNegative( true );
extract_normals.setInputCloud( pSceneNormals );
extract_normals.setIndices( inliers_plane );
extract_normals.filter( *filteredSceneNormals );
if( filteredScene->points.size() == 0 )
return;
// --- Set Our Target Cloud --- //
// Assign to the target FeatureCloud
FeatureCloud target_cloud;
target_cloud.setInputCloud( filteredScene );
// --- Visualize the Filtered Cloud --- //
//visualize( filteredScene, visualizer_o_Ptr );
// --- Set Input Cloud For Alignment --- //
template_align.setTargetCloud( target_cloud );
// --- Align Templates --- //
std::cout << "Searching for best fit" << std::endl;
// Find the best template alignment
TemplateAlignment::Result best_alignment;
int best_index = template_align.findBestAlignment( best_alignment );
std::cerr << "Best alignment index: " << best_index << std::endl;
const FeatureCloud &best_template = object_templates[best_index];
// --- Report Best Match --- //
// Print the alignment fitness score (values less than 0.00002 are good)
std::cerr << "Best fitness score: " << best_alignment.fitness_score << std::endl;
// Print the rotation matrix and translation vector
Eigen::Matrix3f rotation = best_alignment.final_transformation.block<3,3>(0, 0);
Eigen::Vector3f translation = best_alignment.final_transformation.block<3,1>(0, 3);
std::cerr << std::setprecision(3);
std::cerr << std::endl;
std::cerr << " | " << rotation(0,0) << " " << rotation(0,1) << " " << rotation(0,2) << " | " << std::endl;
std::cerr << "R = | " << rotation(1,0) << " " << rotation(1,1) << " " << rotation(1,2) << " | " << std::endl;
std::cerr << " | " << rotation(2,0) << " " << rotation(2,1) << " " << rotation(2,2) << " | " << std::endl;
std::cerr << std::endl;
std::cerr << "t = < " << translation(0) << ", " << translation(1) << ", " << translation(2) << " >" << std::endl << std::endl;
// pcl::PointCloud<pcl::PointXYZ> transformedCloud;
// pcl::transformPointCloud( *best_template.getPointCloud(), transformedCloud, best_alignment.final_transformation);
// visualize( filteredScene, transformedCloud.makeShared(), visualizer_o_Ptr );
// --- Publish --- //
// TODO: Clean up this part.
geometry_msgs::Pose pose;
tf::Matrix3x3 rot( rotation(0,0), rotation(0,1), rotation(0,2),
rotation(1,0), rotation(1,1), rotation(1,2),
rotation(2,0), rotation(2,1), rotation(2,2) );
tf::Quaternion q;
rot.getRotation(q);
pose.orientation.w = q.getW();
pose.orientation.x = q.getX();
pose.orientation.y = q.getY();
pose.orientation.z = q.getZ();
tf::Vector3 t( translation(0), translation(1), translation(2) );
pose.position.x = t.getX();
pose.position.y = t.getY();
pose.position.z = t.getZ();
std::cerr << "Publishing" << std::endl;
pub.publish(pose);
sensor_msgs::PointCloud2 toPub;
pcl::toROSMsg( *filteredScene, toPub );
cloud_pub.publish(toPub);
}
void cloud_callback(const sensor_msgs::PointCloud2ConstPtr& input) {
ROS_DEBUG("Got new pointcloud.");
// Convert the sensor_msgs/PointCloud2 data to pcl/PointCloud
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>), cloud_f (new pcl::PointCloud<pcl::PointXYZ>);
std_msgs::Header header = input->header;
pcl::fromROSMsg(*input, *cloud);
//all the objects needed
pcl::PCDReader reader;
pcl::PassThrough<PointT> pass;
pcl::NormalEstimation<PointT, pcl::Normal> ne;
pcl::SACSegmentationFromNormals<PointT, pcl::Normal> seg;
pcl::PCDWriter writer;
pcl::ExtractIndices<PointT> extract;
pcl::ExtractIndices<pcl::Normal> extract_normals;
pcl::search::KdTree<PointT>::Ptr tree (new pcl::search::KdTree<PointT> ());
//data sets
pcl::PointCloud<PointT>::Ptr cloud_filtered (new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::Normal>::Ptr cloud_normals (new pcl::PointCloud<pcl::Normal>);
pcl::PointCloud<PointT>::Ptr cloud_filtered2 (new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::Normal>::Ptr cloud_normals2 (new pcl::PointCloud<pcl::Normal>);
pcl::ModelCoefficients::Ptr coefficients_plane (new pcl::ModelCoefficients), coefficients_sphere (new pcl::ModelCoefficients);
pcl::PointIndices::Ptr inliers_plane (new pcl::PointIndices), inliers_sphere (new pcl::PointIndices);
ROS_DEBUG("PointCloud before filtering has: %i data points.", (int)cloud->points.size());
//filter our NaNs
pass.setInputCloud (cloud);
pass.setFilterFieldName ("z");
pass.setFilterLimits (0, 2.0);
pass.filter (*cloud_filtered);
ROS_DEBUG("PointCloud after filtering has: %i data points." , (int)cloud_filtered->points.size());
*cloud_filtered = *cloud;//remove the pass through filter basically
//estimate normal points
ne.setSearchMethod (tree);
ne.setInputCloud (cloud_filtered);
//ne.setKSearch(10);
ne.setRadiusSearch(0.025);
ne.compute (*cloud_normals);
// Create the segmentation object for the planar model and set all the parameters
seg.setOptimizeCoefficients (true);
seg.setModelType (pcl::SACMODEL_NORMAL_PLANE);
seg.setNormalDistanceWeight (0.05);
seg.setMethodType (pcl::SAC_RANSAC);
seg.setMaxIterations (100);
seg.setDistanceThreshold (5.0);
seg.setInputCloud (cloud_filtered);
seg.setInputNormals (cloud_normals);
// Obtain the plane inliers and coefficients
seg.segment (*inliers_plane, *coefficients_plane);
std::cerr << "Plane coefficients: " << *coefficients_plane << std::endl;
// Extract the planar inliers from the input cloud
extract.setInputCloud (cloud_filtered);
extract.setIndices (inliers_plane);
extract.setNegative (false);
// Remove the planar inliers, extract the rest
extract.setNegative (true);
extract.filter (*cloud_filtered2);
extract_normals.setNegative (true);
extract_normals.setInputCloud (cloud_normals);
extract_normals.setIndices (inliers_plane);
extract_normals.filter (*cloud_normals2);
seg.setOptimizeCoefficients (true);
seg.setModelType (pcl::SACMODEL_SPHERE);
seg.setMethodType (pcl::SAC_RANSAC);
seg.setNormalDistanceWeight (0.1);
seg.setMaxIterations (100);
seg.setDistanceThreshold (5.0);
seg.setRadiusLimits (0, 1.0);
seg.setInputCloud (cloud_filtered2);
seg.setInputNormals (cloud_normals2);
extract.setInputCloud (cloud_filtered2);
extract.setIndices (inliers_sphere);
extract.setNegative (false);
pcl::PointCloud<PointT>::Ptr sphere_cloud (new pcl::PointCloud<PointT> ());
extract.filter(*sphere_cloud);
pcl::PointCloud<pcl::PointXYZ> sphere_cloud_in = *sphere_cloud;
sensor_msgs::PointCloud2 sphere_cloud_out;
pcl::toROSMsg(sphere_cloud_in, sphere_cloud_out);
sphere_cloud_out.header = header;
buoy_cloud_pub.publish(sphere_cloud_out);
}
void callback(const sensor_msgs::PointCloud2ConstPtr& cloud)
{
ros::Time whole_start = ros::Time::now();
ros::Time declare_types_start = ros::Time::now();
// filter
pcl::VoxelGrid<sensor_msgs::PointCloud2> voxel_grid;
pcl::PassThrough<sensor_msgs::PointCloud2> pass;
pcl::ExtractIndices<pcl::PointXYZ> extract_indices;
pcl::ExtractIndices<pcl::Normal> extract_normals;
// Normal estimation
pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> normal_estimation;
pcl::SACSegmentationFromNormals<pcl::PointXYZ, pcl::Normal> segmentation_from_normals;
// Create the segmentation object
pcl::SACSegmentation<pcl::PointXYZ> seg;
pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ> ());
pcl::search::KdTree<pcl::PointXYZ>::Ptr tree2 (new pcl::search::KdTree<pcl::PointXYZ> ());
pcl::search::KdTree<pcl::PointXYZ>::Ptr tree3 (new pcl::search::KdTree<pcl::PointXYZ> ());
// The plane and sphere coefficients
pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients ());
pcl::ModelCoefficients::Ptr coefficients_plane (new pcl::ModelCoefficients ());
pcl::ModelCoefficients::Ptr coefficients_cylinder (new pcl::ModelCoefficients ());
pcl::ModelCoefficients::Ptr coefficients_sphere (new pcl::ModelCoefficients ());
// The plane and sphere inliers
pcl::PointIndices::Ptr inliers (new pcl::PointIndices ());
pcl::PointIndices::Ptr inliers_plane (new pcl::PointIndices ());
pcl::PointIndices::Ptr inliers_cylinder (new pcl::PointIndices ());
pcl::PointIndices::Ptr inliers_sphere (new pcl::PointIndices ());
// The point clouds
sensor_msgs::PointCloud2::Ptr voxelgrid_filtered (new sensor_msgs::PointCloud2);
sensor_msgs::PointCloud2::Ptr plane_output_cloud (new sensor_msgs::PointCloud2);
sensor_msgs::PointCloud2::Ptr rest_output_cloud (new sensor_msgs::PointCloud2);
sensor_msgs::PointCloud2::Ptr rest_cloud_filtered (new sensor_msgs::PointCloud2);
sensor_msgs::PointCloud2::Ptr cylinder_output_cloud (new sensor_msgs::PointCloud2);
sensor_msgs::PointCloud2::Ptr sphere_output_cloud (new sensor_msgs::PointCloud2);
// The PointCloud
pcl::PointCloud<pcl::PointXYZ>::Ptr transformed_cloud (new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_plane (new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr remove_transformed_cloud (new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr cylinder_cloud (new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr cylinder_output (new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr sphere_cloud (new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr sphere_output (new pcl::PointCloud<pcl::PointXYZ>);
// The cloud normals
pcl::PointCloud<pcl::Normal>::Ptr cloud_normals (new pcl::PointCloud<pcl::Normal> ()); // for plane
pcl::PointCloud<pcl::Normal>::Ptr cloud_normals2 (new pcl::PointCloud<pcl::Normal> ()); // for cylinder
pcl::PointCloud<pcl::Normal>::Ptr cloud_normals3 (new pcl::PointCloud<pcl::Normal> ()); // for sphere
ros::Time declare_types_end = ros::Time::now();
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/*
* Voxel grid Filtering
*/
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// // Create VoxelGrid filtering
// voxel_grid.setInputCloud (cloud);
// voxel_grid.setLeafSize (0.01, 0.01, 0.01);
// voxel_grid.filter (*voxelgrid_filtered);
//
// // Convert the sensor_msgs/PointCloud2 data to pcl/PointCloud
// pcl::fromROSMsg (*voxelgrid_filtered, *transformed_cloud);
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/*
* Passthrough Filtering
*/
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// pass through filter
// pass.setInputCloud (cloud);
// pass.setFilterFieldName ("z");
// pass.setFilterLimits (0, 1.5);
// pass.filter (*cloud_filtered);
//
// // Convert the sensor_msgs/PointCloud2 data to pcl/PointCloud
// pcl::fromROSMsg (*cloud_filtered, *transformed_cloud);
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/*
* Estimate point normals
*/
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// ros::Time estimate_start = ros::Time::now();
//
// // Convert the sensor_msgs/PointCloud2 data to pcl/PointCloud
// pcl::fromROSMsg (*cloud, *transformed_cloud);
//
// // Estimate point normals
// normal_estimation.setSearchMethod (tree);
// normal_estimation.setInputCloud (transformed_cloud);
// normal_estimation.setKSearch (50);
// normal_estimation.compute (*cloud_normals);
//
// ros::Time estimate_end = ros::Time::now();
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/*
* Create and processing the plane extraction
*/
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// ros::Time plane_start = ros::Time::now();
//
// // Create the segmentation object for the planar model and set all the parameters
// segmentation_from_normals.setOptimizeCoefficients (true);
// segmentation_from_normals.setModelType (pcl::SACMODEL_NORMAL_PLANE);
// segmentation_from_normals.setNormalDistanceWeight (0.1);
// segmentation_from_normals.setMethodType (pcl::SAC_RANSAC);
// segmentation_from_normals.setMaxIterations (100);
// segmentation_from_normals.setDistanceThreshold (0.03);
// segmentation_from_normals.setInputCloud (transformed_cloud);
// segmentation_from_normals.setInputNormals (cloud_normals);
//
// // Obtain the plane inliers and coefficients
// segmentation_from_normals.segment (*inliers_plane, *coefficients_plane);
// //std::cerr << "Plane coefficients: " << *coefficients_plane << std::endl;
//
// // Extract the planar inliers from the input cloud
// extract_indices.setInputCloud (transformed_cloud);
// extract_indices.setIndices (inliers_plane);
// extract_indices.setNegative (false);
// extract_indices.filter (*cloud_plane);
//
// pcl::toROSMsg (*cloud_plane, *plane_output_cloud);
// plane_pub.publish(plane_output_cloud);
//
// ros::Time plane_end = ros::Time::now();
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
ros::Time plane_start = ros::Time::now();
pcl::fromROSMsg (*cloud, *transformed_cloud);
seg.setOptimizeCoefficients (true);
seg.setModelType (pcl::SACMODEL_PLANE);
seg.setMethodType (pcl::SAC_RANSAC);
seg.setMaxIterations (1000);
seg.setDistanceThreshold (0.01);
seg.setInputCloud (transformed_cloud);
seg.segment (*inliers_plane, *coefficients_plane);
extract_indices.setInputCloud(transformed_cloud);
extract_indices.setIndices(inliers_plane);
extract_indices.setNegative(false);
extract_indices.filter(*cloud_plane);
pcl::toROSMsg (*cloud_plane, *plane_output_cloud);
plane_pub.publish(plane_output_cloud);
ros::Time plane_end = ros::Time::now();
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/*
* Extract rest plane and passthrough filtering
*/
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
ros::Time rest_pass_start = ros::Time::now();
// Create the filtering object
// Remove the planar inliers, extract the rest
extract_indices.setNegative (true);
extract_indices.filter (*remove_transformed_cloud);
transformed_cloud.swap (remove_transformed_cloud);
// publish result of Removal the planar inliers, extract the rest
pcl::toROSMsg (*transformed_cloud, *rest_output_cloud);
rest_pub.publish(rest_output_cloud);
// Convert the sensor_msgs/PointCloud2 data to pcl/PointCloud
// pcl::fromROSMsg (*rest_output_cloud, *cylinder_cloud);
// pass through filter
pass.setInputCloud (rest_output_cloud);
pass.setFilterFieldName ("z");
pass.setFilterLimits (0, 2.5);
pass.filter (*rest_cloud_filtered);
ros::Time rest_pass_end = ros::Time::now();
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/*
* for cylinder features
*/
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/*
// Convert the sensor_msgs/PointCloud2 data to pcl/PointCloud
pcl::fromROSMsg (*rest_cloud_filtered, *cylinder_cloud);
// Estimate point normals
normal_estimation.setSearchMethod (tree2);
normal_estimation.setInputCloud (cylinder_cloud);
normal_estimation.setKSearch (50);
normal_estimation.compute (*cloud_normals2);
// Create the segmentation object for sphere segmentation and set all the paopennirameters
segmentation_from_normals.setOptimizeCoefficients (true);
segmentation_from_normals.setModelType (pcl::SACMODEL_CYLINDER);
segmentation_from_normals.setMethodType (pcl::SAC_RANSAC);
segmentation_from_normals.setNormalDistanceWeight (0.1);
segmentation_from_normals.setMaxIterations (10000);
segmentation_from_normals.setDistanceThreshold (0.05);
segmentation_from_normals.setRadiusLimits (0, 0.5);
segmentation_from_normals.setInputCloud (cylinder_cloud);
segmentation_from_normals.setInputNormals (cloud_normals2);
// Obtain the sphere inliers and coefficients
segmentation_from_normals.segment (*inliers_cylinder, *coefficients_cylinder);
//std::cerr << "Cylinder coefficients: " << *coefficients_cylinder << std::endl;
// Publish the sphere cloud
extract_indices.setInputCloud (cylinder_cloud);
extract_indices.setIndices (inliers_cylinder);
extract_indices.setNegative (false);
extract_indices.filter (*cylinder_output);
if (cylinder_output->points.empty ())
std::cerr << "Can't find the cylindrical component." << std::endl;
pcl::toROSMsg (*cylinder_output, *cylinder_output_cloud);
cylinder_pub.publish(cylinder_output_cloud);
*/
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/*
* for sphere features
*/
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
ros::Time sphere_start = ros::Time::now();
// Convert the sensor_msgs/PointCloud2 data to pcl/PointCloud
pcl::fromROSMsg (*rest_cloud_filtered, *sphere_cloud);
// Estimate point normals
normal_estimation.setSearchMethod (tree3);
normal_estimation.setInputCloud (sphere_cloud);
normal_estimation.setKSearch (50);
normal_estimation.compute (*cloud_normals3);
// Create the segmentation object for sphere segmentation and set all the paopennirameters
segmentation_from_normals.setOptimizeCoefficients (true);
//segmentation_from_normals.setModelType (pcl::SACMODEL_SPHERE);
segmentation_from_normals.setModelType (pcl::SACMODEL_SPHERE);
segmentation_from_normals.setMethodType (pcl::SAC_RANSAC);
segmentation_from_normals.setNormalDistanceWeight (0.1);
segmentation_from_normals.setMaxIterations (10000);
segmentation_from_normals.setDistanceThreshold (0.05);
segmentation_from_normals.setRadiusLimits (0, 0.2);
segmentation_from_normals.setInputCloud (sphere_cloud);
segmentation_from_normals.setInputNormals (cloud_normals3);
// Obtain the sphere inliers and coefficients
segmentation_from_normals.segment (*inliers_sphere, *coefficients_sphere);
//std::cerr << "Sphere coefficients: " << *coefficients_sphere << std::endl;
// Publish the sphere cloud
extract_indices.setInputCloud (sphere_cloud);
extract_indices.setIndices (inliers_sphere);
extract_indices.setNegative (false);
extract_indices.filter (*sphere_output);
if (sphere_output->points.empty ())
std::cerr << "Can't find the sphere component." << std::endl;
pcl::toROSMsg (*sphere_output, *sphere_output_cloud);
sphere_pub.publish(sphere_output_cloud);
ros::Time sphere_end = ros::Time::now();
std::cout << "cloud size : " << cloud->width * cloud->height << std::endl;
std::cout << "plane size : " << transformed_cloud->width * transformed_cloud->height << std::endl;
//std::cout << "plane size : " << cloud_normals->width * cloud_normals->height << std::endl;
//std::cout << "cylinder size : " << cloud_normals2->width * cloud_normals2->height << std::endl;
std::cout << "sphere size : " << cloud_normals3->width * cloud_normals3->height << std::endl;
ros::Time whole_now = ros::Time::now();
printf("\n");
std::cout << "whole time : " << whole_now - whole_start << " sec" << std::endl;
std::cout << "declare types time : " << declare_types_end - declare_types_start << " sec" << std::endl;
//std::cout << "estimate time : " << estimate_end - estimate_start << " sec" << std::endl;
std::cout << "plane time : " << plane_end - plane_start << " sec" << std::endl;
std::cout << "rest and pass time : " << rest_pass_end - rest_pass_start << " sec" << std::endl;
std::cout << "sphere time : " << sphere_end - sphere_start << " sec" << std::endl;
printf("\n");
}
int
main (int argc, char** argv)
{
// All the objects needed
pcl::PCDReader reader;
pcl::PassThrough<PointT> pass;
pcl::NormalEstimation<PointT, pcl::Normal> ne;
pcl::SACSegmentationFromNormals<PointT, pcl::Normal> seg;
pcl::PCDWriter writer;
pcl::ExtractIndices<PointT> extract;
pcl::ExtractIndices<pcl::Normal> extract_normals;
pcl::search::KdTree<PointT>::Ptr tree (new pcl::search::KdTree<PointT> ());
// Datasets
pcl::PointCloud<PointT>::Ptr cloud (new pcl::PointCloud<PointT>);
pcl::PointCloud<PointT>::Ptr cloud_filtered (new pcl::PointCloud<PointT>);
pcl::PointCloud<pcl::Normal>::Ptr cloud_normals (new pcl::PointCloud<pcl::Normal>);
pcl::PointCloud<PointT>::Ptr cloud_filtered2 (new pcl::PointCloud<PointT>);
pcl::PointCloud<pcl::Normal>::Ptr cloud_normals2 (new pcl::PointCloud<pcl::Normal>);
pcl::ModelCoefficients::Ptr coefficients_plane (new pcl::ModelCoefficients), coefficients_cylinder (new pcl::ModelCoefficients);
pcl::PointIndices::Ptr inliers_plane (new pcl::PointIndices), inliers_cylinder (new pcl::PointIndices);
// Read in the cloud data
reader.read ("table_scene_mug_stereo_textured.pcd", *cloud);
std::cerr << "PointCloud has: " << cloud->points.size () << " data points." << std::endl;
// Build a passthrough filter to remove spurious NaNs
pass.setInputCloud (cloud);
pass.setFilterFieldName ("z");
pass.setFilterLimits (0, 1.5);
pass.filter (*cloud_filtered);
std::cerr << "PointCloud after filtering has: " << cloud_filtered->points.size () << " data points." << std::endl;
// Estimate point normals
ne.setSearchMethod (tree);
ne.setInputCloud (cloud_filtered);
ne.setKSearch (50);
ne.compute (*cloud_normals);
// Create the segmentation object for the planar model and set all the parameters
seg.setOptimizeCoefficients (true);
seg.setModelType (pcl::SACMODEL_NORMAL_PLANE);
seg.setNormalDistanceWeight (0.1);
seg.setMethodType (pcl::SAC_RANSAC);
seg.setMaxIterations (100);
seg.setDistanceThreshold (0.03);
seg.setInputCloud (cloud_filtered);
seg.setInputNormals (cloud_normals);
// Obtain the plane inliers and coefficients
seg.segment (*inliers_plane, *coefficients_plane);
std::cerr << "Plane coefficients: " << *coefficients_plane << std::endl;
// Extract the planar inliers from the input cloud
extract.setInputCloud (cloud_filtered);
extract.setIndices (inliers_plane);
extract.setNegative (false);
// Write the planar inliers to disk
pcl::PointCloud<PointT>::Ptr cloud_plane (new pcl::PointCloud<PointT> ());
extract.filter (*cloud_plane);
std::cerr << "PointCloud representing the planar component: " << cloud_plane->points.size () << " data points." << std::endl;
writer.write ("table_scene_mug_stereo_textured_plane.pcd", *cloud_plane, false);
// Remove the planar inliers, extract the rest
extract.setNegative (true);
extract.filter (*cloud_filtered2);
extract_normals.setNegative (true);
extract_normals.setInputCloud (cloud_normals);
extract_normals.setIndices (inliers_plane);
extract_normals.filter (*cloud_normals2);
// Create the segmentation object for cylinder segmentation and set all the parameters
seg.setOptimizeCoefficients (true);
seg.setModelType (pcl::SACMODEL_CYLINDER);
seg.setMethodType (pcl::SAC_RANSAC);
seg.setNormalDistanceWeight (0.1);
seg.setMaxIterations (10000);
seg.setDistanceThreshold (0.05);
seg.setRadiusLimits (0, 0.1);
seg.setInputCloud (cloud_filtered2);
seg.setInputNormals (cloud_normals2);
// Obtain the cylinder inliers and coefficients
seg.segment (*inliers_cylinder, *coefficients_cylinder);
std::cerr << "Cylinder coefficients: " << *coefficients_cylinder << std::endl;
// Write the cylinder inliers to disk
extract.setInputCloud (cloud_filtered2);
extract.setIndices (inliers_cylinder);
extract.setNegative (false);
pcl::PointCloud<PointT>::Ptr cloud_cylinder (new pcl::PointCloud<PointT> ());
extract.filter (*cloud_cylinder);
if (cloud_cylinder->points.empty ())
std::cerr << "Can't find the cylindrical component." << std::endl;
else
{
std::cerr << "PointCloud representing the cylindrical component: " << cloud_cylinder->points.size () << " data points." << std::endl;
writer.write ("table_scene_mug_stereo_textured_cylinder.pcd", *cloud_cylinder, false);
}
return (0);
}
//input should be point cloud that is amplitude filetered, statistical outlier filtered, voxel filtered, coordinate system should be /map
void
PlaneExtraction::extractPlanes(const pcl::PointCloud<Point>::ConstPtr& pc_in,
std::vector<pcl::PointCloud<Point>, Eigen::aligned_allocator<pcl::PointCloud<Point> > >& v_cloud_hull,
std::vector<std::vector<pcl::Vertices> >& v_hull_polygons,
std::vector<pcl::ModelCoefficients>& v_coefficients_plane)
{
static int ctr=0;
static double time=0;
PrecisionStopWatch t;
t.precisionStart();
std::stringstream ss;
ROS_DEBUG("Extract planes");
ROS_DEBUG("Saving files: %d", save_to_file_);
if(save_to_file_)
{
ss.str("");
ss.clear();
ss << file_path_ << "/planes/pc_" << ctr_ << ".pcd";
pcl::io::savePCDFileASCII (ss.str(), *pc_in);
}
//ROS_INFO("pc_in size: %d" , pc_in->size());
// Extract Eucledian clusters
std::vector<pcl::PointIndices> clusters;
cluster_.setInputCloud (pc_in);
cluster_.extract (clusters);
//extractClusters (pc_in, clusters);
ROS_DEBUG ("Number of clusters found: %d", (int)clusters.size ());
//ROS_INFO("Clustering took %f s", t.precisionStop());
//t.precisionStart();
// Estimate point normals
//normal_estimator_.setInputCloud(pc_in);
//normalEstimator.setNumberOfThreads(4);
//pcl::PointCloud<pcl::Normal>::Ptr cloud_normals(new pcl::PointCloud<pcl::Normal> ());
//normal_estimator_.compute(*cloud_normals);
//ROS_INFO("Normal estimation took %f s", t.precisionStop());
//t.precisionStart();
seg_.setInputCloud (pc_in);
//seg_.setInputNormals (cloud_normals);
proj_.setInputCloud (pc_in);
// Go through all clusters and search for planes
extracted_planes_indices_.clear();
for(unsigned int i = 0; i < clusters.size(); ++i)
{
ROS_DEBUG("Processing cluster no. %u", i);
// Extract cluster points
/*pcl::PointCloud<Point> cluster;
extract_.setInputCloud (pc_in);
extract_.setIndices (boost::make_shared<const pcl::PointIndices> (clusters[i]));
extract_.setNegative (false);
extract_.filter (cluster);
ROS_INFO("Extraction1 took %f s", t.precisionStop());
t.precisionStart();*/
/*pcl::PointCloud<Point>::Ptr cluster_ptr = cluster.makeShared();
if(save_to_file_)
{
ss.str("");
ss.clear();
ss << file_path_ << "/planes/cluster_" << ctr_ << ".pcd";
pcl::io::savePCDFileASCII (ss.str(), cluster);
}*/
// Find plane
pcl::PointIndices::Ptr inliers_plane (new pcl::PointIndices ());
//pcl::PointIndices::Ptr clusters_ptr(&clusters[i]);
//pcl::PointIndices::Ptr consider_in_cluster(new pcl::PointIndices ());
//for(unsigned int idx_ctr=0; idx_ctr < clusters[i].indices.size(); idx_ctr++)
// consider_in_cluster->indices.push_back(clusters[i].indices[idx_ctr]);
int ctr = 0;
// iterate over cluster to find all planes until cluster is too small
while(clusters[i].indices.size() > min_plane_size_)
{
//ROS_INFO("Cluster size: %d", (int)consider_in_cluster->indices.size());
seg_.setIndices(boost::make_shared<const pcl::PointIndices> (clusters[i]));
// Obtain the plane inliers and coefficients
pcl::ModelCoefficients coefficients_plane;
seg_.segment (*inliers_plane, coefficients_plane);
// Evaluate plane
if (coefficients_plane.values.size () <=3)
{
//ROS_INFO("Failed to detect plane in scan, skipping cluster");
break;
}
//TODO: parameter
if ( inliers_plane->indices.size() < min_plane_size_)
{
//std::cout << "Plane coefficients: " << *coefficients_plane << std::endl;
//ROS_INFO("Plane detection has %d inliers, below min threshold of %d, skipping cluster", (int)inliers_plane->indices.size(), 150);
break;
}
bool validPlane = false;
validPlane = isValidPlane (coefficients_plane);
//std::cout << " Plane valid = " << validPlane << std::endl;
if(validPlane)
{
// Extract plane points, only needed for storing bag file
ROS_DEBUG("Plane has %d inliers", (int)inliers_plane->indices.size());
/*pcl::PointCloud<Point> dominant_plane;
extract_.setInputCloud(pc_in);
extract_.setIndices(inliers_plane);
extract_.filter(dominant_plane);
if(save_to_file_)
{
ss.str("");
ss.clear();
ss << file_path_ << "/planes/plane_" << ctr_ << "_" << ctr << ".pcd";
pcl::io::savePCDFileASCII (ss.str(), dominant_plane);
}*/
// Project the model inliers
pcl::PointCloud<Point>::Ptr cloud_projected (new pcl::PointCloud<Point>);
proj_.setModelCoefficients (boost::make_shared<pcl::ModelCoefficients>(coefficients_plane));
proj_.setIndices(inliers_plane);
proj_.filter (*cloud_projected);
if(save_to_file_)
{
ss.str("");
ss.clear();
ss << file_path_ << "/planes/plane_pr_" << ctr_ << "_" << ctr << ".pcd";
pcl::io::savePCDFileASCII (ss.str(), *cloud_projected);
}
std::vector<pcl::PointIndices> plane_clusters;
cluster_plane_.setInputCloud (cloud_projected);
cluster_plane_.extract (plane_clusters);
extract_.setInputCloud(cloud_projected);
/*std::cout << "projected: " << cloud_projected->size() << std::endl;
std::cout << "inliers_plane: " << inliers_plane->indices.size() << std::endl;*/
for(unsigned int j=0; j<plane_clusters.size(); j++)
{
pcl::PointCloud<Point> plane_cluster;
extract_.setIndices(boost::make_shared<const pcl::PointIndices> (plane_clusters[j]));
extract_.filter(plane_cluster);
pcl::PointCloud<Point>::Ptr plane_cluster_ptr = plane_cluster.makeShared();
if(plane_cluster_ptr->size() < min_plane_size_) continue;
//else std::cout << "plane cluster has " << plane_cluster_ptr->size() << " points" << std::endl;
// <evaluation_stuff>
extracted_planes_indices_.push_back(std::vector<int>());
//std::cout << "plane_cluster: " << plane_clusters[j].indices.size() << std::endl;
for (size_t idx = 0; idx < plane_clusters[j].indices.size(); ++idx)
{
//std::cout << plane_clusters[j].indices[idx] << " ";
extracted_planes_indices_.back().push_back(inliers_plane->indices[ plane_clusters[j].indices[idx] ]);
}
// </evaluation_stuff>
// Create a Concave Hull representation of the projected inliers
pcl::PointCloud<Point> cloud_hull;
std::vector< pcl::Vertices > hull_polygons;
chull_.setInputCloud (plane_cluster_ptr);
//TODO: parameter
chull_.reconstruct (cloud_hull, hull_polygons);
/*std::cout << "Hull: " << cloud_hull.size() << ", " << hull_polygons[0].vertices.size()
<< ", "<< plane_cluster_ptr->size() << std::endl;*/
if(hull_polygons.size() > 1)
{
extracted_planes_indices_.pop_back();
continue;
ROS_WARN("Extracted Polygon %d contours, separating ...", hull_polygons.size());
pcl::PointCloud<Point>::Ptr cloud_hull_ptr = cloud_hull.makeShared();
pcl::ExtractIndices<Point> extract_2;
extract_2.setInputCloud(cloud_hull_ptr);
for( unsigned int z=0; z<hull_polygons.size(); z++)
{
//ROS_WARN("\tC%d size: %d", z,hull_polygons[z].vertices.size());
pcl::PointCloud<Point> cloud_hull_2;
std::vector< pcl::Vertices > hull_polygons_2;
pcl::PointIndices hull_poly_indices;
for (unsigned int x=0; x<hull_polygons[z].vertices.size(); x++)
hull_poly_indices.indices.push_back(hull_polygons[z].vertices[x]);
//ROS_INFO("Size indices: %d", hull_poly_indices.indices.size());
extract_2.setIndices(boost::make_shared<const pcl::PointIndices> (hull_poly_indices));
extract_2.filter(cloud_hull_2);
//ROS_INFO("Hull 2 size: %d", cloud_hull_2.size());
pcl::Vertices verts;
for(unsigned int y=0; y<cloud_hull_2.size(); y++)
verts.vertices.push_back(y);
verts.vertices.push_back(0);
//ROS_INFO("Verts size: %d", verts.vertices.size());
hull_polygons_2.push_back(verts);
v_cloud_hull.push_back(cloud_hull_2);
v_hull_polygons.push_back(hull_polygons_2);
v_coefficients_plane.push_back(coefficients_plane);
}
}
else
{
//ROS_INFO("Hull size: %d", cloud_hull.size());
//ROS_INFO("Verts size: %d", hull_polygons[0].vertices.size());
v_cloud_hull.push_back(cloud_hull);
v_hull_polygons.push_back(hull_polygons);
v_coefficients_plane.push_back(coefficients_plane);
}
ROS_DEBUG("v_cloud_hull size: %d", (unsigned int)v_cloud_hull.size());
if(save_to_file_)
{
saveHulls(cloud_hull, hull_polygons, ctr);
}
ctr++;
}
}
// Remove plane inliers from indices vector
for(unsigned int idx_ctr1=0; idx_ctr1 < clusters[i].indices.size(); idx_ctr1++)
{
for(unsigned int idx_ctr2=0; idx_ctr2 < inliers_plane->indices.size(); idx_ctr2++)
{
if(clusters[i].indices[idx_ctr1] == inliers_plane->indices[idx_ctr2])
clusters[i].indices.erase(clusters[i].indices.begin()+idx_ctr1);
}
}
//ctr_++;
}
//ROS_INFO("Plane estimation took %f s", t.precisionStop());
//t.precisionStart();
/*if(clusters[i].indices.size() >0 )
{
if(save_to_file_)
{
ss.str("");
ss.clear();
ss << file_path_ << "/planes/rem_pts_" << ctr_ << "_" << ctr << ".pcd";
if(consider_in_cluster->indices.size() == cluster_ptr->size())
pcl::io::savePCDFileASCII (ss.str(), *cluster_ptr);
else
{
pcl::PointCloud<Point> remaining_pts;
extract_.setInputCloud (cluster_ptr);
extract_.setIndices (consider_in_cluster);
extract_.filter (remaining_pts);
pcl::io::savePCDFileASCII (ss.str(), remaining_pts);
}
}
}*/
ctr_++;
}
double step_time = t.precisionStop();
//ROS_INFO("Plane extraction took %f", step_time);
time += step_time;
//ROS_INFO("[plane extraction] Accumulated time at step %d: %f s", ctr, time);
ctr++;
return;
}
void callback(const sensor_msgs::PointCloud2::ConstPtr& cloud)
{
ros::Time whole_start = ros::Time::now();
ros::Time declare_types_start = ros::Time::now();
// filter
pcl::VoxelGrid<sensor_msgs::PointCloud2> voxel_grid;
pcl::PassThrough<sensor_msgs::PointCloud2> pass;
pcl::ExtractIndices<pcl::PointXYZ> extract_indices;
pcl::ExtractIndices<pcl::PointXYZ> extract_indices2;
// Create the segmentation object
pcl::SACSegmentation<pcl::PointXYZ> seg;
// The plane and sphere coefficients
pcl::ModelCoefficients::Ptr coefficients_plane (new pcl::ModelCoefficients ());
pcl::ModelCoefficients::Ptr coefficients_sphere (new pcl::ModelCoefficients ());
// The plane and sphere inliers
pcl::PointIndices::Ptr inliers_plane (new pcl::PointIndices ());
pcl::PointIndices::Ptr inliers_sphere (new pcl::PointIndices ());
// The point clouds
sensor_msgs::PointCloud2::Ptr passthrough_filtered (new sensor_msgs::PointCloud2);
sensor_msgs::PointCloud2::Ptr plane_seg_output_cloud (new sensor_msgs::PointCloud2);
sensor_msgs::PointCloud2::Ptr sphere_RANSAC_output_cloud (new sensor_msgs::PointCloud2);
sensor_msgs::PointCloud2::Ptr rest_whole_cloud (new sensor_msgs::PointCloud2);
sensor_msgs::PointCloud2::Ptr rest_cloud_filtered (new sensor_msgs::PointCloud2);
sensor_msgs::PointCloud2::Ptr sphere_output_cloud (new sensor_msgs::PointCloud2);
sensor_msgs::PointCloud2::Ptr whole_pc (new sensor_msgs::PointCloud2);
sensor_msgs::PointCloud2::Ptr ball_candidate_output_cloud (new sensor_msgs::PointCloud2);
// The PointCloud
pcl::PointCloud<pcl::PointXYZ>::Ptr plane_seg_cloud (new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr plane_seg_output (new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr remove_plane_cloud (new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr sphere_cloud (new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr sphere_output (new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr sphere_RANSAC_output (new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr remove_false_ball_candidate (new pcl::PointCloud<pcl::PointXYZ>);
std::vector<int> inliers;
ros::Time declare_types_end = ros::Time::now();
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/*
* Pass through Filtering
*/
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
ros::Time pass_start = ros::Time::now();
pass.setInputCloud (cloud);
pass.setFilterFieldName ("z");
pass.setFilterLimits (0, 2.5);
pass.filter (*passthrough_filtered);
passthrough_pub.publish(passthrough_filtered);
ros::Time pass_end = ros::Time::now();
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/*
* for plane features pcl::SACSegmentation
*/
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
ros::Time plane_seg_start = ros::Time::now();
// Convert the sensor_msgs/PointCloud2 data to pcl/PointCloud
pcl::fromROSMsg (*passthrough_filtered, *plane_seg_cloud);
// Optional
seg.setOptimizeCoefficients (true);
seg.setModelType (pcl::SACMODEL_PERPENDICULAR_PLANE);
seg.setMethodType (pcl::SAC_RANSAC);
seg.setAxis(Eigen::Vector3f (0, -1, 0)); // best plane should be perpendicular to z-axis
seg.setMaxIterations (40);
seg.setDistanceThreshold (0.05);
seg.setInputCloud (plane_seg_cloud);
seg.segment (*inliers_plane, *coefficients_plane);
extract_indices.setInputCloud(plane_seg_cloud);
extract_indices.setIndices(inliers_plane);
extract_indices.setNegative(false);
extract_indices.filter(*plane_seg_output);
pcl::toROSMsg (*plane_seg_output, *plane_seg_output_cloud);
plane_pub.publish(plane_seg_output_cloud);
ros::Time plane_seg_end = ros::Time::now();
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/*
* Extract rest plane and passthrough filtering
*/
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
ros::Time rest_pass_start = ros::Time::now();
// Create the filtering object
// Remove the planar inliers, extract the rest
extract_indices.setNegative (true);
extract_indices.filter (*remove_plane_cloud);
plane_seg_cloud.swap (remove_plane_cloud);
// publish result of Removal the planar inliers, extract the rest
pcl::toROSMsg (*plane_seg_cloud, *rest_whole_cloud);
rest_whole_pub.publish(rest_whole_cloud); // 'rest_whole_cloud' substituted whole_pc
ros::Time rest_pass_end = ros::Time::now();
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
ros::Time find_ball_start = ros::Time::now();
int iter = 0;
while(iter < 5)
{
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/*
* for sphere features pcl::SACSegmentation
*/
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// Convert the sensor_msgs/PointCloud2 data to pcl/PointCloud
pcl::fromROSMsg (*rest_whole_cloud, *sphere_cloud);
ros::Time sphere_start = ros::Time::now();
// Optional
seg.setOptimizeCoefficients (false);
// Mandatory
seg.setModelType (pcl::SACMODEL_SPHERE);
seg.setMethodType (pcl::SAC_RANSAC);
seg.setMaxIterations (10000);
seg.setDistanceThreshold (0.03);
seg.setRadiusLimits (0.12, 0.16);
seg.setInputCloud (sphere_cloud);
seg.segment (*inliers_sphere, *coefficients_sphere);
//std::cerr << "Sphere coefficients: " << *coefficients_sphere << std::endl;
if (inliers_sphere->indices.empty())
std::cerr << "[[--Can't find the Sphere Candidate.--]]" << std::endl;
else {
extract_indices.setInputCloud(sphere_cloud);
extract_indices.setIndices(inliers_sphere);
extract_indices.setNegative(false);
extract_indices.filter(*sphere_output);
pcl::toROSMsg (*sphere_output, *sphere_output_cloud);
sphere_seg_pub.publish(sphere_output_cloud); // 'sphere_output_cloud' means ball candidate point cloud
}
ros::Time sphere_end = ros::Time::now();
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/*
* for sphere features pcl::SampleConsensusModelSphere
*/
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
ros::Time sphere_RANSAC_start = ros::Time::now();
// created RandomSampleConsensus object and compute the appropriated model
pcl::SampleConsensusModelSphere<pcl::PointXYZ>::Ptr model_s(new pcl::SampleConsensusModelSphere<pcl::PointXYZ> (sphere_output));
pcl::RandomSampleConsensus<pcl::PointXYZ> ransac (model_s);
ransac.setDistanceThreshold (.01);
ransac.computeModel();
ransac.getInliers(inliers);
// copies all inliers of the model computed to another PointCloud
pcl::copyPointCloud<pcl::PointXYZ>(*sphere_output, inliers, *sphere_RANSAC_output);
pcl::toROSMsg (*sphere_RANSAC_output, *sphere_RANSAC_output_cloud);
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/*
* To discriminate a ball
*/
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
double w = 0;
w = double(sphere_RANSAC_output_cloud->width * sphere_RANSAC_output_cloud->height)
/double(sphere_output_cloud->width * sphere_output_cloud->height);
if (w > 0.9) {
BALL = true;
std::cout << "can find a ball" << std::endl;
true_ball_pub.publish(sphere_RANSAC_output_cloud);
break;
} else {
BALL = false;
std::cout << "can not find a ball" << std::endl;
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/*
* Exclude false ball candidate
*/
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//ros::Time rest_pass_start = ros::Time::now();
// Create the filtering object
// Remove the planar inliers, extract the rest
extract_indices2.setInputCloud(plane_seg_cloud);
extract_indices2.setIndices(inliers_sphere);
extract_indices2.setNegative (true);
extract_indices2.filter (*remove_false_ball_candidate);
sphere_RANSAC_output.swap (remove_false_ball_candidate);
// publish result of Removal the planar inliers, extract the rest
pcl::toROSMsg (*sphere_RANSAC_output, *ball_candidate_output_cloud);
rest_ball_candidate_pub.publish(ball_candidate_output_cloud);
rest_whole_cloud = ball_candidate_output_cloud;
}
iter++;
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
ros::Time sphere_RANSAC_end = ros::Time::now();
}
ros::Time find_ball_end = ros::Time::now();
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
ros::Time whole_end = ros::Time::now();
std::cout << "cloud size : " << cloud->width * cloud->height << std::endl;
std::cout << "plane size : " << plane_seg_output_cloud->width * plane_seg_output_cloud->height << std::endl;
std::cout << "rest size : " << rest_whole_cloud->width * rest_whole_cloud->height << std::endl;
std::cout << "sphere size : " << sphere_output_cloud->width * sphere_output_cloud->height << std::endl;
std::cout << "sphere RANSAC size : " << sphere_RANSAC_output_cloud->width * sphere_RANSAC_output_cloud->height << " " << sphere_RANSAC_output->points.size() << std::endl;
std::cout << "sphereness : " << double(sphere_RANSAC_output_cloud->width * sphere_RANSAC_output_cloud->height)
/double(sphere_output_cloud->width * sphere_output_cloud->height) << std::endl;
std::cout << "model coefficient : " << coefficients_plane->values[0] << " "
<< coefficients_plane->values[1] << " "
<< coefficients_plane->values[2] << " "
<< coefficients_plane->values[3] << " " << std::endl;
printf("\n");
std::cout << "whole time : " << whole_end - whole_start << " sec" << std::endl;
std::cout << "declare types time : " << declare_types_end - declare_types_start << " sec" << std::endl;
std::cout << "passthrough time : " << pass_end - pass_start << " sec" << std::endl;
std::cout << "plane time : " << plane_seg_end - plane_seg_start << " sec" << std::endl;
std::cout << "rest and pass time : " << rest_pass_end - rest_pass_start << " sec" << std::endl;
std::cout << "find a ball time : " << find_ball_end - find_ball_start << " sec" << std::endl;
//std::cout << "sphere time : " << sphere_end - sphere_start << " sec" << std::endl;
//std::cout << "sphere ransac time : " << sphere_RANSAC_end - sphere_RANSAC_start << " sec" << std::endl;
printf("\n----------------------------------------------------------------------------\n");
printf("\n");
}
int
main (int argc, char** argv)
{
// All the objects needed
pcl::PCDReader reader;
pcl::PassThrough<PointT> pass;
pcl::NormalEstimation<PointT, pcl::Normal> ne;
pcl::SACSegmentationFromNormals<PointT, pcl::Normal> seg;
pcl::PCDWriter writer;
pcl::ExtractIndices<PointT> extract;
pcl::ExtractIndices<pcl::Normal> extract_normals;
pcl::search::KdTree<PointT>::Ptr tree (new pcl::search::KdTree<PointT> ());
// Datasets
pcl::PointCloud<PointT>::Ptr cloud (new pcl::PointCloud<PointT>);
pcl::PointCloud<PointT>::Ptr cloud_filtered (new pcl::PointCloud<PointT>);
pcl::PointCloud<pcl::Normal>::Ptr cloud_normals (new pcl::PointCloud<pcl::Normal>);
pcl::PointCloud<PointT>::Ptr cloud_filtered2 (new pcl::PointCloud<PointT>);
pcl::PointCloud<pcl::Normal>::Ptr cloud_normals2 (new pcl::PointCloud<pcl::Normal>);
pcl::ModelCoefficients::Ptr coefficients_plane (new pcl::ModelCoefficients), coefficients_cylinder (new pcl::ModelCoefficients);
pcl::PointIndices::Ptr inliers_plane (new pcl::PointIndices), inliers_cylinder (new pcl::PointIndices);
// Read in the cloud data
std::vector<int> filenames = pcl::console::parse_file_extension_argument (argc, argv, ".pcd");
if (filenames.size() == 0) {
PCL_ERROR ("No pcd files provided");
return 0;
}
if (pcl::io::loadPCDFile<PointType> (argv[filenames[0]], *cloud) == -1) {
PCL_ERROR ("Couldn't read file %s.pcd \n", argv[filenames[0]]);
}
std::cerr << "PointCloud has: " << cloud->points.size () << " data points." << std::endl;
// Estimate point normals
ne.setSearchMethod (tree);
ne.setInputCloud (cloud);
ne.setKSearch (10);
ne.compute (*cloud_normals);
// Create the segmentation object for cylinder segmentation and set all the parameters
seg.setOptimizeCoefficients (true);
seg.setModelType (pcl::SACMODEL_CYLINDER);
seg.setMethodType (pcl::SAC_RANSAC);
seg.setNormalDistanceWeight (0.1);
seg.setMaxIterations (10000);
seg.setDistanceThreshold (0.05);
seg.setRadiusLimits (0, 0.1);
seg.setInputCloud (cloud);
seg.setInputNormals (cloud_normals);
// Obtain the cylinder inliers and coefficients
seg.segment (*inliers_cylinder, *coefficients_cylinder);
std::cerr << "Cylinder coefficients: " << *coefficients_cylinder << std::endl;
// Write the cylinder inliers to disk
extract.setInputCloud (cloud);
extract.setIndices (inliers_cylinder);
extract.setNegative (false);
pcl::PointCloud<PointT>::Ptr cloud_cylinder (new pcl::PointCloud<PointT> ());
extract.filter (*cloud_cylinder);
if (cloud_cylinder->points.empty ())
std::cerr << "Can't find the cylindrical component." << std::endl;
else
{
std::cerr << "PointCloud representing the cylindrical component: " << cloud_cylinder->points.size () << " data points." << std::endl;
writer.write ("table_scene_mug_stereo_textured_cylinder.pcd", *cloud_cylinder, false);
}
return (0);
}
int main (int argc, char** argv)
{
// Zmienne ktore uzywamy do segmentacji, filtracji, odczytu i zapisu pliku.
pcl::PCDReader reader;
pcl::PassThrough<PointT> pass;
pcl::NormalEstimation<PointT, pcl::Normal> ne;
pcl::SACSegmentationFromNormals<PointT, pcl::Normal> seg;
pcl::PCDWriter writer;
pcl::ExtractIndices<PointT> extract;
pcl::ExtractIndices<pcl::Normal> extract_normals;
pcl::search::KdTree<PointT>::Ptr tree (new pcl::search::KdTree<PointT> ());
pcl::visualization::CloudViewer viewer ("Cylinder Model Segmentation");
// Zmienne ktore przechowuja kolejno chmury naszych punktów
pcl::PointCloud<PointT>::Ptr cloud (new pcl::PointCloud<PointT>);
pcl::PointCloud<PointT>::Ptr cloud_filtered (new pcl::PointCloud<PointT>);
pcl::PointCloud<pcl::Normal>::Ptr cloud_normals (new pcl::PointCloud<pcl::Normal>);
pcl::PointCloud<PointT>::Ptr cloud_filtered2 (new pcl::PointCloud<PointT>);
pcl::PointCloud<pcl::Normal>::Ptr cloud_normals2 (new pcl::PointCloud<pcl::Normal>);
pcl::ModelCoefficients::Ptr coefficients_plane (new pcl::ModelCoefficients), coefficients_cylinder (new pcl::ModelCoefficients);
pcl::PointIndices::Ptr inliers_plane (new pcl::PointIndices), inliers_cylinder (new pcl::PointIndices);
// Wczytanie chmury punktów
reader.read ("test_pcd.pcd", *cloud);
std::cerr << "PointCloud has: " << cloud->points.size () << " data points." << std::endl;
// Przefiltorwanie chmury punktów w celu usuniecia "fa³szywych" punktów (NaN)
pass.setInputCloud (cloud);
pass.setFilterFieldName ("z");
pass.setFilterLimits (0, 2.8);
pass.filter (*cloud_filtered);
std::cerr << "PointCloud after filtering has: " << cloud_filtered->points.size () << " data points." << std::endl;
// Obliczenie normalnych dla punktów w chmurze
ne.setSearchMethod (tree);
ne.setInputCloud (cloud_filtered);
ne.setKSearch (50);
ne.compute (*cloud_normals);
// Stworzenie obiektu do segmentacji planarnej, ustawienie odpowiednich parametrów
seg.setOptimizeCoefficients (true);
seg.setModelType (pcl::SACMODEL_NORMAL_PLANE);
seg.setNormalDistanceWeight (0.1);
seg.setMethodType (pcl::SAC_RANSAC);
seg.setMaxIterations (100);
seg.setDistanceThreshold (0.03);
seg.setInputCloud (cloud_filtered);
seg.setInputNormals (cloud_normals);
// Segmentacja..
seg.segment (*inliers_plane, *coefficients_plane);
std::cerr << "Plane coefficients: " << *coefficients_plane << std::endl;
// Wy³uskanie obiektu ktory segmentowalismy
extract.setInputCloud (cloud_filtered);
extract.setIndices (inliers_plane);
extract.setNegative (true);
extract.filter (*cloud_filtered2);
extract_normals.setNegative (true);
extract_normals.setInputCloud (cloud_normals);
extract_normals.setIndices (inliers_plane);
extract_normals.filter (*cloud_normals2);
// Utworzenie obiektu do segmentacji cylindrycznej, ustawienie odpowiednich parametrów
seg.setOptimizeCoefficients (true);
seg.setModelType (pcl::SACMODEL_CYLINDER);
seg.setMethodType (pcl::SAC_RANSAC);
seg.setNormalDistanceWeight (0.1);
seg.setMaxIterations (10000);
// Maksymalna odleglosc pomiedzy punktam a prost¹ ktora wyznaczana jest przy segmentacji moze wynosic 33 cm
seg.setDistanceThreshold (0.33);
// Maksymalny promien cylindra moze miec 40 cm
seg.setRadiusLimits (0, 0.45);
seg.setInputCloud (cloud_filtered2);
seg.setInputNormals (cloud_normals2);
// Segmentacja...
seg.segment (*inliers_cylinder, *coefficients_cylinder);
std::cerr << "Cylinder coefficients: " << *coefficients_cylinder << std::endl;
// Zapisz wynik do pliku na dysku
extract.setInputCloud (cloud_filtered2);
extract.setIndices (inliers_cylinder);
extract.setNegative (false);
pcl::PointCloud<PointT>::Ptr cloud_cylinder (new pcl::PointCloud<PointT> ());
extract.filter (*cloud_cylinder);
if (cloud_cylinder->points.empty ())
std::cerr << "Can't find the cylindrical component." << std::endl;
else
{
std::cerr << "PointCloud representing the cylindrical component: " << cloud_cylinder->points.size () << " data points." << std::endl;
writer.write ("test_pcd_cylinder.pcd", *cloud_cylinder, false);
viewer.showCloud (cloud_cylinder);
while (!viewer.wasStopped ())
{
}
}
return (0);
}