Skip to content

Modularize euclidean clustering #4268

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
wants to merge 19 commits into
base: master
Choose a base branch
from
Open

Modularize euclidean clustering #4268

Show file tree
Hide file tree
Changes from 7 commits
Commits
Show all changes
19 commits
Select commit Hold shift + click to select a range
d54bb58
Modularize extractEuclideanClusters
shrijitsingh99 Jul 13, 2020
1b7a763
Switch to CloudIterator
shrijitsingh99 Jul 13, 2020
a985a57
Switch to Indices & fix noop lambda
shrijitsingh99 Jul 13, 2020
242b8f5
Fix bugs
shrijitsingh99 Jul 13, 2020
0353506
Concentrate checks into main overload
shrijitsingh99 Jul 13, 2020
d4f96b1
Replace '.points[' with '['
shrijitsingh99 Jul 13, 2020
c7d6838
Fix bugs
shrijitsingh99 Jul 13, 2020
4282d2f
Update filter lambda name
shrijitsingh99 Jul 13, 2020
fbf3b4b
Cleanup
shrijitsingh99 Jul 13, 2020
20ac71c
Use `pcl::utils::ignore` to avoid unused variable warnings
shrijitsingh99 Jul 13, 2020
795b545
Move function implementation to `.h` to avoid template instantiation
shrijitsingh99 Jul 13, 2020
bd3bc6b
Switch to `search::Kdtree` instead of `Kdtree`
shrijitsingh99 Jul 13, 2020
08b5155
Use eigen for dot product
shrijitsingh99 Jul 13, 2020
7d4f504
Rename lambdas to be more descriptive
shrijitsingh99 Jul 13, 2020
a2bcb28
Split clustering into 3 parts for cloud, indices and common cloud ite…
shrijitsingh99 Jul 13, 2020
95204a3
Update error messages to be technically accurate
shrijitsingh99 Jul 13, 2020
0128a1f
Add docstring
shrijitsingh99 Jul 14, 2020
228fae4
Add functor checks and improve docstring
shrijitsingh99 Jul 16, 2020
b6283ca
Fix nullptr bug
shrijitsingh99 Jul 16, 2020
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
175 changes: 35 additions & 140 deletions segmentation/include/pcl/segmentation/extract_clusters.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -45,6 +45,13 @@

namespace pcl
{
template <typename PointT, typename FunctorT> void
extractEuclideanClusters (
const PointCloud<PointT> &cloud, const Indices &indices,
FunctorT filter, const typename search::Search<PointT>::Ptr &tree,
float tolerance, std::vector<PointIndices> &clusters,
unsigned int min_pts_per_cluster = 1, unsigned int max_pts_per_cluster = std::numeric_limits<int>::max());

//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/** \brief Decompose a region of space into clusters based on the Euclidean distance between points
* \param cloud the point cloud message
Expand All @@ -56,7 +63,7 @@ namespace pcl
* \param max_pts_per_cluster maximum number of points that a cluster may contain (default: max int)
* \ingroup segmentation
*/
template <typename PointT> void
template <typename PointT> void
extractEuclideanClusters (
const PointCloud<PointT> &cloud, const typename search::Search<PointT>::Ptr &tree,
float tolerance, std::vector<PointIndices> &clusters,
Expand All @@ -76,7 +83,7 @@ namespace pcl
*/
template <typename PointT> void
extractEuclideanClusters (
const PointCloud<PointT> &cloud, const std::vector<int> &indices,
const PointCloud<PointT> &cloud, const Indices &indices,
const typename search::Search<PointT>::Ptr &tree, float tolerance, std::vector<PointIndices> &clusters,
unsigned int min_pts_per_cluster = 1, unsigned int max_pts_per_cluster = (std::numeric_limits<int>::max) ());

Expand All @@ -102,79 +109,24 @@ namespace pcl
unsigned int min_pts_per_cluster = 1,
unsigned int max_pts_per_cluster = (std::numeric_limits<int>::max) ())
{
if (tree->getInputCloud ()->points.size () != cloud.points.size ())
{
PCL_ERROR ("[pcl::extractEuclideanClusters] Tree built for a different point cloud dataset (%lu) than the input cloud (%lu)!\n", tree->getInputCloud ()->points.size (), cloud.points.size ());
return;
}
if (cloud.points.size () != normals.points.size ())
{
PCL_ERROR ("[pcl::extractEuclideanClusters] Number of points in the input point cloud (%lu) different than normals (%lu)!\n", cloud.points.size (), normals.points.size ());
return;
}

// Create a bool vector of processed point indices, and initialize it to false
std::vector<bool> processed (cloud.points.size (), false);

std::vector<int> nn_indices;
std::vector<float> nn_distances;
// Process all points in the indices vector
for (std::size_t i = 0; i < cloud.points.size (); ++i)
{
if (processed[i])
continue;

std::vector<unsigned int> seed_queue;
int sq_idx = 0;
seed_queue.push_back (static_cast<int> (i));

processed[i] = true;

while (sq_idx < static_cast<int> (seed_queue.size ()))
{
// Search for sq_idx
if (!tree->radiusSearch (seed_queue[sq_idx], tolerance, nn_indices, nn_distances))
{
sq_idx++;
continue;
}

for (std::size_t j = 1; j < nn_indices.size (); ++j) // nn_indices[0] should be sq_idx
{
if (processed[nn_indices[j]]) // Has this point been processed before ?
continue;

//processed[nn_indices[j]] = true;
// [-1;1]
double dot_p = normals[i].normal[0] * normals[nn_indices[j]].normal[0] +
normals[i].normal[1] * normals[nn_indices[j]].normal[1] +
normals[i].normal[2] * normals[nn_indices[j]].normal[2];
if ( std::acos (std::abs (dot_p)) < eps_angle )
{
processed[nn_indices[j]] = true;
seed_queue.push_back (nn_indices[j]);
}
}

sq_idx++;
}

// If this queue is satisfactory, add to the clusters
if (seed_queue.size () >= min_pts_per_cluster && seed_queue.size () <= max_pts_per_cluster)
{
pcl::PointIndices r;
r.indices.resize (seed_queue.size ());
for (std::size_t j = 0; j < seed_queue.size (); ++j)
r.indices[j] = seed_queue[j];
auto lambda = [&](int i, int j, const Indices& nn_indices) -> bool {
//processed[nn_indices[j]] = true;
// [-1;1]
double dot_p = normals[i].normal[0] * normals[nn_indices[j]].normal[0] +
normals[i].normal[1] * normals[nn_indices[j]].normal[1] +
normals[i].normal[2] * normals[nn_indices[j]].normal[2];
return std::acos (std::abs (dot_p)) < eps_angle;
};

// These two lines should not be needed: (can anyone confirm?) -FF
std::sort (r.indices.begin (), r.indices.end ());
r.indices.erase (std::unique (r.indices.begin (), r.indices.end ()), r.indices.end ());
Indices indices;

r.header = cloud.header;
clusters.push_back (r); // We could avoid a copy by working directly in the vector
}
}
pcl::extractEuclideanClusters(cloud, indices, lambda, tree, tolerance, clusters, min_pts_per_cluster, max_pts_per_cluster);
}


Expand All @@ -196,91 +148,34 @@ namespace pcl
template <typename PointT, typename Normal>
void extractEuclideanClusters (
const PointCloud<PointT> &cloud, const PointCloud<Normal> &normals,
const std::vector<int> &indices, const typename KdTree<PointT>::Ptr &tree,
const Indices &indices, const typename KdTree<PointT>::Ptr &tree,
float tolerance, std::vector<PointIndices> &clusters, double eps_angle,
unsigned int min_pts_per_cluster = 1,
unsigned int max_pts_per_cluster = (std::numeric_limits<int>::max) ())
{
// \note If the tree was created over <cloud, indices>, we guarantee a 1-1 mapping between what the tree returns
//and indices[i]
if (tree->getInputCloud ()->points.size () != cloud.points.size ())
{
PCL_ERROR ("[pcl::extractEuclideanClusters] Tree built for a different point cloud dataset (%lu) than the input cloud (%lu)!\n", tree->getInputCloud ()->points.size (), cloud.points.size ());
return;
}
if (tree->getIndices ()->size () != indices.size ())
{
PCL_ERROR ("[pcl::extractEuclideanClusters] Tree built for a different set of indices (%lu) than the input set (%lu)!\n", tree->getIndices ()->size (), indices.size ());
return;
}
if (cloud.points.size () != normals.points.size ())
{
PCL_ERROR ("[pcl::extractEuclideanClusters] Number of points in the input point cloud (%lu) different than normals (%lu)!\n", cloud.points.size (), normals.points.size ());
return;
}
// Create a bool vector of processed point indices, and initialize it to false
std::vector<bool> processed (cloud.points.size (), false);

std::vector<int> nn_indices;
std::vector<float> nn_distances;
// Process all points in the indices vector
for (std::size_t i = 0; i < indices.size (); ++i)
{
if (processed[indices[i]])
continue;

std::vector<int> seed_queue;
int sq_idx = 0;
seed_queue.push_back (indices[i]);

processed[indices[i]] = true;

while (sq_idx < static_cast<int> (seed_queue.size ()))
{
// Search for sq_idx
if (!tree->radiusSearch (cloud[seed_queue[sq_idx]], tolerance, nn_indices, nn_distances))
{
sq_idx++;
continue;
}

for (std::size_t j = 1; j < nn_indices.size (); ++j) // nn_indices[0] should be sq_idx
{
if (processed[nn_indices[j]]) // Has this point been processed before ?
continue;

//processed[nn_indices[j]] = true;
// [-1;1]
double dot_p =
normals[indices[i]].normal[0] * normals[indices[nn_indices[j]]].normal[0] +
normals[indices[i]].normal[1] * normals[indices[nn_indices[j]]].normal[1] +
normals[indices[i]].normal[2] * normals[indices[nn_indices[j]]].normal[2];
if ( std::acos (std::abs (dot_p)) < eps_angle )
{
processed[nn_indices[j]] = true;
seed_queue.push_back (nn_indices[j]);
}
}

sq_idx++;
}

// If this queue is satisfactory, add to the clusters
if (seed_queue.size () >= min_pts_per_cluster && seed_queue.size () <= max_pts_per_cluster)
{
pcl::PointIndices r;
r.indices.resize (seed_queue.size ());
for (std::size_t j = 0; j < seed_queue.size (); ++j)
r.indices[j] = seed_queue[j];
if (indices.empty())
return;

// These two lines should not be needed: (can anyone confirm?) -FF
std::sort (r.indices.begin (), r.indices.end ());
r.indices.erase (std::unique (r.indices.begin (), r.indices.end ()), r.indices.end ());
// Create a bool vector of processed point indices, and initialize it to false
std::vector<bool> processed (cloud.points.size (), false);

r.header = cloud.header;
clusters.push_back (r);
}
}
auto lambda = [&](int &i, int &j, Indices& nn_indices) -> bool {
//processed[nn_indices[j]] = true;
// [-1;1]
double dot_p =
normals[indices[i]].normal[0] * normals[indices[nn_indices[j]]].normal[0] +
normals[indices[i]].normal[1] * normals[indices[nn_indices[j]]].normal[1] +
normals[indices[i]].normal[2] * normals[indices[nn_indices[j]]].normal[2];
return std::acos (std::abs (dot_p)) < eps_angle;
};

pcl::extractEuclideanClusters(cloud, indices, lambda, tree, tolerance, clusters, min_pts_per_cluster, max_pts_per_cluster);
}

//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
Expand Down
Loading