More allocated voxels close to surface for planning

[Martin-Oehler]

Hello,
I am using Voxblox to plan arm motions with an ESDF, but I ran intro problems when the viewing angle to the surface is very sharp. Ideally you want to have a big margin of defined voxels around every surface so you can detect collisions early. This isn't the case for sharp viewing angles.

The arm get's into collision because voxels very close to the surface are not defined.
For proper planning it has to look more like this:

I created this by simply setting up a big wall on the left.

Do you have an idea how to implement my use-case? I think it is a common problem for planning with voxblox.
One way might be to add a new esdf integrator that updates and allocates even unobserved neighboring voxels if the voxels' distance is below a certain threshold.

Kind regards,
Martin

[ZacharyTaylor]

Hi Martin,
This is an interesting issue.

One way to get some more voxels allocated near the surface of shallow returns is to simply increase the truncation distance of the TSDF from the default 2*voxel_size to something larger such as 5 or 10x. However, if no ray passes through the area your system is trying to plan in this won't solve your problem.

I would be hesitant to allow the ESDF to update unobserved voxels as this would allow for situations where the map is confident an area was obstacle free when in reality moving into this space could cause a collision. In our current work on planning in the voxblox ESDF for MAVs we overcome this problem by setting all unknown space in the TSDF to occupied before building an ESDF out of it. This ensures the system will never try to enter an area it has not explicitly observed to be safe and could also work in your application.

[Martin-Oehler]

Hi Zachary,

thanks for your help.

However, if no ray passes through the area your system is trying to plan in this won't solve your problem.

The sensor can actually look above the ramp, but since nothing is behind it, no rays are created that would mark the voxels as unoccupied.

setting all unknown space in the TSDF to occupied before building an ESDF out of it

Isn't that problematic in open spaces? For example if you don't have a ceiling, everything above the sensor height is unobserved and would then be occupied.

[ZacharyTaylor]

It is a problem in open spaces. We get around this by making use of the freespace_pointcloud topic which is activated by setting use_freespace_pointcloud to true.

We then run our system with a modified stereo vision driver that outputs two separate pointclouds. The first pointcloud is the normal surface points. The second output is a pointcloud that gives an estimate of how far away a point in a certain direction can safely be assumed to be free see ethz-asl/image_undistort#18 for details. Also see #78

[Martin-Oehler]

Ok, that makes sense. The combination of free space pointclouds and setting unknown voxels to occupied should solve my problem.

How exactly do you set all unknown space in the TSDF to occupied? Do you have some kind of "work area" where you just allocate every voxel? Otherwise there is theoretically an unlimited amount of unknown voxels.

[ZacharyTaylor]

Sorry for taking a while to get back to you. A quick way is to just set all the unknown voxels in allocated blocks to occupied, as this constrains it to the areas near observed space

[Martin-Oehler]

Ok, that should answer all my questions. Thanks for your thorough help.