[SupervoxelClustering] setIndices not working on SupervoxelClustering Classs

[tanismar]

Describe the bug
When using the superVoxelClustering class to segment objects in a scene using LCCP, I'd like to do so only on a subset of the points, (ie, removing the indices that correspond to the plane, and/or those too far away from the camera). I obtain these desired indices to be considered and pass them to the class using the setIndices function, but the supervoxels get computed for the totality of the input pointcloud, and therefore also the LCCP segmentation. I need the output to be in form of indices from the original pointcloud.

Context
My goal is to perform LCCP segmentation on a subset of indices of the original pointcloud.

Expected behavior
Like most other PCL functions, superVoxelClustering inherits setIndices from PCLBase. However, it seems to ignore this indices, since the result is computed on the whole input pointcloud

Current Behavior
Instead of computing the superVoxelClustering using the subset of indices passed via setIndices, it computes it for all the pointcloud.

To Reproduce
Same code as the lccp example,
https://github.com/PointCloudLibrary/pcl/blob/master/examples/segmentation/example_lccp_segmentation.cpp
where lines 272 to 319 have been modified as follows, to make use of indices:

(input pointcloud is any pointcloud with a plane and some objects on top of it. )

// Find plane  in pointcloud
pcl::PointIndices::Ptr plane_ids(new pcl::PointIndices);
pcl::ModelCoefficients::Ptr coeffs(new pcl::ModelCoefficients);
pcl::SACSegmentation<pcl::PointXYZRGBNormal> seg;
seg.setOptimizeCoefficients(true);
seg.setMaxIterations(200);
seg.setModelType(pcl::SACMODEL_PLANE);
seg.setMethodType(pcl::SAC_RANSAC);
seg.setDistanceThreshold(0.01);
seg.setInputCloud(input_pointcloud);
seg.segment(*plane_ids, *coeffs);

// Extract indices of points not in plane
pcl::PointIndices::Ptr non_plane_ids(new pcl::PointIndices);
pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr non_plane_cloud(new pcl::PointCloud<pcl::PointXYZRGBNormal>);
pcl::ExtractIndices<pcl::PointXYZRGBNormal> extract;
extract.setInputCloud(input_pointcloud);
extract.setIndices(plane_ids);
extract.setNegative(true);
extract.filter(non_plane_ids->indices);
extract.filter(*non_cloud_plane);

// Split pointcloud to fit SuperVoxelClustering input format
pcl::PointCloud<PointT>::Ptr cloud_RGB(new pcl::PointCloud<PointT>);
pcl::PointCloud<pcl::Normal>::Ptr cloud_normals(new pcl::PointCloud<pcl::Normal>);
pcl::copyPointCloud(*input_pointcloud, *cloud_RGB);
pcl::copyPointCloud(*input_pointcloud, *cloud_normals);

// Compute SuperVoxel Clustering
pcl::SupervoxelClustering<PointT> super(0.005, 0.02);
super.setUseSingleCameraTransform(false);
super.setInputCloud(cloud_RGB);
super.setNormalCloud(cloud_normals);
super.setIndices(non_plane_ids);         // <------------------- XXXX This is the line where the bug is XXXXX
super.setColorImportance(0.0f);
super.setSpatialImportance(1.0f);
super.setNormalImportance(4.0f);
std::map<std::uint32_t, pcl::Supervoxel<PointT>::Ptr> supervoxel_clusters;
PCL_INFO("Extracting supervoxels\n");
super.extract(supervoxel_clusters);
super.refineSupervoxels(2, supervoxel_clusters);

PCL_INFO("Getting supervoxel adjacency\n");
std::multimap<std::uint32_t, std::uint32_t> supervoxel_adjacency;
super.getSupervoxelAdjacency(supervoxel_adjacency);

PCL_INFO("Starting LCCP Segmentation\n");
pcl::LCCPSegmentation<PointT> lccp;
lccp.setConcavityToleranceThreshold(10.0);
lccp.setSanityCheck(false);
lccp.setSmoothnessCheck(true, 0.005, 0.02, 0.1);
lccp.setKFactor(1);
lccp.setInputSupervoxels(supervoxel_clusters, supervoxel_adjacency);
lccp.setMinSegmentSize(5);
lccp.segment();

PCL_INFO("Interpolation voxel cloud -> input cloud and relabeling\n");
pcl::PointCloud<pcl::PointXYZL>::Ptr sv_labeled_cloud = super.getLabeledCloud();
pcl::PointCloud<pcl::PointXYZL>::Ptr lccp_labeled_cloud = sv_labeled_cloud->makeShared();
lccp.relabelCloud(*lccp_labeled_cloud);
SuperVoxelAdjacencyList sv_adjacency_list;
lccp.getSVAdjacencyList (sv_adjacency_list);  // Needed for visualization

Screenshots/Code snippets
Example (but repeats in every example):
Original Pointcloud (input_pointcloud):

Pointcloud after plane removal (non_plane_cloud)

Supervoxel clustering results (despite setIndices(non_plane_ids):

LCCP Segmentation results:

Your Environment (please complete the following information):

• OS: Ubuntu 18.04
• Compiler:GCC 7.5
• PCL Version 1.8

Possible Solution

Not obligatory, but suggest a fix/reason for the bug. Feel free to create a PR if you feel comfortable.

Additional context

Add any other context about the problem here.

[mvieth]

Thanks for reporting. We will have to see how difficult it is to integrate that into the SupervoxelClustering class. As a workaround, you could create two new clouds (an rgb cloud and a normal cloud) without the plane, e.g. with ExtractIndices, and pass those to SupervoxelClustering.

[kunaltyagi]

We will have to see how difficult it is to integrate that into the SupervoxelClustering class

@tanismar Internally, the heavy lifting is performed by OcTree and KDTrees. You could add overload to SupervoxelClustering::setInputCloud to accept indices and pass them to the adjacency OcTree. This alone might be sufficient.

Though I'm basing this entirely on old knowledge of its implementation

[tanismar]

Hi @kunaltyagi , I will try that, thanks. @mvieth , unfortunately, that woulnd't be enough, since I need the results as indices with respect to the original pointcloud (very much like every other function behaves when taking .setIndices input).

[kunaltyagi]

@tanismar if it works, please report back and we can add that as a PR :)

[tanismar]

Hi! I am sorry, I haven't been able to work on this since my original post. Has anyone tried implementing @kunaltyagi suggestion?
It is an option clearly missing from the LCCP segmentation

[tanismar]

For reasons related to the build system at my workplace, properly fixing the bug inside PCL is too complicated. However, I found a working workaround that is quite simple and works well:
Instead of passing the full original pointcloud and the indices corresponding to the cropped pointcloud "select_indices", I use those indices to create an input pointcloud with only the selected points, which is the one passed to the SuperVoxel+LCCP segmentation algorithm.

// XXX Since lccp.setIndices() does not work, pass only the objects pointcloud, and then retrieve the original indices PointCloud::Ptr select_cloud(new PointCloud); PointCloudNormals::Ptr select_normals_cloud(new PointCloudNormals); pcl::copyPointCloud(*i_pointCloud, select_indices, *select_cloud); pcl::copyPointCloud(*i_normals, select_indices, *select_cloud_cloud);
[...] Here goes all the Supervoxel and LCCP Segmentation bit (see above).

We retrieve the result in the form of the labeledCloud:

pcl::PointCloud<pcl::PointXYZL>::Ptr supervoxel_cloud = super.getLabeledCloud(); pcl::PointCloud<pcl::PointXYZL>::Ptr select_labeled_cloud = supervoxel_cloud->makeShared(); lccp.relabelCloud(*select_labeled_cloud);
Then, from the output select_labeled_cloud, and using the , the select_indices are used to retrieve back the indices of those points, for each segmented cluster, from the original pointcloud.

`
std::vectorpcl::PointIndices cluster_list;

// For each point in labeled cloud:
std::vector<int> label_indices;  // Keeps all labels found so far
for (int i = 0; i < labeled_cloud->points.size(); i++)
{
    // Retrieve label at point
    pcl::PointXYZL pt_label = labeled_cloud->points[i];
    int label = (int)pt_label.label;
    int label_index;

    // Find label index if previously seen, or add it to map if not found yet.
    std::vector<int>::iterator it = find(label_indices.begin(), label_indices.end(), label);
    if (it == label_indices.end())
    {
        label_index = label_indices.size();
        label_indices.push_back(label);

        // Add empty cluster;
        pcl::PointIndices cluster;
        cluster_list.push_back(cluster);
    }
    else
    {
        label_index = it - label_indices.begin();
    }

    // Add current point to cluster:
    cluster_list[label_index].indices.push_back(select_indices[i]);
}

`

The most important line to do with the reverse mapping is
cluster_list[label_index].indices.push_back(indices[i]);
So that instead of just adding to the cluster the index of the point with that label from the cropped pointcloud, the one added is the one from the original pointcloud, via select_indices[i]

Hope this helps for other users of LCCP