diff --git a/cpp/kiss_icp/core/Registration.cpp b/cpp/kiss_icp/core/Registration.cpp index 6bd1fb75..ded9fb61 100644 --- a/cpp/kiss_icp/core/Registration.cpp +++ b/cpp/kiss_icp/core/Registration.cpp @@ -23,10 +23,8 @@ #include "Registration.hpp" #include -#include #include #include -#include #include #include @@ -46,7 +44,7 @@ using Matrix3_6d = Eigen::Matrix; using Vector6d = Eigen::Matrix; } // namespace Eigen -using Correspondences = tbb::concurrent_vector>; +using Correspondences = std::vector>; using LinearSystem = std::pair; namespace { @@ -63,17 +61,30 @@ Correspondences DataAssociation(const std::vector &points, using points_iterator = std::vector::const_iterator; Correspondences correspondences; correspondences.reserve(points.size()); - tbb::parallel_for( + correspondences = tbb::parallel_reduce( // Range tbb::blocked_range{points.cbegin(), points.cend()}, - [&](const tbb::blocked_range &r) { + // Identity + correspondences, + // 1st lambda: Parallel computation + [&](const tbb::blocked_range &r, Correspondences res) -> Correspondences { + res.reserve(r.size()); std::for_each(r.begin(), r.end(), [&](const auto &point) { const auto &[closest_neighbor, distance] = voxel_map.GetClosestNeighbor(point); if (distance < max_correspondance_distance) { - correspondences.emplace_back(point, closest_neighbor); + res.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; }