Reusing octomap data from rosbag as a VoxelGrid

[tahsinkose]

Hey there, I have a collection of rosbag files with octomap data. I want to reproduce them in cuRobo as a curobo.geom.types.VoxelGrid. I adopted an octomap->curobo.geom.types.PointCloud->curobo.geom.types.Mesh conversion, but the result is not very satisfactory. See below:

Having those voxels as meshes is not very good, I would like to have proper voxels in a typical octomap. Is there a way to do that?

[balakumar-s]

You can try to increase the pitch when converting from pointcloud to mesh, that would help.

You cannot directly use octomap voxels in cuRobo as cuRobo requires signed distance. If you are using a depth camera, then you can use nvblox to generate a ESDF voxel map. Another approach is to compute a signed distance grid from octomap and use the voxel collision checker.

Here is an example of how to load a esdf grid into our collision checker:

curobo/benchmark/curobo_voxel_benchmark.py

Line 310 in 14b554b

world_voxel_collision.update_voxel_data(esdf)

[tahsinkose]

@balakumar-s thanks for the answer. The problem is that I don't have that ESDF grid in the first place, but I still want to be able to use the world description stored in octomap voxels 🙂 I managed to produce the following scene using trimesh.voxel.VoxelGrid.

I'm interested in the binary octomap format, i.e. the cells are marked only as free or occupied, not in their occupancy probabilities. In this simpler form, those voxels are basically cubes with center points. Thus the procedure boils down to:

// Calculate voxel indices based on the cube side length (resolution)
// Create an empty 3D encoding matrix of the required dimensions -- min and max of the voxel indices computed above.
// Fill these volume with 0 -- all free in the beginning
// Subtract the min indices from voxel indices to adjust the indexing.
// Mark adjusted indices as True (or 1.0) in the 3D encoding matrix.
// Create a homogeneous transformation matrix that specifies the offset from the center of the world to the closest corner of the volume.
voxel_grid = trimesh.voxel.VoxelGrid(encoding, transform=transform)
voxel_mesh = voxel_grid.as_boxes()
return curobo.geom.types.Mesh(name="octomap", pose=[0,0,0,1,0,0,0], vertices=voxel_mesh.vertices, faces=voxel_mesh.faces)

It is still a mesh after all, hence its collision-checking performance cannot be near of NV blox or a voxel grid, but it allows motion planning on the existing planning scenes.

Another approach is to compute a signed distance grid from octomap and use the voxel collision checker.

Any hints on this one? I read through cuMotion's update_voxel_grid, but it still requires a depth cam to exist, which would fill the underlying NVblox structure in parallel and return the ESDF data on request to be later processed in get_esdf_voxel_grid