Tiziano/Remove voxel block

[tizianoGuadagnino]

This PR aims to follow up on our discussion among the KISS core team about making the points contained in our beloved VoxelHashMap effective for data association.

My proposal here is to have points equally spaced into each voxel. Based on the fact that our 3d world can be represented as a set of (locally) 2d surfaces, we can compute the desired minimum distance between points (from now on called map_resolution) without adding parameter via:

$$\text{map resolution} = \sqrt(\frac{\text{voxel size}*\text{voxel size}}{\text{max points per voxel}})$$

I will now proceed to benchmark and update this PR on-demand so that we have a record of what happened.

Just remove the Voxel block as a struct, to have more control over the point addition in the VoxelHashMap

[benemer]

Two ideas from my side:

1. We can also compute the resolution by assuming equally sized 3D cubes instead of 2D surfaces, this leads to the formula

$$\text{map resolution} = \sqrt[3]{\frac{\text{voxel size}^3}{\text{max points per voxel}}}$$

Although the effect is probably not too large, it looks more correct to me.

2. How about exposing the map_resolution as parameter and then computing the max_number_of_points from the formula above? This is probably more intuitive than thinking about a specific number of points in a 3D voxel. For example, "I want a map with voxel size 1 meter and the points should be 50 cm apart" is more intuitive than "I want a map with voxel size 1 meter with a maximum of 8 points"

[tizianoGuadagnino]

Regarding (1), while the data structure represents 3d space, we know that the environment is made mainly by local 2d surfaces. If you look into voxels, you will realize that most of the space is non-occupied as we never measure a 3d volume but just its borders (or contour), which is locally 2d. If we use the formula you propose, this will result in points being farther away from each other than necessary.

The formula I propose is more consistent with reality in most cases and does not have a downside in my perspective.

Regarding (2), I see your point and agree, but I also have doubts that we are seeing things from an "expert" perspective. I am also not sure about changing the parameter set now. The community is already used to these parameters, and this change just uses the same parameters to get a more intelligent way of adding points. A config change is probably a bit too much for that. On this point, @nachovizzo can also give us his perspective.

[benemer]

I agree with small voxel sizes, but not necessarily with larger ones like the 1m default we have. Also, we still measure the distance to the map points in the volumetric 3D space, not on the surface.

Anyway, we can just finish this PR and then check both distance computations to see if it has a practical impact. I just wanted to raise this because, for me, this is the most general way of thinking about a point distribution in a 3D voxel without drawing an assumption about how this data is obtained or the environment looks like.

[benemer]

By the way, another way to achieve this spacing between map points would be to use a smaller voxel size and store only a single point per voxel. This would also wipe out the max_num_points parameter. I just see two challenges from this: 1. We need to make sure the source is not too dense, by maybe having a map voxel size and a downsampling voxel size and 2. the nearest neighbor search needs to be extended to search more neighbors, maybe depending on the association threshold we anyway impose.

[tizianoGuadagnino]

Runtime is not affected, and the accuracy also seems comparable overall. I think this is mostly a big improvement for sparse sensors. I will fix the windows build now.

[nachovizzo]

Runtime is not affected, and the accuracy also seems comparable overall. I think this is mostly a big improvement for sparse sensors. I will fix the windows build now.

This is fantastic

RE: runtime

The only "new computation I see" is

std::any_of(voxel_points.cbegin(), voxel_points.cend(),
            [&](const auto &voxel_point) {
                return (voxel_point - point).norm() < map_resolution;
            })) {

The rest are just "cosmetic" changes, the memory of the map should remain exactly the same, as we sitll store 20 points per voxel (although now they are equally spread)

Which makes this change more attractive

[tizianoGuadagnino]

Also, thanks to STL, this block:

std::any_of(voxel_points.cbegin(), voxel_points.cend(),
            [&](const auto &voxel_point) {
                return (voxel_point - point).norm() < map_resolution;
            })) {

Will stop the execution once it found the first occurrence in which the predicate is true, which runtime-wise, is fantastic for us.

[tizianoGuadagnino]

Ok, now it builds also on Windows (you cannot use the or keyword to do the or apparently). From my side, that is all; I will wait for any further check you want to do.