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 2 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
2 changes: 1 addition & 1 deletion examples/segmentation/example_extract_clusters_normals.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -69,7 +69,7 @@ main (int, char **argv)
std::cout << "Estimated the normals" << std::endl;

// Creating the kdtree object for the search method of the extraction
pcl::search::KdTree<pcl::PointXYZ>::Ptr tree;
auto tree = std::make_shared<pcl::search::KdTree<pcl::PointXYZ>>();
tree->setInputCloud (cloud_ptr);

// Extracting Euclidean clusters using cloud and its normals
Expand Down
67 changes: 38 additions & 29 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 @@ -47,19 +47,30 @@
namespace pcl
{

namespace detail {
template <typename PointT, typename Function>
constexpr static bool is_functor_for_additional_filter_criteria_v =
pcl::is_invocable_r_v<bool,
Function,
const pcl::remove_cvref_t<pcl::PointCloud<PointT>>&,
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

With const and reference collapsing rules, isn't this the same as const remove_volatile_t<pcl::PointCloud<PointT>>&,?

Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

It is. Which leads to the next question, why do you care if the user supplies a type which is volatile?

Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I think it's more about remove_cvref_t being used to strip const and reference qualifiers, and nobody gives a thought about volatile. As far as I'm concerned I wouldn't consider volatile in such a case and would go with const pcl::PointCloud<PointT>&? directly.

Copy link
Member

@kunaltyagi kunaltyagi Jul 17, 2020 •
edited
Loading

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Just chiming in, being the author of the original snippet this is copied from: it's a mistake from when I was taking in the functor itself (typename FunctorT) and needed this to get arguments and constrain the input to const PointCloud<PointT>& (no volatile allowed). But the design changed, and got simplified thanks to the reviews but this managed to slip in.

We know the type here, and remove_X is doing nothing of value here

pcl::index_t,
const pcl::remove_cvref_t<Indices>&,
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

same comment here.

pcl::index_t>;
}

//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/** \brief Decompose a region of space into clusters based on the Euclidean distance between points
* \param it cloud iterator for iterating over the points
* \param cloud the point cloud message
* \param additional_filter_criteria functor which is used as an additonal criteria that all points being added to the cluster must satisfy
* \param additional_filter_criteria a functor specifying a criterion that must be satisfied by all point added to the cluster
* \param tree the spatial locator (e.g., kd-tree) used for nearest neighbors searching
* \note the tree has to be created as a spatial locator on \a cloud and \a indices
* \param tolerance the spatial cluster tolerance as a measure in L2 Euclidean space
* \param clusters the resultant clusters containing point indices (as a vector of PointIndices)
* \param min_pts_per_cluster minimum number of points that a cluster may contain (default: 1)
* \param max_pts_per_cluster maximum number of points that a cluster may contain (default: max int)
* \warning It is assumed that that the tree built on the same indices and cloud as passed here,
* if that is not the case then things can go very wrong. For speed reasons, we only check the sizes and not the content
* \warning The cloud/indices passed here must be the same ones used to build the tree.
* For performance reasons PCL on warns if the sizes are not equal and no checks are performed to verify if the content is the same
* \ingroup segmentation
*/
template <typename PointT, typename FunctorT> void
Expand All @@ -68,6 +79,10 @@ namespace pcl
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())
{
static_assert(detail::is_functor_for_additional_filter_criteria_v<PointT, FunctorT>,
"Functor signature must be similar to `bool(const PointCloud<PointT>&, "
"index_t, const Indices&, index_t)`");

if (tree->getInputCloud ()->points.size () != cloud.points.size ())
{
PCL_ERROR ("[pcl::extractEuclideanClusters] Tree built with a different point cloud size (%lu) than the input cloud (%lu)!\n", tree->getInputCloud ()->points.size (), cloud.points.size ());
Expand All @@ -83,37 +98,31 @@ namespace pcl
if (processed[it.getCurrentIndex()])
continue;

index_t sq_idx = 0;
clusters.emplace_back();
auto& seed_queue = clusters.back();
seed_queue.indices.push_back (it.getCurrentIndex());

processed[it.getCurrentIndex()] = true;

while (sq_idx < static_cast<index_t> (seed_queue.indices.size()))
for (index_t sq_idx = 0; sq_idx < static_cast<index_t> (seed_queue.indices.size()); ++sq_idx)
{
Indices nn_indices;
std::vector<float> nn_distances;

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

for (index_t j = nn_start_idx; j < nn_indices.size (); ++j) // can't assume sorted (default isn't!)
{
if (processed[nn_indices[j]]) // Has this point been processed before ?
continue;

if (additional_filter_criteria(it.getCurrentIndex(), j, nn_indices)) {
if (additional_filter_criteria(cloud, it.getCurrentIndex(), nn_indices, j)) {
seed_queue.indices.push_back(nn_indices[j]);
processed[nn_indices[j]] = true;
}
}

sq_idx++;
}

// If this queue is satisfactory, add to the clusters
Expand All @@ -127,15 +136,15 @@ namespace pcl
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/** \brief Decompose a region of space into clusters based on the Euclidean distance between points
* \param cloud the point cloud message
* \param additional_filter_criteria functor which is used as an additonal criteria that all points being added to the cluster must satisfy
* \param additional_filter_criteria a functor specifying a criterion that must be satisfied by all point added to the cluster
* \param tree the spatial locator (e.g., kd-tree) used for nearest neighbors searching
* \note the tree has to be created as a spatial locator on \a cloud and \a indices
* \param tolerance the spatial cluster tolerance as a measure in L2 Euclidean space
* \param clusters the resultant clusters containing point indices (as a vector of PointIndices)
* \param min_pts_per_cluster minimum number of points that a cluster may contain (default: 1)
* \param max_pts_per_cluster maximum number of points that a cluster may contain (default: max int)
* \warning It is assumed that that the tree built on the same indices and cloud as passed here,
* if that is not the case then things can go very wrong. For speed reasons, we only check the sizes and not the content
* \warning The cloud/indices passed here must be the same ones used to build the tree.
* For performance reasons PCL on warns if the sizes are not equal and no checks are performed to verify if the content is the same
* \ingroup segmentation
*/
template <typename PointT, typename FunctorT> void
Expand All @@ -153,15 +162,15 @@ namespace pcl
/** \brief Decompose a region of space into clusters based on the Euclidean distance between points
* \param cloud the point cloud message
* \param indices a list of point indices to use from \a cloud
* \param additional_filter_criteria functor which is used as an additonal criteria that all points being added to the cluster must satisfy
* \param additional_filter_criteria a functor specifying a criterion that must be satisfied by all point added to the cluster
* \param tree the spatial locator (e.g., kd-tree) used for nearest neighbors searching
* \note the tree has to be created as a spatial locator on \a cloud and \a indices
* \param tolerance the spatial cluster tolerance as a measure in L2 Euclidean space
* \param clusters the resultant clusters containing point indices (as a vector of PointIndices)
* \param min_pts_per_cluster minimum number of points that a cluster may contain (default: 1)
* \param max_pts_per_cluster maximum number of points that a cluster may contain (default: max int)
* \warning It is assumed that that the tree built on the same indices and cloud as passed here,
* if that is not the case then things can go very wrong. For speed reasons, we only check the sizes and not the content
* \warning The cloud/indices passed here must be the same ones used to build the tree.
* For performance reasons PCL on warns if the sizes are not equal and no checks are performed to verify if the content is the same
* \ingroup segmentation
*/
template <typename PointT, typename FunctorT> void
Expand Down Expand Up @@ -227,7 +236,7 @@ namespace pcl
* \note the tree has to be created as a spatial locator on \a cloud
* \param tolerance the spatial cluster tolerance as a measure in the L2 Euclidean space
* \param clusters the resultant clusters containing point indices (as a vector of PointIndices)
* \param eps_angle the maximum allowed difference between normals in radians for cluster/region growing
* \param max_angle the maximum allowed difference between normals in radians for cluster/region growing
* \param min_pts_per_cluster minimum number of points that a cluster may contain (default: 1)
* \param max_pts_per_cluster maximum number of points that a cluster may contain (default: max int)
* \ingroup segmentation
Expand All @@ -236,7 +245,7 @@ namespace pcl
extractEuclideanClusters (
const PointCloud<PointT> &cloud, const PointCloud<Normal> &normals,
float tolerance, const typename search::Search<PointT>::Ptr &tree,
std::vector<PointIndices> &clusters, double eps_angle,
std::vector<PointIndices> &clusters, double max_angle,
unsigned int min_pts_per_cluster = 1,
unsigned int max_pts_per_cluster = (std::numeric_limits<int>::max) ())
{
Expand All @@ -246,11 +255,11 @@ namespace pcl
return;
}

eps_angle = std::max(std::abs(eps_angle), M_PI);
auto cos_eps_angle = std::cos(eps_angle);
auto normal_deviation_filter = [&](index_t i, index_t j, const Indices& nn_indices) -> bool {
max_angle = std::min(std::abs(max_angle), M_PI);
auto cos_max_angle = std::cos(max_angle);
auto normal_deviation_filter = [&](const PointCloud<PointT> &cloud, index_t i, const Indices& nn_indices, index_t j) -> bool {
double dot_p = normals[i].getNormalVector3fMap().dot(normals[nn_indices[j]].getNormalVector3fMap());
return std::abs(dot_p) < cos_eps_angle;
return std::abs(dot_p) < cos_max_angle;
};
pcl::extractEuclideanClusters(cloud, normal_deviation_filter, tree, tolerance, clusters, min_pts_per_cluster, max_pts_per_cluster);
}
Expand All @@ -266,7 +275,7 @@ namespace pcl
* \note the tree has to be created as a spatial locator on \a cloud
* \param tolerance the spatial cluster tolerance as a measure in the L2 Euclidean space
* \param clusters the resultant clusters containing point indices (as PointIndices)
* \param eps_angle the maximum allowed difference between normals in degrees for cluster/region growing
* \param max_angle the maximum allowed difference between normals in degrees for cluster/region growing
* \param min_pts_per_cluster minimum number of points that a cluster may contain (default: 1)
* \param max_pts_per_cluster maximum number of points that a cluster may contain (default: max int)
* \ingroup segmentation
Expand All @@ -275,7 +284,7 @@ namespace pcl
void extractEuclideanClusters (
const PointCloud<PointT> &cloud, const PointCloud<Normal> &normals,
const Indices &indices, const typename search::Search<PointT>::Ptr &tree,
float tolerance, std::vector<PointIndices> &clusters, double eps_angle,
float tolerance, std::vector<PointIndices> &clusters, double max_angle,
unsigned int min_pts_per_cluster = 1,
unsigned int max_pts_per_cluster = (std::numeric_limits<int>::max) ())
{
Expand All @@ -288,12 +297,12 @@ namespace pcl
if (indices.empty())
return;

eps_angle = std::max(std::abs(eps_angle), M_PI);
auto cos_eps_angle = std::cos(eps_angle);
auto normal_deviation_filter = [&](index_t i, index_t j, Indices& nn_indices) -> bool {
max_angle = std::min(std::abs(max_angle), M_PI);
auto cos_max_angle = std::cos(max_angle);
auto normal_deviation_filter = [&](const PointCloud<PointT> &cloud, index_t i, const Indices& nn_indices, index_t j) -> bool {
double dot_p =
normals[indices[i]].getNormalVector3fMap().dot(normals[indices[nn_indices[j]]].getNormalVector3fMap());
return std::abs(dot_p) < cos_eps_angle;
return std::abs(dot_p) < cos_max_angle;
};

pcl::extractEuclideanClusters(cloud, indices, normal_deviation_filter, tree, tolerance, clusters, min_pts_per_cluster, max_pts_per_cluster);
Expand Down
10 changes: 5 additions & 5 deletions segmentation/include/pcl/segmentation/impl/extract_clusters.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -48,9 +48,9 @@ pcl::extractEuclideanClusters (const PointCloud<PointT> &cloud,
unsigned int min_pts_per_cluster,
unsigned int max_pts_per_cluster)
{
auto extract_all = [&](int i, int j, const Indices& nn_indices) {
utils::ignore(i, j, nn_indices);
return true;
auto extract_all = [&](const PointCloud<PointT> &cloud, index_t i, const Indices& nn_indices, index_t j) {
utils::ignore(cloud, i, nn_indices, j);
return true;
};
pcl::extractEuclideanClusters<PointT>(cloud, extract_all, tree, tolerance, clusters, min_pts_per_cluster, max_pts_per_cluster);
}
Expand All @@ -68,8 +68,8 @@ pcl::extractEuclideanClusters (const PointCloud<PointT> &cloud,
if (indices.empty())
return;

auto extract_all = [&](index_t i, index_t j, const Indices& nn_indices) {
utils::ignore(i, j, nn_indices);
auto extract_all = [&](const PointCloud<PointT> &cloud, index_t i, const Indices& nn_indices, index_t j) {
utils::ignore(cloud, i, nn_indices, j);
return true;
};
pcl::extractEuclideanClusters(cloud, indices, extract_all, tree, tolerance, clusters, min_pts_per_cluster, max_pts_per_cluster);
Expand Down