bool process_ros(duplo_v0::Process_PCD::Request &req,
duplo_v0::Process_PCD::Response &res)
{
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudz(new pcl::PointCloud<pcl::PointXYZRGB>); //Filtered cloud
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_inliers(new pcl::PointCloud<pcl::PointXYZRGB>); //Inliers after removing outliers
pcl::PointCloud<pcl::PointXYZRGB>::Ptr planar_cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr object_cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cluster1(new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::fromROSMsg(req.pcd_in,*cloud);
/****************** Filter out the non-table points ******************/
if (pass_through(cloud,cloudz) != 0)
{
std::cerr << "Pass-through filter error" << std::endl;
return false;
}
/* ********* Segmentation of the cloud into table and object clouds **********/
planar_seg(cloudz,planar_cloud,object_cloud,"table_try.pcd","object_try.pcd");
/********************** Cluster objects based on Euclidean distance **********/
//vector<double> volumes = cluster(object_cloud);
int num_clusters_found = cluster(object_cloud);
if (num_clusters_found == 0)
the_chosen_one.data.clear();
res.n_clusters = num_clusters_found;
processed_pointcloud = true;
return true;
}
void RANSACSphere::ransac() {
pcl::PointCloud<pcl::PointXYZ> cloud = in_pcl.read();
pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients);
pcl::PointIndices::Ptr inliers (new pcl::PointIndices);
// Create the segmentation object
pcl::SACSegmentation<pcl::PointXYZ> seg;
// Optional
seg.setOptimizeCoefficients (true);
// Mandatory
seg.setModelType (pcl::SACMODEL_SPHERE);
seg.setMethodType (pcl::SAC_RANSAC);
seg.setDistanceThreshold (0.01);
seg.setInputCloud (cloud.makeShared ());
seg.segment (*inliers, *coefficients);
if (inliers->indices.size () == 0)
{
//PCL_ERROR ("Could not estimate a planar model for the given dataset.");
cout<<"Could not estimate a planar model for the given dataset."<<endl;
}
//info
std::cout << "Model coefficients: " << coefficients->values[0] << " "
<< coefficients->values[1] << " "
<< coefficients->values[2] << " "
<< coefficients->values[3] << std::endl;
std::cout << "Model inliers: " << inliers->indices.size () << std::endl;
for (size_t i = 0; i < inliers->indices.size (); ++i)
std::cout << inliers->indices[i] << " " << cloud.points[inliers->indices[i]].x << " "
<< cloud.points[inliers->indices[i]].y << " "
<< cloud.points[inliers->indices[i]].z << std::endl;
//////////////////////////
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_inliers (new pcl::PointCloud<pcl::PointXYZ> ());
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_outliers (new pcl::PointCloud<pcl::PointXYZ> ());
pcl::ExtractIndices<pcl::PointXYZ> extract;
extract.setInputCloud (cloud.makeShared());
extract.setIndices (inliers);
extract.setNegative (false);
extract.filter (*cloud_inliers);
//std::cout << "PointCloud representing the planar component: " << cloud_inliers->points.size () << " data points." << std::endl;
// Remove the planar inliers, extract the rest
extract.setNegative (true);
extract.filter (*cloud_outliers);
//*cloud_filtered = *cloud_f;
out_outliers.write(cloud_outliers);
out_inliers.write(cloud_inliers);
}
bool DataProcessor::processDataCallback(duplo_v1::Process_PCD::Request &req,
duplo_v1::Process_PCD::Response &res)
{
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudz(new pcl::PointCloud<pcl::PointXYZRGB>); //Filtered cloud
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_inliers(new pcl::PointCloud<pcl::PointXYZRGB>); //Inliers after removing outliers
pcl::PointCloud<pcl::PointXYZRGB>::Ptr everything_else(new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::fromROSMsg(req.pcd_in,*cloud);
ROS_INFO("Inside process.");
get_object_cloud(cloud,planar_cloud_,object_cloud_);
processed_pointcloud_ = true;
res.done = true;
process();
ROS_INFO("\n \n");
ros::Duration(2).sleep();
return true;
}
