[Features] our_cvfh use own impl of extract clusters

[larshg]

Describe the bug

OURCVFHEstimation used its own impl of extractEuclideanClustersSmooth (ie. using normals also)
See:

pcl/features/include/pcl/features/impl/our_cvfh.hpp

Lines 73 to 157 in 7004787

	//////////////////////////////////////////////////////////////////////////////////////////////
	template<typename PointInT, typename PointNT, typename PointOutT> void
	pcl::OURCVFHEstimation<PointInT, PointNT, PointOutT>::extractEuclideanClustersSmooth (const pcl::PointCloud<pcl::PointNormal> &cloud,
	const pcl::PointCloud<pcl::PointNormal> &normals,
	float tolerance,
	const pcl::search::Search<pcl::PointNormal>::Ptr &tree,
	std::vector<pcl::PointIndices> &clusters, double eps_angle,
	unsigned int min_pts_per_cluster,
	unsigned int max_pts_per_cluster)
	{
	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<std::size_t> seed_queue;
	std::size_t sq_idx = 0;
	seed_queue.push_back (i);
	processed[i] = true;
	while (sq_idx < 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.points[seed_queue[sq_idx]].normal[0] * normals.points[nn_indices[j]].normal[0]
	+ normals.points[seed_queue[sq_idx]].normal[1] * normals.points[nn_indices[j]].normal[1] + normals.points[seed_queue[sq_idx]].normal[2]
	* normals.points[nn_indices[j]].normal[2];
	if (std::abs (std::acos (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];
	std::sort (r.indices.begin (), r.indices.end ());
	r.indices.erase (std::unique (r.indices.begin (), r.indices.end ()), r.indices.end ());
	r.header = cloud.header;
	clusters.push_back (r); // We could avoid a copy by working directly in the vector
	}
	}
	}

Context

Looking into the various cluster algorithms.

Expected behavior

Let our_cvfh use the cluster algorithms in segmentation module.

Current Behavior

It has its own implementation which doesn't get fixed. ie. latest commit about abs/acos to the dot_p.

Your Environment (please complete the following information):

• OS: Windows
• Compiler: VS2019
• PCL Version Head

Possible Solution

Remove the local implementation and use the segmentation method from the segmentation module.

Additional context

Wonder if there are other places where this happens or something similiar😄

[kunaltyagi]

Remove the local implementation and use the segmentation method from the segmentation module

Are the implementations significantly similar (or dare I say: same)?

[larshg]

Except from the bug fix it is the same 😄 Unless I have missed some, but I don't think so. Even the comments are exact the same, so it seems like a direct copy.

[larshg]

Ahh.. had another look:
It actually seems to include the fix which is proposed here: #3842 ie.

double dot_p = normals.points[seed_queue[sq_idx]].normal[0] * normals.points[nn_indices[j]].normal[0]
            + normals.points[seed_queue[sq_idx]].normal[1] * normals.points[nn_indices[j]].normal[1] + normals.points[seed_queue[sq_idx]].normal[2]
            * normals.points[nn_indices[j]].normal[2];

which is not yet present in the current extract_clusters in the segmentation module.

[kunaltyagi]

That fix is in another PR IIRC

If this is the case, I'd say: go ahead and merge the 2 implementations