void geonDetector::initialize()
{
totalCount = 0;
pcl::PointCloud<PointT>::Ptr cloud1 (new pcl::PointCloud<PointT>());
pcl::PointCloud<PointT>::Ptr cloud_filtered1 (new pcl::PointCloud<PointT>());
pcl::PointCloud<PointT>::Ptr cloud_filtered21 (new pcl::PointCloud<PointT>());
pcl::PointCloud<pcl::Normal>::Ptr cloud_normals1 (new pcl::PointCloud<pcl::Normal>());
pcl::PointCloud<pcl::Normal>::Ptr cloud_normals21 (new pcl::PointCloud<pcl::Normal>());
pcl::ModelCoefficients::Ptr coefficients1 (new pcl::ModelCoefficients());
pcl::PointIndices::Ptr geonIndices1 (new pcl::PointIndices());
pcl::search::KdTree<PointT>::Ptr tree2 (new pcl::search::KdTree<PointT>());
this->cloud = cloud1;
this->cloud_filtered = cloud_filtered1;
this->cloud_normals = cloud_normals1;
this->cloud_filtered2 = cloud_filtered21;
this->cloud_normals2 = cloud_normals21;
this->coefficients = coefficients1;
this->geonIndices = geonIndices1;
this->tree = tree2;
}
void PlaneFinder::pcCallback(const sensor_msgs::PointCloud2::ConstPtr & pc)
{
pcl::PCLPointCloud2::Ptr pcl_pc(new pcl::PCLPointCloud2);
pcl::PCLPointCloud2::Ptr cloud_filtered (new pcl::PCLPointCloud2 ());
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_filtered1 (new pcl::PointCloud<pcl::PointXYZRGB>());
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_filtered2 (new pcl::PointCloud<pcl::PointXYZRGB>());
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_filtered3 (new pcl::PointCloud<pcl::PointXYZRGB>());
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_f (new pcl::PointCloud<pcl::PointXYZRGB>());
pcl::PCLPointCloud2::Ptr cloud_out (new pcl::PCLPointCloud2 ());
sensor_msgs::PointCloud2 pc2;
double height = -0.5;
// Transformation into PCL type PointCloud2
pcl_conversions::toPCL(*(pc), *(pcl_pc));
//////////////////
// Voxel filter //
//////////////////
pcl::VoxelGrid<pcl::PCLPointCloud2> sor;
sor.setInputCloud (pcl_pc);
sor.setLeafSize (0.01f, 0.01f, 0.01f);
sor.filter (*cloud_filtered);
// Transformation into ROS type
//pcl::toPCLPointCloud2(*(cloud_filtered2), *(cloud_out));
//pcl_conversions::moveFromPCL(*(cloud_filtered), pc2);
//debug2_pub.publish(pc2);
// Transformation into PCL type PointCloud<pcl::PointXYZRGB>
pcl::fromPCLPointCloud2(*(cloud_filtered), *(cloud_filtered1));
if(pcl_ros::transformPointCloud("map", *(cloud_filtered1), *(cloud_filtered2), tf_))
{
////////////////////////
// PassThrough filter //
////////////////////////
pcl::PassThrough<pcl::PointXYZRGB> pass;
pass.setInputCloud (cloud_filtered2);
pass.setFilterFieldName ("x");
pass.setFilterLimits (-0.003, 0.9);
//pass.setFilterLimitsNegative (true);
pass.filter (*cloud_filtered2);
pass.setInputCloud (cloud_filtered2);
pass.setFilterFieldName ("y");
pass.setFilterLimits (-0.5, 0.5);
//pass.setFilterLimitsNegative (true);
pass.filter (*cloud_filtered2);
/////////////////////////
// Planar segmentation //
/////////////////////////
pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients);
pcl::PointIndices::Ptr inliers (new pcl::PointIndices);
// Create the segmentation object
pcl::SACSegmentation<pcl::PointXYZRGB> seg;
// Optional
seg.setOptimizeCoefficients (true);
// Mandatory
seg.setModelType (pcl::SACMODEL_PLANE);
seg.setMethodType (pcl::SAC_RANSAC);
//seg.setMaxIterations (1000);
seg.setDistanceThreshold (0.01);
// Create the filtering object
pcl::ExtractIndices<pcl::PointXYZRGB> extract;
int i = 0, nr_points = (int) cloud_filtered2->points.size ();
// While 50% of the original cloud is still there
while (cloud_filtered2->points.size () > 0.5 * nr_points)
{
// Segment the largest planar component from the remaining cloud
seg.setInputCloud (cloud_filtered2);
seg.segment (*inliers, *coefficients);
if (inliers->indices.size () == 0)
{
std::cerr << "Could not estimate a planar model for the given dataset." << std::endl;
break;
}
if( (fabs(coefficients->values[0]) < 0.02) &&
(fabs(coefficients->values[1]) < 0.02) &&
(fabs(coefficients->values[2]) > 0.9) )
{
std::cerr << "Model coefficients: " << coefficients->values[0] << " "
<< coefficients->values[1] << " "
<< coefficients->values[2] << " "
<< coefficients->values[3] << std::endl;
height = coefficients->values[3];
// Extract the inliers
extract.setInputCloud (cloud_filtered2);
extract.setIndices (inliers);
extract.setNegative (false);
extract.filter (*cloud_filtered3);
// Transformation into ROS type
//pcl::toPCLPointCloud2(*(cloud_filtered3), *(cloud_out));
//pcl_conversions::moveFromPCL(*(cloud_out), pc2);
//debug_pub.publish(pc2);
}
// Create the filtering object
extract.setNegative (true);
extract.filter (*cloud_f);
cloud_filtered2.swap (cloud_f);
i++;
}
/*
pass.setInputCloud (cloud_filtered2);
pass.setFilterFieldName ("z");
pass.setFilterLimits (height, 1.5);
//pass.setFilterLimitsNegative (true);
pass.filter (*cloud_filtered2);
*/
// Transformation into ROS type
//pcl::toPCLPointCloud2(*(cloud_filtered2), *(cloud_out));
//pcl_conversions::moveFromPCL(*(cloud_out), pc2);
//debug_pub.publish(pc2);
/////////////////////////////////
// Statistical Outlier Removal //
/////////////////////////////////
pcl::StatisticalOutlierRemoval<pcl::PointXYZRGB> sor;
sor.setInputCloud (cloud_filtered2);
sor.setMeanK (50);
sor.setStddevMulThresh (1.0);
sor.filter (*cloud_filtered2);
//////////////////////////////////
// Euclidian Cluster Extraction //
//////////////////////////////////
// Creating the KdTree object for the search method of the extraction
pcl::search::KdTree<pcl::PointXYZRGB>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZRGB>);
tree->setInputCloud (cloud_filtered2);
std::vector<pcl::PointIndices> cluster_indices;
pcl::EuclideanClusterExtraction<pcl::PointXYZRGB> ec;
ec.setClusterTolerance (0.02); // 2cm
ec.setMinClusterSize (50);
ec.setMaxClusterSize (5000);
ec.setSearchMethod (tree);
ec.setInputCloud (cloud_filtered2);
ec.extract (cluster_indices);
int j = 0;
for (std::vector<pcl::PointIndices>::const_iterator it = cluster_indices.begin (); it != cluster_indices.end (); ++it)
{
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_cluster1 (new pcl::PointCloud<pcl::PointXYZRGB>);
for (std::vector<int>::const_iterator pit = it->indices.begin (); pit != it->indices.end (); pit++)
cloud_cluster1->points.push_back (cloud_filtered2->points[*pit]);
cloud_cluster1->width = cloud_cluster1->points.size ();
cloud_cluster1->height = 1;
cloud_cluster1->is_dense = true;
cloud_cluster1->header.frame_id = "/map";
if(j == 0) {
// Transformation into ROS type
pcl::toPCLPointCloud2(*(cloud_cluster1), *(cloud_out));
pcl_conversions::moveFromPCL(*(cloud_out), pc2);
debug_pub.publish(pc2);
}
else {
// Transformation into ROS type
pcl::toPCLPointCloud2(*(cloud_cluster1), *(cloud_out));
pcl_conversions::moveFromPCL(*(cloud_out), pc2);
debug2_pub.publish(pc2);
}
j++;
}
// Transformation into ROS type
//pcl::toPCLPointCloud2(*(cloud_filtered2), *(cloud_out));
//pcl_conversions::moveFromPCL(*(cloud_out), pc2);
//debug2_pub.publish(pc2);
//debug2_pub.publish(*pc_map);
}
}
