Instrument NDT map extraction

[hidmic]

Feature description

Connected to #93. The vast majority of SLAM systems do not provide NDT maps. Whether it's occupancy grids, voxel grids, point clouds, meshes, pose graphs, we need a procedure or tool to turn these (at least one) into something we can populate NDT maps (#309) with.

Implementation considerations

https://github.com/cogsys-tuebingen/cslibs_ndt may be of interest.

[hidmic]

Some relevant notes and ideas.

• Conversion APIs in https://github.com/cogsys-tuebingen/cslibs_ndt do not seem to readily address this.

• The basic idea in Biber et. al. (https://ieeexplore.ieee.org/document/1249285) is simple though: just discretize 2D space, and compute normal distribution parameters for each cell that is not empty. Magnusson's dissertation has some interesting ideas on how to extend this to 3D efficiently.

• Considering that occupancy grids in ROS are usually ternary in value and do not actually convey any occupancy probability, it is trivial to derive an NDT map from them (i.e just act like every occupied grid cell was a laser hit and proceed with the algorithm). Resolution may be poor, but I think it is probably as good as it gets with this little information.

• We can do much better with serialized SLAM graphs that include range information. IIRC both Cartographer ROS and SLAM Toolbox offer those, in their own particular serialization format (the former uses Protobuf, the latter uses boost serialization formats).

• We are going to need a way to read and write these maps to file. I want to propose we standardize beluga I/O to use HDF5. It is designed for efficient I/O of large, multi-dimensional data, and it is mainstream enough for implementations to exist in a dozen languages (C++, C, Python, R, Rust, and a long etc.). It would make it very easy to work with beluga "artifacts" (e.g. for troubleshooting or benchmarking, using Python). Of course, this is in addition to any ROS messages and/or Rviz plugins we (or someone?) may eventually develop to work with these maps in ROS.

[serraramiro1]

@hidmic @nahueespinosa
On the HDF5 format:

I'm currently working on the serialization part of this issue.
I personally don't think this format is great for storing maps.

My argument is that the format is meant for flexible datasets, and for maps we want the exact opposite.
I think we want a strongly defined format specifically for maps. Whether that's protobuf, ROS messages, etc.

I can see us supporting HDF5 for benchmarking datasets but for maps I just don't see it.

On top of that, just from skimming through this my stomach started to ache 🤢 .

Thoughts?

[hidmic]

My argument is that the format is meant for flexible datasets, and for maps we want the exact opposite.

Hmm, how so? A collection of N-dimensional Gaussian distributions of M size can easily fit in a M * (N + N * (N + 1) / 2) matrix (i.e. M rows of N columns for the mean vector plus N * (N + 1) / 2 for the covariance matrix, which will always be symmetric). Ultimately, I think the decision goes back to #309 and how we want to deal with this data. It shouldn't be a pain to deserialize into our in-memory data structures.

I think we want a strongly defined format specifically for maps. Whether that's protobuf, ROS messages, etc.

The thing I don't love about those is that they are always painful to integrate. HDF5 (or NetCDF or Parquet or any of those) is easy to work with and well supported by people that is not us :)

What do you think @nahueespinosa?

On top of that, just from skimming through this my stomach started to ache 🤢 .

Oof, yeah, that type erased I/O isn't pretty.

[nahueespinosa]

I think we want a strongly defined format specifically for maps. Whether that's protobuf, ROS messages, etc.

I'm not well versed in storage formats, but HDF5 does seem like a widely-supported format. Also, unlike protobuf and ROS messages, it is self-describing. This isn't something we particularly need today, but it's certainly useful for external tools that don't need/want to create a dependency on our packages.

You probably have more experience than me, but HDF5 also supports data slicing which seems to lend itself well to map slicing for localization in big environments. Although we we're not going to solve that today.

[hidmic]

FWIW we could use HighFive instead of raw libhdf5.