Reduce memory allocation

[tizianoGuadagnino]

Motivation

After doing some heaptrack analysis in PRBonn/kinematic-icp/pull/22, it turns out that our beloved system performs an insane amount of unnecessary allocations.

This PR

This change can be synthesized with "don't let that vector to reallocate for each point".

std::vector<Voxel> GetAdjacentVoxels(const Voxel &voxel, int adjacent_voxels = 1) {
    std::vector<Voxel> voxel_neighborhood; // <--- NO RESERVE HAS BEEN CALLED
    for (int i = voxel.x() - adjacent_voxels; i < voxel.x() + adjacent_voxels + 1; ++i) {
        for (int j = voxel.y() - adjacent_voxels; j < voxel.y() + adjacent_voxels + 1; ++j) {
            for (int k = voxel.z() - adjacent_voxels; k < voxel.z() + adjacent_voxels + 1; ++k) {
                voxel_neighborhood.emplace_back(i, j, k);
            }
        }
    }
    return voxel_neighborhood;
}

This code does not preallocate memory, and even worse, this reallocation happens for each point in the scan for which we are computing associations. The funny thing is that we perfectly know how these voxel offsets look, so I just precompute them. The result in terms of the number of allocations is incredible.

Results

Memory allocations