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

[nachovizzo]

Something I tried moons ago, and also worked fine (mainly style):

const std::array<Voxel, 27> &GetAdjacentVoxels() {
    static const auto ADJACENT_VOXELS = [&]() -> std::array<Voxel, 27> {
        std::array<Voxel, 27> output;
        // clang-format off
        size_t idx = 0;
        for (int i = -1; i <= 1 ; ++i) {
        for (int j = -1; j <= 1 ; ++j) {
        for (int k = -1; k <= 1 ; ++k) {
            output[idx++] = Voxel{i,j,k};
        }}}
        // clang-format on
        return output;
    }();
    return ADJACENT_VOXELS;
}

And down the line:

std::array<Voxel, 27> GetVoxelNeighborhood(const Voxel &voxel) {
    auto voxel_neighborhood = GetAdjacentVoxels();
    for (auto &adjacent_voxel : voxel_neighborhood) adjacent_voxel += voxel;
    return voxel_neighborhood;
}

This is for "more readability." as later we will "search in the neighboring voxels", instead of the shift+voxel

[tizianoGuadagnino]

To me, it looks a bit too complicated for what it should be. I will compile the decision using -Wpedantic @benemer ;)

[benemer]

I don't have a strong opinion on this. I see one advantage with Nacho's solution: it is easier to consider more than one neighboring voxel (in case you need to have a small voxel size but still find nearest neighbors).

How about a compromise?

static const std::array<Voxel, 27> voxel_shifts = []() {
    std::array<Voxel, 27> output;
    size_t idx = 0;
    for (int i = -1; i <= 1; ++i) {
        for (int j = -1; j <= 1; ++j) {
            for (int k = -1; k <= 1; ++k) {
                output[idx++] = Voxel{i, j, k};
            }
        }
    }
    return output;
}();

This keeps the simplicity but allows better readability and simpler extension to more neighboring voxels.