template <typename PointT> void
pcl16::EuclideanClusterExtraction<PointT>::extract (std::vector<PointIndices> &clusters)
{
if (!initCompute () ||
(input_ != 0 && input_->points.empty ()) ||
(indices_ != 0 && indices_->empty ()))
{
clusters.clear ();
return;
}
// Initialize the spatial locator
if (!tree_)
{
if (input_->isOrganized ())
tree_.reset (new pcl16::search::OrganizedNeighbor<PointT> ());
else
tree_.reset (new pcl16::search::KdTree<PointT> (false));
}
// Send the input dataset to the spatial locator
tree_->setInputCloud (input_, indices_);
extractEuclideanClusters (*input_, *indices_, tree_, static_cast<float> (cluster_tolerance_), clusters, min_pts_per_cluster_, max_pts_per_cluster_);
//tree_->setInputCloud (input_);
//extractEuclideanClusters (*input_, tree_, cluster_tolerance_, clusters, min_pts_per_cluster_, max_pts_per_cluster_);
// Sort the clusters based on their size (largest one first)
std::sort (clusters.rbegin (), clusters.rend (), comparePointClusters);
deinitCompute ();
}
void FeatSegment::segment(const PointCloud<PointXYZRGB >::Ptr cloud, PointCloud<PointXYZRGB >::Ptr outcloud)
{
// PARAM - Lots of them
int min_pts_per_cluster = 10;
int max_pts_per_cluster = INT_MAX; //3000000;
assert(max_pts_per_cluster > 3000000); // Avoid overflow
int number_neighbours = 50; // PARAM
float radius = 0.025; // 0.025 PARAM
float angle = 0.52; // PARAM
// Required KdTrees
pcl::search::KdTree<PointXYZRGB >::Ptr NormalsTree(new pcl::search::KdTree<PointXYZRGB >);
pcl::search::KdTree<PointXYZRGB >::Ptr ClustersTree(new pcl::search::KdTree<PointXYZRGB >);
PointCloud<Normal >::Ptr CloudNormals(new PointCloud<Normal >);
NormalEstimation<PointXYZRGB, Normal > NormalsFinder;
std::vector<PointIndices > clusters;
ClustersTree->setInputCloud(cloud);
// Set normal estimation parameters
NormalsFinder.setKSearch(number_neighbours);
NormalsFinder.setSearchMethod(NormalsTree);
NormalsFinder.setInputCloud(cloud);
NormalsFinder.compute(*CloudNormals);
extractEuclideanClusters(cloud, CloudNormals, ClustersTree, radius, clusters, angle, min_pts_per_cluster, max_pts_per_cluster);
fprintf(stderr, "Number of clusters found matching the given constraints: %d.", (int)clusters.size ());
std::ofstream FileStr2;
FileStr2.open("live_segments.txt", ios::app); // NOTE: Don't add the ios:trunc flag here!
// Copy to clusters to segments
for (size_t i = 0; i < clusters.size (); ++i)
{
if(FileStr2.is_open())
{
for(int j = 0; j < clusters[i].indices.size(); ++j)
{
if(j == clusters[i].indices.size() - 1)
{
FileStr2 << clusters[i].indices.at(j) << endl;
continue; // Also break;
}
FileStr2 << clusters[i].indices.at(j) << " ";
}
FeatPointCloud * Segment = new FeatPointCloud(m_SceneCloud, clusters[i].indices, m_ConfigFName);
m_Segments.push_back(Segment);
}
else
cout << "Failed to open file.\n";
}
FileStr2.close();
}
