Question about querying the esdf for distance and gradient

[c-salmi]

Hi,

First of all thanks for making this code publicly available! It's great the generation of an accurate ESDF is working so well.

I have a question about how to most easily query the distance and gradient of a point (x,y,z) or list of points, when using the 3D esdf generation. I did not find a corresponding method to call from the Mapper class for requested point(s).

Would highly appreciate it if you could point me in the right direction on where to look/modify nvblox to query for distance and gradient. Thanks in advance!

[alexmillane]

To get the ESDF value and gradient you need to query voxels in the ESDF layer. Once you have an ESDF voxel you can read it's value and compute the gradient from the voxel's "parent".

How you query an ESDF voxel depends on where you want the resulting value. There are 3 options.

You want the resulting values on the CPU Use this function to grab ESDF voxels in a CPU vector https://github.com/nvidia-isaac/nvblox/blob/public/nvblox/include/nvblox/core/layer.h#L171

You want the resulting values in a GPU vector We implement a function similar to the function above in a release due in the next couple of weeks. This function will return a device vector rather than a host vector.

You want the resulting value inside a GPU kernel We have an example for querying calculating distances and gradients inside a kernel here: https://github.com/nvidia-isaac/nvblox/blob/public/nvblox/examples/src/esdf_query.cu

So there are a few options. Hope this helps.

[c-salmi]

Thanks for the quick and elaborate response! We want the distance and gradient for our motion planner that runs on cpu, so I will try to add a service in nvblox_ros that calls the getVoxels method for a requested list of points.

[haowu-figure]

To get the ESDF value and gradient you need to query voxels in the ESDF layer. Once you have an ESDF voxel you can read it's value and compute the gradient from the voxel's "parent".

How you query an ESDF voxel depends on where you want the resulting value. There are 3 options.

You want the resulting values on the CPU Use this function to grab ESDF voxels in a CPU vector https://github.com/nvidia-isaac/nvblox/blob/public/nvblox/include/nvblox/core/layer.h#L171

You want the resulting values in a GPU vector We implement a function similar to the function above in a release due in the next couple of weeks. This function will return a device vector rather than a host vector.

You want the resulting value inside a GPU kernel We have an example for querying calculating distances and gradients inside a kernel here: https://github.com/nvidia-isaac/nvblox/blob/public/nvblox/examples/src/esdf_query.cu

So there are a few options. Hope this helps.

Hi, could you elaborate a bit more on what you mean by “compute the gradient from the voxel's parent”?

Thanks!

[alexmillane]

Thanks for your interest in nvblox

For each EsdfVoxel, the EsdfVoxel::parent_direction is the relative position of the closest voxel on the surface. The (ideal) distance function has a gradient of magnitude 1 pointing towards this point. So you could take the parent direction, normalize it and this is the negative gradient direction.

Does that help?

[haowu-figure]

Yes. Really appreciate it. Does that also apply to the 2D esdf?