Optimize obstacle segmentation for organized point clouds

[matlabbe]

The idea is that normal computation speed could be increased and be more accurate when we received an organized point cloud. The normal clustering could be also faster. In both case, no radius search required.

This could be useful for point cloud created from depth camera, or lidar scans ordered by rings.

[naitiknakrani-eic]

Hi @matlabbe Can you please explain more about your idea? We analyzed timing and cluster segmentation using radiusSearch is taking huge computation time. We ae trying to optimize this time using GPU.

You mention when received point cloud is ordered, clustering is faster. When we provided ordered point cloud data, rtabmap::process removes NaN points at some point and send unordered point cloud in segmentObstaclesFromGround function.

[matlabbe]

Are you trying the pull request? There could be some voxel filtering to disable like setting Grid/PreVoxelFiltering to false.

[naitiknakrani-eic]

No not yet. Just wanted to understand your idea, how unordered and ordered point cloud effects on segmentation computation time. When you say normal clustering are you referring pcl::EuclideanClusterExtraction ?

[matlabbe]

Based on the description of this PR, with organized point clouds we can

• compute faster normals using approach like https://pcl.readthedocs.io/projects/tutorials/en/latest/normal_estimation_using_integral_images.html (no need to search nearest neighbor of each point to estimate the normal, as it is organized, we know already the closest neighbors for each point/pixel).
• for normal clustering, based on the draft, it does indeed mean doing the job of pcl::EuclideanClusterExtraction to cluster point clouds but without having to do nearest neighbor search. The idea is to do a floodfill-like algorithm in the organized cloud. Instead of color difference, it is depth difference between the points (/pixels).