OctoMap fails to fully detect moving objects

[csvicbot-7]

Hi @matlabbe

I'm encountering an issue with OctoMap when using it to detect obstacles. Initially, when I launch RTAB-Map, OctoMap detects all obstacles correctly. However, when someone walks around the LiDAR sensor, OctoMap fails to detect the person’s entire body.

octomap_issue.mp4

In the attached files, you can see the contrast between the two topics: /rtabmap/local_grid_obstacle (red points) and /rtabmap/octomap_obstacles (cubes). OctoMap detects only a small part of the person's body, while the /rtabmap/local_grid_obstacle topic captures the person's body more accurately.

To illustrate this issue further, I’ve attached the following files in the link

• A database
• A rosbag
• A launch file

Additionally, I am unable to visualize the /rtabmap/octomap_full topic in RViz. I have already installed the octomap_rviz_plugins package, but I receive the error message "Failed to create octree structure."

Could you please provide some advice on how to resolve these issues?

Thanks.

[matlabbe]

You should use ColorOccupancyGrid display type:

Looking at the octomap, we see more cubes added:

However, if the obstacle cloud doesn't get updated similarly, it means the occupancy probability inside those cubes are below GridGlobal/OccupancyThr (default 0.5). Here showing cell probability:

Not sure why in your video there are no points added around the head where it seems keeping more bright green cubes. A rosbag with the ouster point cloud could be useful to test with.

Note also that OctoMap is not meant to track dynamic obstacles as its refresh rate is low (1 Hz by default). For fast dynamic objects, consider using a local voxel grid.

[csvicbot-7]

Hi @matlabbe

Thank you for your response.

I was testing octomap in two modes: Grid/RayTracing parameter set to true and false.

Grid/RayTracing = true

In the first part of the video you can see a person walking in front of the sensor. The whole body of the person is identified as an obstacle, but as he moves, the head part is not refreshed. Why does this happen?

However, after a few seconds if I enable and disable the topic (/rtabmap/octomap_full) in rviz, the head voxels disappear. But if I have a node that subscribes to the map (/rtabmap/octomap_full) these voxels never disappear, unless the node stops subscribing to the map. This can be seen at min 00:51 of the video.
The node that records the rosbag subscribes to the map topic. So while the recording is running the voxels are not refreshed; but when the recording stops the voxels disappear.

raytracing_true.mp4

Grid/RayTracing = false

In this case the person's body is detected as an obstacle but the voxels are not refreshed. Why does this happen?

As in the previous case, after a few seconds the voxels corresponding to the person's body are refreshed, only if there are no nodes subscribed to the map topic (min 00:39).

raytracing_false.mp4

In both cases, why if there is a node subscribing to the map topic (/rtabmap/octomap_full) some voxels are not refreshed?

I’ve attached the following files in the link

• Two rosbags (one for each case), each rosbag has: /tf (transformations), /os_cloud_node/point (Ouster point cloud), /rtabmap/octomap_full (map).
• Database
• Launch file

Thank you in advance for your reply.

[matlabbe]

Thanks for the video and the bag. The reason why the octomap is cleared after unsubscribing to octomap topic is that rtabmap will flush the whole octomap kept in RAM and reconstruct it from scratch with nodes in the graph. As you are using RGBD/LinearUpdate=0.05 and RGBD/AngularUpdate=0.05, the graph doesn't increase in size because the lidar is not moving. So the resulting re-generated octomap contains only the first node added at the beggining (where nobody was in front of the sensor). If you set RGBD/LinearUpdate=0, you swill see that the octomap is re-created exactly like before unsubscribing to it.

And for why the octomap is updated even if no nodes are added to map, it is because of parameter map_always_update is true.

The main issue however is why the head is not removed from the octomap? It is because Grid/MaxObstacleHeight is set to 2 meters, so all lidar points over that threshold are not used for ray tracing.

Setting to 0 would make the head cleared.

EDIT: Obviously setting Grid/MacObstacleHeight=0 will make the ceiling appears as an obstacle in the 2D projected occupancy grid. To ignore the ceiling when generating the 2D occupancy grid from octomap, we would need to add a if here like this:

if(pt.z() < 2) {
    // projected on ground
    oPtr[oi][0] = pt.x()-halfCellSize;
    oPtr[oi][1] = pt.y()-halfCellSize;
    ++oi;
}

cheers,
Mathieu