void segment_region_growing(pcl::PointCloud<pcl::PointXYZRGBA>::Ptr cloud, int index, pcl::PointCloud<pcl::PointXYZRGBA>::Ptr output_cloud) {
pcl::search::Search<pcl::PointXYZRGBA>::Ptr tree = boost::shared_ptr< pcl::search::Search<pcl::PointXYZRGBA> > (new pcl::search::KdTree<pcl::PointXYZRGBA>);
pcl::PointCloud <pcl::Normal>::Ptr normals (new pcl::PointCloud <pcl::Normal>);
pcl::NormalEstimation<pcl::PointXYZRGBA, pcl::Normal> normal_estimator;
normal_estimator.setSearchMethod (tree);
normal_estimator.setInputCloud (cloud);
normal_estimator.setKSearch (50);
normal_estimator.compute (*normals);
pcl::IndicesPtr indices (new std::vector <int>);
pcl::PassThrough<pcl::PointXYZRGBA> pass;
pass.setInputCloud (cloud);
pass.setFilterFieldName ("z"); // Filter the huge quantity of points at the origin that mean nothing
pass.setFilterLimits (-0.01, 0.01);
// pass.setFilterFieldName ("y");
// pass.setFilterLimits (-0.1, 0.1);
// pass.setFilterFieldName ("x");
// pass.setFilterLimits (-0.1, 0.1);
pass.setFilterLimitsNegative(true);
pass.filter (*indices);
pcl::PointCloud <pcl::PointXYZRGBA>::Ptr temp_visualization (new pcl::PointCloud <pcl::PointXYZRGBA>);
pcl::PointIndices filter_pts;
pcl::PointIndices::Ptr cluster_ptr(new pcl::PointIndices());
cluster_ptr->indices = *indices;
extract_indices(cloud, temp_visualization, *cluster_ptr);
pcl::RegionGrowing<pcl::PointXYZRGBA, pcl::Normal> reg;
reg.setMinClusterSize (10);
reg.setMaxClusterSize (100000);
reg.setSearchMethod (tree);
reg.setNumberOfNeighbours (30);
reg.setIndices(indices);
reg.setInputCloud (cloud);
reg.setInputNormals (normals);
// reg.setSmoothnessThreshold (3.0 / 180.0 * M_PI);
reg.setSmoothnessThreshold (15.0 / 180.0 * M_PI); // More relaxed angle constraints
reg.setCurvatureThreshold (1.0);
// std::vector <pcl::PointIndices> clusters;
// reg.extract (clusters);
/*
pcl::PointCloud <pcl::PointXYZRGB>::Ptr colored_cloud = reg.getColoredCloud();
pcl::PointCloud <pcl::PointXYZRGBA>::Ptr colored_cloud_rgba(new pcl::PointCloud <pcl::PointXYZRGBA>);
pcl::copyPointCloud(*colored_cloud, *colored_cloud_rgba);
visualize(colored_cloud_rgba, "colored segmentation");
*/
pcl::PointIndices::Ptr cluster(new pcl::PointIndices());
reg.getSegmentFromPoint(index, *cluster);
extract_indices(cloud, output_cloud, *cluster);
}
template<typename Point> void
TableObjectCluster<Point>::extractClusters(
const pcl::PointIndices::Ptr& pc_roi,
std::vector<PointCloudPtr>& object_clusters,
std::vector<pcl::PointIndices>& object_cluster_indices)
{
//ROS_INFO("input: %d,%d", input_->width, input_->height);
#ifdef PCL_VERSION_COMPARE //fuerte
//typename pcl::search::OrganizedNeighbor<Point>::Ptr clusters_tree( new pcl::search::OrganizedNeighbor<Point>());
typename pcl::search::KdTree<Point>::Ptr clusters_tree (new pcl::search::KdTree<Point>());
#else //electric
typename pcl::KdTreeFLANN<Point>::Ptr clusters_tree (new pcl::KdTreeFLANN<Point> ());
#endif
pcl::EuclideanClusterExtraction<Point> cluster_obj;
cluster_obj.setClusterTolerance (cluster_tolerance_);
cluster_obj.setMinClusterSize (min_cluster_size_);
cluster_obj.setInputCloud (input_);
cluster_obj.setIndices(pc_roi);
cluster_obj.setSearchMethod (clusters_tree);
cluster_obj.extract (object_cluster_indices);
pcl::ExtractIndices<Point> ei;
ei.setInputCloud(input_);
for(unsigned int i = 0; i < object_cluster_indices.size(); ++i)
{
PointCloudPtr cluster_ptr(new pcl::PointCloud<Point>);
boost::shared_ptr<pcl::PointIndices> ind_ptr(&object_cluster_indices[i], null_deleter());
ei.setIndices(ind_ptr);
ei.filter(*cluster_ptr);
object_clusters.push_back(cluster_ptr);
}
}
void extract_indices(pcl::PointCloud<pcl::PointXYZRGBA>::Ptr input_cloud, pcl::PointCloud<pcl::PointXYZRGBA>::Ptr output_cloud, pcl::PointIndices &cluster) {
pcl::ExtractIndices<pcl::PointXYZRGBA> extract;
extract.setInputCloud(input_cloud);
pcl::PointIndices::Ptr cluster_ptr(new pcl::PointIndices(cluster));
extract.setIndices(cluster_ptr);
extract.setNegative(false);
extract.filter(*output_cloud);
}
template<typename Point> void
TableObjectCluster<Point>::calculateBoundingBoxesOld(pcl::PointIndices::Ptr& pc_roi,
std::vector<PointCloudPtr >& object_clusters,
std::vector<pcl::PointCloud<pcl::PointXYZ> >& bounding_boxes)
{
ROS_INFO("roi: %d", pc_roi->indices.size());
#ifdef PCL_VERSION_COMPARE //fuerte
typename pcl::search::KdTree<Point>::Ptr clusters_tree (new pcl::search::KdTree<Point>());
//typename pcl::search::OrganizedNeighbor<Point>::Ptr clusters_tree( new pcl::search::OrganizedNeighbor<Point>());
#else //electric
typename pcl::KdTreeFLANN<Point>::Ptr clusters_tree (new pcl::KdTreeFLANN<Point> ());
#endif
pcl::EuclideanClusterExtraction<Point> cluster_obj;
// Table clustering parameters
cluster_obj.setClusterTolerance (cluster_tolerance_);
cluster_obj.setMinClusterSize (min_cluster_size_);
cluster_obj.setInputCloud (input_);
cluster_obj.setIndices(pc_roi);
//cluster_obj.setSearchMethod (clusters_tree);
std::vector<pcl::PointIndices> object_cluster_indices;
cluster_obj.extract (object_cluster_indices);
pcl::ExtractIndices<Point> ei;
ei.setInputCloud(input_);
for(unsigned int i = 0; i < object_cluster_indices.size(); ++i)
{
boost::shared_ptr<pcl::PointIndices> ind_ptr(&object_cluster_indices[i], null_deleter());
ei.setIndices(ind_ptr);
PointCloudPtr cluster_ptr(new pcl::PointCloud<Point>);
ei.filter(*cluster_ptr);
object_clusters.push_back(cluster_ptr);
pcl::PointCloud<pcl::PointXYZ> bb;
Eigen::Vector4f min_pt, max_pt;
pcl::getMinMax3D(*input_, object_cluster_indices[i], min_pt, max_pt);
//if(fabs(max_pt(2)-min_pt(2))<0.03) continue;
pcl::PointXYZ p;
p.x = min_pt(0);
p.y = min_pt(1);
p.z = min_pt(2);
bb.push_back(p);
p.x = max_pt(0);
p.y = max_pt(1);
p.z = max_pt(2);
bb.push_back(p);
bounding_boxes.push_back(bb);
}
}
