Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Reduce memory allocation #410

Open
wants to merge 6 commits into
base: main
Choose a base branch
from
Open

Reduce memory allocation #410

Show file tree
Hide file tree
Changes from 2 commits
Commits
Show all changes
6 commits
Select commit Hold shift + click to select a range
1fdd09b
Zero additional allocations
tizianoGuadagnino Dec 4, 2024
a741512
We know this shifts
tizianoGuadagnino Dec 4, 2024
5ae3283
Revert VoxelHashMap change -> Allocations go in a separate PR
tizianoGuadagnino Dec 4, 2024
4521454
Revert "Revert VoxelHashMap change -> Allocations go in a separate PR"
tizianoGuadagnino Dec 4, 2024
6e56ac7
Revert concurrent vector change
tizianoGuadagnino Dec 4, 2024
a094695
Some renaming for clarity
tizianoGuadagnino Dec 5, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
23 changes: 6 additions & 17 deletions cpp/kiss_icp/core/Registration.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -23,8 +23,10 @@
#include "Registration.hpp"

#include <tbb/blocked_range.h>
#include <tbb/concurrent_vector.h>
#include <tbb/global_control.h>
#include <tbb/info.h>
#include <tbb/parallel_for.h>
#include <tbb/parallel_reduce.h>
#include <tbb/task_arena.h>

Expand All @@ -44,7 +46,7 @@ using Matrix3_6d = Eigen::Matrix<double, 3, 6>;
using Vector6d = Eigen::Matrix<double, 6, 1>;
} // namespace Eigen

using Correspondences = std::vector<std::pair<Eigen::Vector3d, Eigen::Vector3d>>;
using Correspondences = tbb::concurrent_vector<std::pair<Eigen::Vector3d, Eigen::Vector3d>>;
using LinearSystem = std::pair<Eigen::Matrix6d, Eigen::Vector6d>;

namespace {
Expand All @@ -61,30 +63,17 @@ Correspondences DataAssociation(const std::vector<Eigen::Vector3d> &points,
using points_iterator = std::vector<Eigen::Vector3d>::const_iterator;
Correspondences correspondences;
correspondences.reserve(points.size());
correspondences = tbb::parallel_reduce(
tbb::parallel_for(
// Range
tbb::blocked_range<points_iterator>{points.cbegin(), points.cend()},
// Identity
correspondences,
// 1st lambda: Parallel computation
[&](const tbb::blocked_range<points_iterator> &r, Correspondences res) -> Correspondences {
res.reserve(r.size());
[&](const tbb::blocked_range<points_iterator> &r) {
std::for_each(r.begin(), r.end(), [&](const auto &point) {
const auto &[closest_neighbor, distance] = voxel_map.GetClosestNeighbor(point);
if (distance < max_correspondance_distance) {
res.emplace_back(point, closest_neighbor);
correspondences.emplace_back(point, closest_neighbor);
}
});
return res;
},
// 2nd lambda: Parallel reduction
[](Correspondences a, const Correspondences &b) -> Correspondences {
a.insert(a.end(), //
std::make_move_iterator(b.cbegin()), //
std::make_move_iterator(b.cend()));
return a;
});

return correspondences;
}

Expand Down
22 changes: 7 additions & 15 deletions cpp/kiss_icp/core/VoxelHashMap.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -31,18 +31,11 @@

namespace {
using kiss_icp::Voxel;

std::vector<Voxel> GetAdjacentVoxels(const Voxel &voxel, int adjacent_voxels = 1) {
std::vector<Voxel> voxel_neighborhood;
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;
}
static const std::array<Voxel, 27> shifts{
Copy link
Collaborator

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

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

Copy link
Collaborator Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

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

Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

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.

{{0, 0, 0}, {1, 0, 0}, {-1, 0, 0}, {0, 1, 0}, {0, -1, 0}, {0, 0, 1}, {0, 0, -1},
{1, 1, 0}, {1, -1, 0}, {-1, 1, 0}, {-1, -1, 0}, {1, 0, 1}, {1, 0, -1}, {-1, 0, 1},
{-1, 0, -1}, {0, 1, 1}, {0, 1, -1}, {0, -1, 1}, {0, -1, -1}, {1, 1, 1}, {1, 1, -1},
{1, -1, 1}, {1, -1, -1}, {-1, 1, 1}, {-1, 1, -1}, {-1, -1, 1}, {-1, -1, -1}}};
} // namespace

namespace kiss_icp {
Expand All @@ -51,12 +44,11 @@ std::tuple<Eigen::Vector3d, double> VoxelHashMap::GetClosestNeighbor(
const Eigen::Vector3d &query) const {
// Convert the point to voxel coordinates
const auto &voxel = PointToVoxel(query, voxel_size_);
// Get nearby voxels on the map
const auto &query_voxels = GetAdjacentVoxels(voxel);
// Find the nearest neighbor
Eigen::Vector3d closest_neighbor = Eigen::Vector3d::Zero();
double closest_distance = std::numeric_limits<double>::max();
std::for_each(query_voxels.cbegin(), query_voxels.cend(), [&](const auto &query_voxel) {
std::for_each(shifts.cbegin(), shifts.cend(), [&](const auto &voxel_shift) {
const auto &query_voxel = voxel + voxel_shift;
auto search = map_.find(query_voxel);
if (search != map_.end()) {
const auto &points = search.value();
Expand Down
Loading