Merge Myself

PRBonn · Jan 9, 2025 · e58c060 · e58c060
1 parent 18b102a
commit e58c060
Showing 1 changed file with 9 additions and 20 deletions.
diff --git a/cpp/kinematic_icp/registration/Registration.cpp b/cpp/kinematic_icp/registration/Registration.cpp
@@ -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>
 
@@ -37,7 +39,7 @@
 #include <tuple>
 
 using LinearSystem = std::pair<Eigen::Matrix2d, Eigen::Vector2d>;
-using Correspondences = std::vector<std::pair<Eigen::Vector3d, Eigen::Vector3d>>;
+using Correspondences = tbb::concurrent_vector<std::pair<Eigen::Vector3d, Eigen::Vector3d>>;
 
 namespace {
 constexpr double epsilon = std::numeric_limits<double>::min();
@@ -61,33 +63,20 @@ Correspondences DataAssociation(const std::vector<Eigen::Vector3d> &points,
                                 const Sophus::SE3d &T,
                                 const double max_correspondance_distance) {
     using points_iterator = std::vector<Eigen::Vector3d>::const_iterator;
-    Correspondences associations;
-    associations.reserve(points.size());
-    associations = tbb::parallel_reduce(
+    Correspondences correspondences;
+    correspondences.reserve(points.size());
+    tbb::parallel_for(
         // Range
         tbb::blocked_range<points_iterator>{points.cbegin(), points.cend()},
-        // Identity
-        associations,
-        // 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(T * 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 associations;
+    return correspondences;
 }
 
 Eigen::Vector2d ComputePerturbation(const Correspondences &correspondences,