Remove unnecessary exposed utility function from ROS API

.github/CODEOWNERS

@@ -0,0 +1 @@
+* @nachovizzo @benemer @tizianoGuadagnino

config/advanced.yaml

@@ -16,3 +16,7 @@ adaptive_threshold:
   fixed_threshold: 0.3 # <- optional, disables adaptive threshold
   # initial_threshold: 2.0
   # min_motion_th: 0.1
+
+registration:
+  max_num_iterations: 500 # <- optional
+  convergence_criterion: 0.0001 # <- optional

cpp/kiss_icp/core/Registration.cpp

@@ -27,89 +27,126 @@
 
 #include <algorithm>
 #include <cmath>
+#include <numeric>
 #include <sophus/se3.hpp>
 #include <sophus/so3.hpp>
 #include <tuple>
 
+#include "VoxelHashMap.hpp"
+
 namespace Eigen {
 using Matrix6d = Eigen::Matrix<double, 6, 6>;
 using Matrix3_6d = Eigen::Matrix<double, 3, 6>;
 using Vector6d = Eigen::Matrix<double, 6, 1>;
 }  // namespace Eigen
+using Associations = std::vector<std::pair<Eigen::Vector3d, Eigen::Vector3d>>;
+using LinearSystem = std::pair<Eigen::Matrix6d, Eigen::Vector6d>;
 
 namespace {
-
 inline double square(double x) { return x * x; }
 
-struct ResultTuple {
-    ResultTuple() {
-        JTJ.setZero();
-        JTr.setZero();
-    }
-
-    ResultTuple operator+(const ResultTuple &other) {
-        this->JTJ += other.JTJ;
-        this->JTr += other.JTr;
-        return *this;
-    }
-
-    Eigen::Matrix6d JTJ;
-    Eigen::Vector6d JTr;
-};
-
 void TransformPoints(const Sophus::SE3d &T, std::vector<Eigen::Vector3d> &points) {
     std::transform(points.cbegin(), points.cend(), points.begin(),
                    [&](const auto &point) { return T * point; });
 }
 
-constexpr int MAX_NUM_ITERATIONS_ = 500;
-constexpr double ESTIMATION_THRESHOLD_ = 0.0001;
+Eigen::Vector3d GetClosestNeighbor(const Eigen::Vector3d &point,
+                                   const kiss_icp::VoxelHashMap &voxel_map) {
+    const auto &query_voxels = voxel_map.GetAdjacentVoxels(point);
+    const auto &neighbors = voxel_map.GetPoints(query_voxels);
+    Eigen::Vector3d closest_neighbor;
+    double closest_distance2 = std::numeric_limits<double>::max();
+    std::for_each(neighbors.cbegin(), neighbors.cend(), [&](const auto &neighbor) {
+        double distance = (neighbor - point).squaredNorm();
+        if (distance < closest_distance2) {
+            closest_neighbor = neighbor;
+            closest_distance2 = distance;
+        }
+    });
+    return closest_neighbor;
+}
+
+Associations FindAssociations(const std::vector<Eigen::Vector3d> &points,
+                              const kiss_icp::VoxelHashMap &voxel_map,
+                              double max_correspondance_distance) {
+    using points_iterator = std::vector<Eigen::Vector3d>::const_iterator;
+    Associations associations;
+    associations.reserve(points.size());
+    associations = tbb::parallel_reduce(
+        // Range
+        tbb::blocked_range<points_iterator>{points.cbegin(), points.cend()},
+        // Identity
+        associations,
+        // 1st lambda: Parallel computation
+        [&](const tbb::blocked_range<points_iterator> &r, Associations res) -> Associations {
+            res.reserve(r.size());
+            for (const auto &point : r) {
+                Eigen::Vector3d closest_neighbor = GetClosestNeighbor(point, voxel_map);
+                if ((closest_neighbor - point).norm() < max_correspondance_distance) {
+                    res.emplace_back(point, closest_neighbor);
+                }
+            }
+            return res;
+        },
+        // 2nd lambda: Parallel reduction
+        [](Associations a, const Associations &b) -> Associations {
+            a.insert(a.end(),                              //
+                     std::make_move_iterator(b.cbegin()),  //
+                     std::make_move_iterator(b.cend()));
+            return a;
+        });
+
+    return associations;
+}
 
-std::tuple<Eigen::Matrix6d, Eigen::Vector6d> BuildLinearSystem(
-    const std::vector<Eigen::Vector3d> &source,
-    const std::vector<Eigen::Vector3d> &target,
-    double kernel) {
-    auto compute_jacobian_and_residual = [&](auto i) {
-        const Eigen::Vector3d residual = source[i] - target[i];
+LinearSystem BuildLinearSystem(const Associations &associations, double kernel) {
+    auto compute_jacobian_and_residual = [](auto association) {
+        const auto &[source, target] = association;
+        const Eigen::Vector3d residual = source - target;
         Eigen::Matrix3_6d J_r;
         J_r.block<3, 3>(0, 0) = Eigen::Matrix3d::Identity();
-        J_r.block<3, 3>(0, 3) = -1.0 * Sophus::SO3d::hat(source[i]);
+        J_r.block<3, 3>(0, 3) = -1.0 * Sophus::SO3d::hat(source);
         return std::make_tuple(J_r, residual);
     };
 
+    auto sum_linear_systems = [](LinearSystem a, const LinearSystem &b) {
+        a.first += b.first;
+        a.second += b.second;
+        return a;
+    };
+
+    auto GM_weight = [&](double residual2) { return square(kernel) / square(kernel + residual2); };
+
+    using associations_iterator = Associations::const_iterator;
     const auto &[JTJ, JTr] = tbb::parallel_reduce(
         // Range
-        tbb::blocked_range<size_t>{0, source.size()},
+        tbb::blocked_range<associations_iterator>{associations.cbegin(), associations.cend()},
         // Identity
-        ResultTuple(),
+        LinearSystem(Eigen::Matrix6d::Zero(), Eigen::Vector6d::Zero()),
         // 1st Lambda: Parallel computation
-        [&](const tbb::blocked_range<size_t> &r, ResultTuple J) -> ResultTuple {
-            auto Weight = [&](double residual2) {
-                return square(kernel) / square(kernel + residual2);
-            };
-            auto &[JTJ_private, JTr_private] = J;
-            for (auto i = r.begin(); i < r.end(); ++i) {
-                const auto &[J_r, residual] = compute_jacobian_and_residual(i);
-                const double w = Weight(residual.squaredNorm());
-                JTJ_private.noalias() += J_r.transpose() * w * J_r;
-                JTr_private.noalias() += J_r.transpose() * w * residual;
-            }
-            return J;
+        [&](const tbb::blocked_range<associations_iterator> &r, LinearSystem J) -> LinearSystem {
+            return std::transform_reduce(
+                r.begin(), r.end(), J, sum_linear_systems, [&](const auto &association) {
+                    const auto &[J_r, residual] = compute_jacobian_and_residual(association);
+                    const double w = GM_weight(residual.squaredNorm());
+                    return LinearSystem(J_r.transpose() * w * J_r,        // JTJ
+                                        J_r.transpose() * w * residual);  // JTr
+                });
         },
         // 2nd Lambda: Parallel reduction of the private Jacboians
-        [&](ResultTuple a, const ResultTuple &b) -> ResultTuple { return a + b; });
+        sum_linear_systems);
 
-    return std::make_tuple(JTJ, JTr);
+    return {JTJ, JTr};
 }
 }  // namespace
 
 namespace kiss_icp {
 
-Sophus::SE3d RegisterFrame(const std::vector<Eigen::Vector3d> &frame,
-                           const VoxelHashMap &voxel_map,
-                           const Sophus::SE3d &initial_guess,
-                           double max_correspondence_distance,
-                           double kernel) {
+Sophus::SE3d Registration::AlignPointsToMap(const std::vector<Eigen::Vector3d> &frame,
+                                            const VoxelHashMap &voxel_map,
+                                            const Sophus::SE3d &initial_guess,
+                                            double max_correspondence_distance,
+                                            double kernel) {
     if (voxel_map.Empty()) return initial_guess;
 
     // Equation (9)
@@ -118,19 +155,19 @@ Sophus::SE3d RegisterFrame(const std::vector<Eigen::Vector3d> &frame,
 
     // ICP-loop
     Sophus::SE3d T_icp = Sophus::SE3d();
-    for (int j = 0; j < MAX_NUM_ITERATIONS_; ++j) {
+    for (int j = 0; j < max_num_iterations_; ++j) {
         // Equation (10)
-        const auto &[src, tgt] = voxel_map.GetCorrespondences(source, max_correspondence_distance);
+        const auto associations = FindAssociations(source, voxel_map, max_correspondence_distance);
         // Equation (11)
-        const auto &[JTJ, JTr] = BuildLinearSystem(src, tgt, kernel);
+        const auto &[JTJ, JTr] = BuildLinearSystem(associations, kernel);
         const Eigen::Vector6d dx = JTJ.ldlt().solve(-JTr);
         const Sophus::SE3d estimation = Sophus::SE3d::exp(dx);
         // Equation (12)
         TransformPoints(estimation, source);
         // Update iterations
         T_icp = estimation * T_icp;
         // Termination criteria
-        if (dx.norm() < ESTIMATION_THRESHOLD_) break;
+        if (dx.norm() < convergence_criterion_) break;
     }
     // Spit the final transformation
     return T_icp * initial_guess;

cpp/kiss_icp/core/Registration.hpp

@@ -30,9 +30,17 @@
 
 namespace kiss_icp {
 
-Sophus::SE3d RegisterFrame(const std::vector<Eigen::Vector3d> &frame,
-                           const VoxelHashMap &voxel_map,
-                           const Sophus::SE3d &initial_guess,
-                           double max_correspondence_distance,
-                           double kernel);
+struct Registration {
+    explicit Registration(int max_num_iteration, double convergence_criterion)
+        : max_num_iterations_(max_num_iteration), convergence_criterion_(convergence_criterion) {}
+
+    Sophus::SE3d AlignPointsToMap(const std::vector<Eigen::Vector3d> &frame,
+                                  const VoxelHashMap &voxel_map,
+                                  const Sophus::SE3d &initial_guess,
+                                  double max_correspondence_distance,
+                                  double kernel);
+
+    int max_num_iterations_;
+    double convergence_criterion_;
+};
 }  // namespace kiss_icp

cpp/kiss_icp/core/VoxelHashMap.cpp

@@ -22,107 +22,43 @@
 // SOFTWARE.
 #include "VoxelHashMap.hpp"
 
-#include <tbb/blocked_range.h>
-#include <tbb/parallel_reduce.h>
-
 #include <Eigen/Core>
 #include <algorithm>
 #include <limits>
 #include <tuple>
 #include <utility>
 #include <vector>
 
-// This parameters are not intended to be changed, therefore we do not expose it
-namespace {
-struct ResultTuple {
-    ResultTuple(std::size_t n) {
-        source.reserve(n);
-        target.reserve(n);
-    }
-    std::vector<Eigen::Vector3d> source;
-    std::vector<Eigen::Vector3d> target;
-};
-}  // namespace
-
 namespace kiss_icp {
 
-VoxelHashMap::Vector3dVectorTuple VoxelHashMap::GetCorrespondences(
-    const Vector3dVector &points, double max_correspondance_distance) const {
-    // Lambda Function to obtain the KNN of one point, maybe refactor
-    auto GetClosestNeighboor = [&](const Eigen::Vector3d &point) {
-        auto kx = static_cast<int>(point[0] / voxel_size_);
-        auto ky = static_cast<int>(point[1] / voxel_size_);
-        auto kz = static_cast<int>(point[2] / voxel_size_);
-        std::vector<Voxel> voxels;
-        voxels.reserve(27);
-        for (int i = kx - 1; i < kx + 1 + 1; ++i) {
-            for (int j = ky - 1; j < ky + 1 + 1; ++j) {
-                for (int k = kz - 1; k < kz + 1 + 1; ++k) {
-                    voxels.emplace_back(i, j, k);
-                }
+std::vector<VoxelHashMap::Voxel> VoxelHashMap::GetAdjacentVoxels(const Eigen::Vector3d &point,
+                                                                 int adjacent_voxels) const {
+    auto kx = static_cast<int>(point[0] / voxel_size_);
+    auto ky = static_cast<int>(point[1] / voxel_size_);
+    auto kz = static_cast<int>(point[2] / voxel_size_);
+    std::vector<Voxel> voxel_neighborhood;
+    for (int i = kx - adjacent_voxels; i < kx + adjacent_voxels + 1; ++i) {
+        for (int j = ky - adjacent_voxels; j < ky + adjacent_voxels + 1; ++j) {
+            for (int k = kz - adjacent_voxels; k < kz + adjacent_voxels + 1; ++k) {
+                voxel_neighborhood.emplace_back(i, j, k);
             }
         }
+    }
+    return voxel_neighborhood;
+}
 
-        using Vector3dVector = std::vector<Eigen::Vector3d>;
-        Vector3dVector neighboors;
-        neighboors.reserve(27 * max_points_per_voxel_);
-        std::for_each(voxels.cbegin(), voxels.cend(), [&](const auto &voxel) {
-            auto search = map_.find(voxel);
-            if (search != map_.end()) {
-                const auto &points = search->second.points;
-                if (!points.empty()) {
-                    for (const auto &point : points) {
-                        neighboors.emplace_back(point);
-                    }
-                }
-            }
-        });
-
-        Eigen::Vector3d closest_neighbor;
-        double closest_distance2 = std::numeric_limits<double>::max();
-        std::for_each(neighboors.cbegin(), neighboors.cend(), [&](const auto &neighbor) {
-            double distance = (neighbor - point).squaredNorm();
-            if (distance < closest_distance2) {
-                closest_neighbor = neighbor;
-                closest_distance2 = distance;
-            }
-        });
-
-        return closest_neighbor;
-    };
-    using points_iterator = std::vector<Eigen::Vector3d>::const_iterator;
-    const auto [source, target] = tbb::parallel_reduce(
-        // Range
-        tbb::blocked_range<points_iterator>{points.cbegin(), points.cend()},
-        // Identity
-        ResultTuple(points.size()),
-        // 1st lambda: Parallel computation
-        [max_correspondance_distance, &GetClosestNeighboor](
-            const tbb::blocked_range<points_iterator> &r, ResultTuple res) -> ResultTuple {
-            auto &[src, tgt] = res;
-            src.reserve(r.size());
-            tgt.reserve(r.size());
-            for (const auto &point : r) {
-                Eigen::Vector3d closest_neighboors = GetClosestNeighboor(point);
-                if ((closest_neighboors - point).norm() < max_correspondance_distance) {
-                    src.emplace_back(point);
-                    tgt.emplace_back(closest_neighboors);
-                }
+std::vector<Eigen::Vector3d> VoxelHashMap::GetPoints(const std::vector<Voxel> &query_voxels) const {
+    std::vector<Eigen::Vector3d> points;
+    points.reserve(query_voxels.size() * static_cast<size_t>(max_points_per_voxel_));
+    std::for_each(query_voxels.cbegin(), query_voxels.cend(), [&](const auto &query) {
+        auto search = map_.find(query);
+        if (search != map_.end()) {
+            for (const auto &point : search->second.points) {
+                points.emplace_back(point);
             }
-            return res;
-        },
-        // 2nd lambda: Parallel reduction
-        [](ResultTuple a, const ResultTuple &b) -> ResultTuple {
-            auto &[src, tgt] = a;
-            const auto &[srcp, tgtp] = b;
-            src.insert(src.end(),  //
-                       std::make_move_iterator(srcp.begin()), std::make_move_iterator(srcp.end()));
-            tgt.insert(tgt.end(),  //
-                       std::make_move_iterator(tgtp.begin()), std::make_move_iterator(tgtp.end()));
-            return a;
-        });
-
-    return std::make_tuple(source, target);
+        }
+    });
+    return points;
 }
 
 std::vector<Eigen::Vector3d> VoxelHashMap::Pointcloud() const {
@@ -137,13 +73,14 @@ std::vector<Eigen::Vector3d> VoxelHashMap::Pointcloud() const {
     return points;
 }
 
-void VoxelHashMap::Update(const Vector3dVector &points, const Eigen::Vector3d &origin) {
+void VoxelHashMap::Update(const std::vector<Eigen::Vector3d> &points,
+                          const Eigen::Vector3d &origin) {
     AddPoints(points);
     RemovePointsFarFromLocation(origin);
 }
 
-void VoxelHashMap::Update(const Vector3dVector &points, const Sophus::SE3d &pose) {
-    Vector3dVector points_transformed(points.size());
+void VoxelHashMap::Update(const std::vector<Eigen::Vector3d> &points, const Sophus::SE3d &pose) {
+    std::vector<Eigen::Vector3d> points_transformed(points.size());
     std::transform(points.cbegin(), points.cend(), points_transformed.begin(),
                    [&](const auto &point) { return pose * point; });
     const Eigen::Vector3d &origin = pose.translation();