atenpas / gpd

Detect 6-DOF grasp poses in point clouds
BSD 2-Clause "Simplified" License
598 stars 233 forks source link

bug in calculating the filtered pointcloud after sample indexing #132

Open rmessaou opened 1 year ago

rmessaou commented 1 year ago

Hi Atenpas,

I am using gpd in my project and I had difficulties to work with the indexed pointcloud as input. It seems like the region that I am defining is not taken into consideration when the grasps are generated.

After a thorough look into the code, in the function where the filtering of the pointcloud using the indices is done, I noticed this:

https://github.com/atenpas/gpd/blob/6327f20eabfcba41a05fdd2e2ba408153dc2e958/src/gpd/util/cloud.cpp#L206-L222

and specifically this line

https://github.com/atenpas/gpd/blob/6327f20eabfcba41a05fdd2e2ba408153dc2e958/src/gpd/util/cloud.cpp#L215

So it seems like the indices that are being kept are not "the sample indices" but actually their indices in the for loop list (so technically you are saving the index i instead of sampleindices[i]).

Therefore shouldn't this line be instead like this

 indices_to_keep.push_back(sample_indices_[i]);

Can you confirm this ? Thank you and best regards Rayene

anmakon commented 1 year ago

Hi @rmessaou, I'm a bit late to the game here, but your fix just saved my day and I can confirm it's working! I think this bug could have been introduced somewhere along with the restructuring of the code in commit #83035e5, since I had earlier versions (with a weird version of ROS kinetic) working before without this issue.

Thank you!