int main(int argc, char** argv) {
ros::init(argc, argv, "downsample");
ros::NodeHandle n;
downsample_pub = n.advertise<sensor_msgs::PointCloud2>("/downsample", 10);
depth_sub = n.subscribe("/camera/depth/points", 10, depthPointsCallback);
ros::Rate loop_rate(10);
// Set variables for filters
vox.setLeafSize (0.01, 0.01, 0.01);
sor.setMeanK (10);
sor.setStddevMulThresh (0.1);
ec.setClusterTolerance (0.02); // 2cm
ec.setMinClusterSize (20);
ec.setMaxClusterSize (25000);
ec.setSearchMethod (tree);
//seg.setOptimizeCoefficients (true);
// Mandatory
seg.setModelType (pcl::SACMODEL_PLANE);
seg.setMethodType (pcl::SAC_RANSAC);
seg.setDistanceThreshold (0.02);
while(ros::ok()) {
ros::spin();
}
return 0;
}
Extractor() {
tree.reset(new pcl::KdTreeFLANN<Point > ());
cloud.reset(new pcl::PointCloud<Point>);
cloud_filtered.reset(new pcl::PointCloud<Point>);
cloud_normals.reset(new pcl::PointCloud<pcl::Normal>);
coefficients_plane_.reset(new pcl::ModelCoefficients);
coefficients_cylinder_.reset(new pcl::ModelCoefficients);
inliers_plane_.reset(new pcl::PointIndices);
inliers_cylinder_.reset(new pcl::PointIndices);
// Filter Pass
pass.setFilterFieldName("z");
pass.setFilterLimits(-100, 100);
// VoxelGrid for Downsampling
LEAFSIZE = 0.01f;
vg.setLeafSize(LEAFSIZE, LEAFSIZE, LEAFSIZE);
// Any object < CUT_THRESHOLD will be abandoned.
//CUT_THRESHOLD = (int) (LEAFSIZE * LEAFSIZE * 7000000); // 700
CUT_THRESHOLD = 700; //for nonfiltering
// Clustering
cluster_.setClusterTolerance(0.06 * UNIT);
cluster_.setMinClusterSize(50.0);
cluster_.setSearchMethod(clusters_tree_);
// Normals
ne.setSearchMethod(tree);
ne.setKSearch(50); // 50 by default
// plane SAC
seg_plane.setOptimizeCoefficients(true);
seg_plane.setModelType(pcl::SACMODEL_NORMAL_PLANE);
seg_plane.setNormalDistanceWeight(0.1); // 0.1
seg_plane.setMethodType(pcl::SAC_RANSAC);
seg_plane.setMaxIterations(1000); // 10000
seg_plane.setDistanceThreshold(0.05); // 0.03
// cylinder SAC
seg_cylinder.setOptimizeCoefficients(true);
seg_cylinder.setModelType(pcl::SACMODEL_CYLINDER);
seg_cylinder.setMethodType(pcl::SAC_RANSAC);
seg_cylinder.setNormalDistanceWeight(0.1);
seg_cylinder.setMaxIterations(10000);
seg_cylinder.setDistanceThreshold(0.02); // 0.05
seg_cylinder.setRadiusLimits(0.02, 0.07); // [0, 0.1]
}
bool compute_object(const int i, std::vector<pcl::PointIndices > &segments) {
// Call get_plane, get_cylinder, get_sphere
// float plane_score = compute_plane(); // plane_inliers_ has the pc.
// float cylinder_score = compute_cylinder(); // cylinder_inliers_ has the pc
// Select the lowest score and store it in object_inliers_.
// inliers_object_ = (plane_score > cylinder_score ? inliers_plane_ : inliers_cylinder_);
inliers_object_ = inliers_plane_;
// inliers_object_ = inliers_cylinder_;
// Test if the point cloud is too small
if (inliers_object_->indices.size() < CUT_THRESHOLD) {
std::cerr << "object inliers has " << inliers_object_->indices.size()
<< " < " << CUT_THRESHOLD << " points. Aborting..." << std::endl;
return false;
}
/*
// Debugging info
pcl::PointCloud<Point>::Ptr best_object_(new pcl::PointCloud<Point>);
pcl::copyPointCloud(*cloud_filtered, *inliers_object_, *best_object_);
std::stringstream debug_s0;
debug_s0 << "no_cluster_object_" << i << ".pcd";
pcl::io::savePCDFile(debug_s0.str(), *best_object_);
*/
// Euclidean Clustering
std::vector<pcl::PointIndices> clusters;
cluster_.setInputCloud(cloud_filtered);
cluster_.setIndices(inliers_object_);
cluster_.extract(clusters);
std::cout << "Number of Euclidian clusters found: " << clusters.size() << std::endl;
// Check if there is no clusters
if (clusters.size() == 0) {
std::cerr << "No clusters found. Aborting..." << std::endl;
return false;
}
main_cluster_.reset(new pcl::PointIndices(clusters.at(0)));
// Extract the inliers from the input cloud
extract.setInputCloud(cloud_filtered);
extract.setIndices(main_cluster_);
extract.setNegative(false);
pcl::PointCloud<Point>::Ptr cloud_plane(new pcl::PointCloud<Point > ());
extract.filter(*cloud_plane);
// Test if cluster[0] is too small
if (main_cluster_->indices.size() < CUT_THRESHOLD) {
std::cerr << "object cluster[0] has " << main_cluster_->indices.size()
<< " < " << CUT_THRESHOLD << " points. Not writing to disk..." << std::endl;
} else {
// Write the inliers to disk
std::stringstream debug_s;
if (inliers_object_ == inliers_cylinder_) {
debug_s << "cylinder_object_" << i << ".pcd";
} else if (inliers_object_ == inliers_plane_) {
debug_s << "plane_object_" << i << ".pcd";
} else {
debug_s << "unknown_object_" << i << ".pcd";
}
pcl::io::savePCDFile(debug_s.str(), *cloud_plane);
std::cout << "PointCloud representing the extracted component: " <<
cloud_plane->points.size() << " data points." << std::endl;
// Insert the points to segments for cfg3D
pcl::PointIndices::Ptr originalIndices(new pcl::PointIndices());
originalIndices->indices.resize(cloud_plane->size());
for (unsigned int j = 0; j < originalIndices->indices.size(); j++) {
originalIndices->indices.at(j) = cloud_plane->points.at(j).rgba;
}
segments.push_back(*originalIndices);
// Debugging for cfg3D
pcl::PointCloud<Point>::Ptr cfg_segment_cloud(new pcl::PointCloud<Point > ());
pcl::ExtractIndices<Point> cfg_extract;
cfg_extract.setNegative(false);
cfg_extract.setInputCloud(cloud);
cfg_extract.setIndices(originalIndices);
cfg_extract.filter(*cfg_segment_cloud);
/*
cfg_segment_cloud->points.resize(originalIndices->indices.size());
for (unsigned int j = 0; j < originalIndices->indices.size(); j++) {
unsigned int index = originalIndices->indices.at(j);
cfg_segment_cloud->points.at(j).x = cloud->points.at(index).x;
cfg_segment_cloud->points.at(j).y = cloud->points.at(index).y;
cfg_segment_cloud->points.at(j).z = cloud->points.at(index).z;
}
*/
std::stringstream cfg_debug_s;
cfg_debug_s << "cfg_debug_segment_" << i << ".pcd";
pcl::io::savePCDFile(cfg_debug_s.str(), *cfg_segment_cloud);
}
// Remove cluster[0], update cloud_filtered and cloud_normals
extract.setNegative(true);
extract.filter(*cloud_filtered);
extract_normals.setNegative(true);
extract_normals.setInputCloud(cloud_normals);
extract_normals.setIndices(main_cluster_);
extract_normals.filter(*cloud_normals);
return true;
}
void depthPointsCallback(const sensor_msgs::PointCloud2ConstPtr& cloud_msg) {
// Instantiate various clouds
pcl::PCLPointCloud2* cloud_intermediate = new pcl::PCLPointCloud2;
pcl::PCLPointCloud2ConstPtr cloudPtr(cloud_intermediate);
pcl::PointCloud<pcl::PointXYZ> cloud;
// Convert to PCL data type
pcl_conversions::toPCL(*cloud_msg, *cloud_intermediate);
// Apply Voxel Filter on PCLPointCloud2
vox.setInputCloud (cloudPtr);
vox.setInputCloud (cloudPtr);
vox.filter (*cloud_intermediate);
// Convert PCL::PointCloud2 to PCL::PointCloud<PointXYZ>
pcl::fromPCLPointCloud2(*cloud_intermediate, cloud);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_p = cloud.makeShared();
// Apply Passthrough Filter
pass.setFilterFieldName ("z");
pass.setFilterLimits (0.3, 1);
pass.setInputCloud (cloud_p);
//pass.setFilterLimitsNegative (true);
pass.filter (*cloud_p);
// Apply Passthrough Filter
pass.setFilterFieldName ("x");
pass.setFilterLimits (-0.2, 0.2);
pass.setInputCloud (cloud_p);
pass.setFilterFieldName ("z");
pass.setFilterLimits (0.0, 3.0);
//pass.setFilterLimitsNegative (true);
pass.filter (*cloud_p);
// Apply Statistical Noise Filter
sor.setInputCloud (cloud_p);
sor.filter (*cloud_p);
// Planar segmentation: Remove large planes? Or extract floor?
pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients);
pcl::PointIndices::Ptr inliers (new pcl::PointIndices);
int nr_points = (int) cloud_p->points.size ();
Eigen::Vector3f lol (0, 1, 0);
seg.setEpsAngle( 30.0f * (3.14f/180.0f) );
seg.setAxis(lol);
//while(cloud_p->points.size () > 0.2 * nr_points) {
sor.setInputCloud (cloud_p);
sor.filter (*cloud_p);
// Create the segmentation object
pcl::SACSegmentation<pcl::PointXYZ> seg;
// Optional
seg.setOptimizeCoefficients (true);
// Mandatory
seg.setModelType (pcl::SACMODEL_PLANE);
seg.setMethodType (pcl::SAC_RANSAC);
seg.setDistanceThreshold (0.01);
seg.setInputCloud (cloud_p);
seg.segment (*inliers, *coefficients);
if (inliers->indices.size () == 0)
{
//break;
}
else {
/*std::cout << "Model coefficients: " << coefficients->values[0] << " "
<< coefficients->values[1] << " "
<< coefficients->values[2] << " "
<< coefficients->values[3] << "\n";*/
pcl::ExtractIndices<pcl::PointXYZ> extract;
extract.setInputCloud(cloud_p);
extract.setIndices(inliers);
extract.setNegative(true);
extract.filter(*cloud_p);
}
//}
Eigen::Vector3f lol_p (0.5f, 0, 0.5f);
seg.setAxis(lol_p);
while(cloud_p->points.size () > 0.1 * nr_points) {
seg.setInputCloud (cloud_p);
seg.segment (*inliers, *coefficients);
if (inliers->indices.size () == 0)
{
break;
}
else {
/*std::cout << "Model coefficients: " << coefficients->values[0] << " "
<< coefficients->values[1] << " "
<< coefficients->values[2] << " "
<< coefficients->values[3] << "\n";*/
pcl::ExtractIndices<pcl::PointXYZ> extract;
extract.setInputCloud(cloud_p);
extract.setIndices(inliers);
extract.setNegative(true);
extract.filter(*cloud_p);
}
}
// Apply Statistical Noise Filter
sor.setInputCloud (cloud_p);
sor.filter (*cloud_p);
if(cloud_p->points.size() > 0) {
std::vector<pcl::PointIndices> cluster_indices;
tree->setInputCloud (cloud_p);
ec.setInputCloud (cloud_p);
ec.extract (cluster_indices);
std::cout << "Clusters detected: " << cluster_indices.size() << "\n";
// Convert to ROS data type
}
// Convert to ROS data type
sensor_msgs::PointCloud2 output;
pcl::toROSMsg(*cloud_p, output);
// Publish the data
downsample_pub.publish(output);
}
bool compute_object(const int i) {
// Call get_plane, get_cylinder, get_sphere
float plane_score = compute_plane(); // plane_inliers_ has the pc.
float cylinder_score = compute_cylinder(); // cylinder_inliers_ has the pc
// Select the lowest score and store it in object_inliers_.
inliers_object_ = (plane_score > cylinder_score ? inliers_plane_ : inliers_cylinder_);
// inliers_object_ = inliers_plane_;
// inliers_object_ = inliers_cylinder_;
// Test if the point cloud is too small
if (inliers_object_->indices.size() < CUT_THRESHOLD) {
std::cerr << "object inliers has "<< inliers_object_->indices.size()
<< " < " << CUT_THRESHOLD <<" points. Aborting..." << std::endl;
return false;
}
/*
// Debugging info
pcl::PointCloud<Point>::Ptr best_object_(new pcl::PointCloud<Point>);
pcl::copyPointCloud(*cloud_filtered, *inliers_object_, *best_object_);
std::stringstream debug_s0;
debug_s0 << "no_cluster_object_" << i << ".pcd";
pcl::io::savePCDFile(debug_s0.str(), *best_object_);
*/
// Euclidean Clustering
std::vector<pcl::PointIndices> clusters;
cluster_.setInputCloud(cloud_filtered);
cluster_.setIndices(inliers_object_);
cluster_.extract(clusters);
std::cout << "Number of Euclidian clusters found: " << clusters.size() << std::endl;
// Check if there is no clusters
if (clusters.size() == 0) {
std::cerr << "No clusters found. Aborting..." << std::endl;
return false;
}
main_cluster_.reset(new pcl::PointIndices(clusters.at(0)));
// Extract the inliers from the input cloud
extract.setInputCloud(cloud_filtered);
extract.setIndices(main_cluster_);
extract.setNegative(false);
pcl::PointCloud<Point>::Ptr cloud_plane(new pcl::PointCloud<Point > ());
extract.filter(*cloud_plane);
// Test if cluster[0] is too small
if (main_cluster_->indices.size() < CUT_THRESHOLD) {
std::cerr << "object cluster[0] has "<< main_cluster_->indices.size()
<< " < " << CUT_THRESHOLD <<" points. Not writing to disk..." << std::endl;
} else {
// Write the inliers to disk
std::stringstream debug_s;
if (inliers_object_ == inliers_cylinder_) {
debug_s << "cylinder_object_" << i << ".pcd";
} else if (inliers_object_ == inliers_plane_) {
debug_s << "plane_object_" << i << ".pcd";
} else {
debug_s << "unknown_object_" << i << ".pcd";
}
pcl::io::savePCDFile(debug_s.str(), *cloud_plane);
std::cout << "PointCloud representing the extracted component: " <<
cloud_plane->points.size() << " data points." << std::endl;
}
// Remove cluster[0], update cloud_filtered and cloud_normals
extract.setNegative(true);
extract.filter(*cloud_filtered);
extract_normals.setNegative(true);
extract_normals.setInputCloud(cloud_normals);
extract_normals.setIndices(main_cluster_);
extract_normals.filter(*cloud_normals);
return true;
}
