3D Ray Tracing in obstacle-free directions

[merlin]

Hello Matthieu!

First of all, thank you so much your great work on RTAB-Map and your continuous support of the community. Much appreciated!

Context

I am trying to develop a UAV navigation stack as part of my PhD thesis. I recently integrated RTAB-Map and apart from some minor (probably tuning-related) issues (walls appearing doubled in the simulations) it seems like a good match for my application.

But a problem appeared when I tried to use the OctoMap output in a large outdoor scenario as input for a frontier-based exploration algorithm (which I adapted from other researchers). In essence, it looks for frontier cells (free cells neighboring unknown cells) in a given exploration volume, clusters them, calculates a score for each cluster based on expected information gain and distance, and chooses the best one as the drone's next goal. In my application, it should terminate when a minimum percentage of the volume was explored.

Question

This requires the underlying SLAM solution to explicitly map free cells. With the Grid/RayTracing parameter, RTAB-Map does this for each cell along rays between the sensor and obstacles. But it doesn't do this for any other direction where no obstacle was hit, e.g. towards the sky. See the screenshot for an example.



Now I was wondering, do you think there is a sensible way to add this functionality to RTAB-Map?

I suspect tracing all rays on every map update would be too expensive, so a downsample might have to do. But what would otherwise be necessary? Could, for example, sensor updates trigger tracing for (some) rays which exceeded the maximum range without interfering with the rest of the map update process? Thought I'd better ask for your opinion on this before starting time-consuming experiments on my own =)

I looked into OctoMap.cpp but could not understand it's inner workings 100% yet. If you find the time, some additional code documentation would certainly be appreciated!

Many thanks and regards,
Merlin

[matlabbe]

Hi Merlin,

With 2D LiDAR, there is a parameter to enable this:

Param: Grid/Scan2dUnknownSpaceFilled = "false"             [Unknown space filled. Only used with 2D laser scans. Use Grid/RangeMax to set maximum range if laser scan max range is to set.]

To be able to ray trace, we need some parameters telling us the min and max orientation range of the lidar, with its increment, as seen in sensor_msgs/LaserScan msg.

With 3D LiDAR, rtabmap subcribes to sensor_msgs/PointCloud2 for which we cannot know the end point of empty scans, we only know the obstacles position, or NaN/inf values for unknown data.

For OctoMap created from depth image projection, that could be possible to do (with camera calibration, we could ray trace a pixel not having valid depth values to some maximum range), but rtabmap's library converts depth images to point clouds before updating the OctoMap, so we lose the camera calibration stuff as we deal only with a point cloud (of valid points) like the 3D LiDAR.

From a practical perspective, I don't use often the Grid/Scan2dUnknownSpaceFilled parameter, because I don't want to pollute the grid with empty cells where the lidar failed to return a reading (because the beam didn't return for some reasons while there are real obstacles). However, if the drone is in a large room in which its sensor cannot hit the ceiling, I can see indeed a problem for exploration. What you could do is to provide fake points in the sensor_msgs/PointCloud2 with range higher than the max range of the sensor, then use Grid/RangeMax to limit the ray tracing to real maximum range.

cheers,
Mathieu

[merlin]

Hello Matthieu,

Thank you very much for the elaborate answer! I will try your suggestion of enhancing a point cloud with fake points as soon as I can. Hopefully, I can get back to you with some results then :)