Dynamic obstacle clearing and adding in the costmap

[arainbilal]

Required more information about using the nvblox for dynamic obstacles, in the particular clearing. The attached video shows the nvblox running on a desktop computer. The costmap shows the adding and clearing of the obstacles.

https://user-images.githubusercontent.com/9365220/199213762-51e71d7e-e396-4b83-9ac2-2e4c06ce8896.mp4

Both the ESDF and the TSDF are updated at 30 FPS. Thanks in advance for your help.

[arainbilal]

Going through the code, in particular for the clearing. Could be an idea to use the same approach for clearing outside the radius. The following may be helpful: nvblox_ros:

• Update the slice mapper_->updateEsdfSlice(min_height_, max_height_, slice_height_);
• distance slice for the costmap converter_.distanceMapSliceFromLayer(mapper_->esdf_layer(), slice_height_, &map_slice);

In conversions, finding out the map size using the height and then fill up the image: populateSliceFromLayer(layer, aabb, z_slice_level, voxel_size, kUnknownValue, &image);

After updating the costmap, clear the blocks [need guidance from the developers if this is a good idea :)] layers_.getPtr<EsdfLayer>()->clearBlocks(block_indices_for_deletion); The block indices could be the updated blocks used for the previous update.

Update: Clearing the esdf layer will not help. I think it has some relationship with raycasting. Note the clearing of the previously marked obstacles is observed if the new obstacle appeared in the scene away from the previous one. For example, in the video, the obstacle closer to the camera was cleared when the person move far away in the scene.

[arainbilal]

It could also be interesting to reproduce this using the tests and the simulated scene. For example, the esdf_integrator_test has the updating an removal of objects. The ply files produced from these tests have no triangles for some reason. I therefore, export the csv depth images for the visualization purposes.

[alexmillane]

Hi there arainbilal.

Thank you for your interest in nvblox.

So I'm not sure I understand the issue/question. Is there an issue with the behavior that you see in the video above?

If I had to guess, based on the video, I would say that the issue is that the map is reacting too slowly to dynamics for your liking. Is that correct?

In general, we do not explicitly handle dynamic objects. This is something we're working on.

For the time being:

In order handle dynamic objects well, your best bet at the moment is to experiment with the parameter tsdf_integrator_max_weight. I'll attempt to explain this parameter. You can think of each voxel in the reconstruction performing a running average of observations. The maximum weight is the number of observations that this running average is conducted over. The default parameter is 100, although it is set to 20 in carter_nvblox.yaml... I'm not sure what value you're using? A high value leads to high quality in static scenes, but slow reactions to changes in the scene. I would recommend that you drop this value drastically to say 5, and see if you get better behavior.

Note that these is a tradeoff between reconstruction quality and reaction speed to dynamics, however. But probably the default value of 100, in a scene like this is much too high.

I hope that helps.

[arainbilal]

Hi @alexmillane, thank you for your response. You are right; the issue is that the map is reacting too slowly in adding and clearing the obstacles in front of the robot.
The tsdf_integrator_max_weight was set to 20.0, similar to carter_nvblox.yaml. The application is to avoid obstacles, including people as dynamic obstacles. I have changed the tsdf_integrator_max_weight to 2.0 and the response is much better. There are instances when the map does not clear. Please find the attached video and observe the following timeline in the attached video:

1. From 06 to 11 seconds (approx), obstacles appear and disappear in the scene. A partial map clearing was performed between 11 to 19 seconds.
2. From 19 to 23 seconds (approx), the obstacle appears at a different position (further way in the scene) from the previous one. The map reacted ok but it clears the previous remaining partial obstacle showing on the map (from 06-11 seconds).
3. From 25 to 28 seconds (approx), obstacles appear and disappear in the scene. Not that the map clearing (or reacting to disappearing the obstacle) from 28 to 41 seconds. From the observation, it appears that the map is reacting fast in adding the obstacles but not completely clearing the obstacles. Also, it was observed that the map clears the false obstacles when it reacts to add another (see 19 to 23 seconds approx. in the video). In this application, the reaction to the speed dynamics is more of a focus than the reconstruction quality. Thanks in advance for your help.

https://user-images.githubusercontent.com/9365220/201272974-4bd55d05-ce34-4bd5-9447-7d1d4ff3cc42.mp4

[alexmillane]

Thank you for your detailed analysis. I agree with your observations.

I'll just explain how a voxel is cleared. If a depth measurement is obtained, a ray is cast out to that depth (in effect). Voxels along the way are cleared.

I think what is happening here is that because your depth map has some significant persistent holes in it, no clearing rays are being cast in areas where a person was previously observed and now there is a hole in the depth map. These voxels are then never reobserved until another person enters this area the depth camera calculates a valid depth.

From the video above you're testing on quite a reflective floor. Do you have the realsense's projector enabled? Even if you do it's quite possible the depth camera cannot generate depth on such a reflective surface. In this case, there's not too much that is already in nvblox to help you.

One thing would be to add some decay to the voxels. I think this is actually a good idea, and is what is done in spatio temporal voxel layer. I think that this is probably worth adding, we just haven't had the time yet to do so.

Sorry I can't be more help.

[arainbilal]

I can confirm that the D455 realsense projector is enabled. Thanks for your help and will look forward to the voxel decay added to the nvblox.