Bump version (#20)

* Migrate Python side

* Migrate Cpp

* No need

* Bump versions

* Bump versions

pyproject.toml

@@ -1,14 +1,14 @@
 [project]
 name = "mapmos"
-version = "0.0.1"
+version = "1.0.0"
 description = "Building Volumetric Beliefs for Dynamic Environments Exploiting Map-Based Moving Object Segmentation"
 readme = "README.md"
 authors = [
     { name = "Benedikt Mersch", email = "benedikt.mersch@gmail.com" },
 ]
 license_files = "LICENSE"
 dependencies = [
-    "kiss-icp<=0.4.0",
+    "kiss-icp>=1.0.0",
     "diskcache>=5.3.0",
     "pytorch_lightning>=1.6.4",
 ]

src/mapmos/datasets/mapmos_dataset.py

@@ -236,7 +236,7 @@ def get_scan_and_map(
         self.gt_map.remove_voxels_far_from_location(self.odometry.current_location())
 
         map_points = self.odometry.transform(
-            registered_map_points, np.linalg.inv(self.odometry.current_pose())
+            registered_map_points, np.linalg.inv(self.odometry.last_pose)
         )
 
         scan_timestamps = scan_index * np.ones(len(scan_points))

src/mapmos/odometry.py

@@ -25,8 +25,8 @@
 from typing import Type
 
 from kiss_icp.config import KISSConfig
-from kiss_icp.kiss_icp import KissICP
-from mapmos.registration import register_frame
+from kiss_icp.kiss_icp import KissICP, get_registration
+from mapmos.registration import get_registration
 from mapmos.mapping import VoxelHashMap
 
 
@@ -56,10 +56,11 @@ def __init__(
             max_distance=self.config.data.max_range,
             max_points_per_voxel=self.config.mapping.max_points_per_voxel,
         )
+        self.registration = get_registration(kiss_config)
 
     def register_points(self, points, timestamps, scan_index):
         # Apply motion compensation
-        points = self.compensator.deskew_scan(points, self.poses, timestamps)
+        points = self.compensator.deskew_scan(points, timestamps, self.last_delta)
 
         # Preprocess the input cloud
         points_prep = self.preprocess(points)
@@ -68,25 +69,29 @@ def register_points(self, points, timestamps, scan_index):
         source, points_downsample = self.voxelize(points_prep)
 
         # Get motion prediction and adaptive_threshold
-        sigma = self.get_adaptive_threshold()
+        sigma = self.adaptive_threshold.get_threshold()
 
         # Compute initial_guess for ICP
-        prediction = self.get_prediction_model()
-        initial_guess = self.current_pose() @ prediction
+        initial_guess = self.last_pose @ self.last_delta
 
-        new_pose = register_frame(
+        new_pose = self.registration.align_points_to_map(
             points=source,
             voxel_map=self.local_map,
             initial_guess=initial_guess,
             max_correspondance_distance=3 * sigma,
             kernel=sigma / 3,
         )
 
-        self.adaptive_threshold.update_model_deviation(np.linalg.inv(initial_guess) @ new_pose)
+        # Compute the difference between the prediction and the actual estimate
+        model_deviation = np.linalg.inv(initial_guess) @ new_pose
+
+        # Update step: threshold, local map, delta, and the last pose
+        self.adaptive_threshold.update_model_deviation(model_deviation)
         self.local_map.update(points_downsample, new_pose, scan_index)
-        self.poses.append(new_pose)
+        self.last_delta = np.linalg.inv(self.last_pose) @ new_pose
+        self.last_pose = new_pose
 
-        points_reg = self.transform(points, self.current_pose())
+        points_reg = self.transform(points, self.last_pose)
         return np.asarray(points_reg)
 
     def get_map_points(self):
@@ -98,11 +103,5 @@ def transform(self, points, pose):
         points = (pose @ points_hom.T).T[:, :3]
         return points
 
-    def get_poses(self):
-        return self.poses
-
-    def current_pose(self):
-        return self.poses[-1] if self.poses else np.eye(4)
-
     def current_location(self):
-        return self.current_pose()[:3, 3]
+        return self.last_pose[:3, 3]

src/mapmos/paper_pipeline.py

@@ -72,6 +72,7 @@ def _run_pipeline(self):
             local_scan, timestamps, gt_labels = self._next(scan_index)
             map_points, map_indices = self.odometry.get_map_points()
             scan_points = self.odometry.register_points(local_scan, timestamps, scan_index)
+            self.poses[scan_index - self._first] = self.odometry.last_pose
 
             min_range_mos = self.config.mos.min_range_mos
             max_range_mos = self.config.mos.max_range_mos
@@ -165,7 +166,7 @@ def _run_pipeline(self):
                     pred_labels_map,
                     belief_labels_scan,
                     belief_labels_map,
-                    self.odometry.current_pose(),
+                    self.odometry.last_pose,
                 )
 
             # Probabilistic volumetric fusion with scan and moving map predictions and delay
@@ -185,7 +186,7 @@ def _run_pipeline(self):
 
     def _run_evaluation(self):
         if self.has_gt:
-            self.results.eval_odometry(self.odometry.get_poses(), self.gt_poses)
+            self.results.eval_odometry(self.poses, self.gt_poses)
         self.results.eval_mos(self.confusion_matrix_mos, desc="\nScan Prediction")
         self.results.eval_fps(self.times_mos, desc="Average Frequency MOS")
         self.results.eval_mos(self.confusion_matrix_belief_scan_only, desc="\nBelief, Scan Only")

src/mapmos/pipeline.py

@@ -82,7 +82,7 @@ def __init__(
 
         # Results
         self.results = MOSPipelineResults()
-        self.poses = self.odometry.poses
+        self.poses = np.zeros((self._n_scans, 4, 4))
         self.has_gt = hasattr(self._dataset, "gt_poses")
         self.gt_poses = self._dataset.gt_poses[self._first : self._last] if self.has_gt else None
         self.dataset_name = self._dataset.__class__.__name__
@@ -127,6 +127,7 @@ def _run_pipeline(self):
             local_scan, timestamps, gt_labels = self._next(scan_index)
             map_points, map_indices = self.odometry.get_map_points()
             scan_points = self.odometry.register_points(local_scan, timestamps, scan_index)
+            self.poses[scan_index - self._first] = self.odometry.last_pose
 
             min_range_mos = self.config.mos.min_range_mos
             max_range_mos = self.config.mos.max_range_mos
@@ -187,7 +188,7 @@ def _run_pipeline(self):
                     pred_labels_map,
                     belief_labels_scan,
                     belief_labels_map,
-                    self.odometry.current_pose(),
+                    self.odometry.last_pose,
                 )
 
             # Evaluate and save with delay
@@ -238,7 +239,7 @@ def _next(self, idx):
 
     def _run_evaluation(self):
         if self.has_gt:
-            self.results.eval_odometry(self.odometry.get_poses(), self.gt_poses)
+            self.results.eval_odometry(self.poses, self.gt_poses)
         self.results.eval_mos(self.confusion_matrix_belief, desc="\nBelief")
         self.results.eval_fps(self.times_mos, desc="\nAverage Frequency MOS")
         self.results.eval_fps(self.times_belief, desc="Average Frequency Belief")

src/mapmos/pybind/Registration.cpp

@@ -23,92 +23,166 @@
 #include "Registration.hpp"
 
 #include <tbb/blocked_range.h>
+#include <tbb/global_control.h>
+#include <tbb/info.h>
 #include <tbb/parallel_reduce.h>
 
 #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
 
-namespace {
+using Correspondences = 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();
-    }
+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; });
+}
 
-    ResultTuple operator+(const ResultTuple &other) {
-        this->JTJ += other.JTJ;
-        this->JTr += other.JTr;
-        return *this;
+using Voxel = mapmos::VoxelHashMap::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;
+}
 
-    Eigen::Matrix6d JTJ;
-    Eigen::Vector6d JTr;
-};
+std::tuple<Eigen::Vector3d, double> GetClosestNeighbor(const Eigen::Vector3d &point,
+                                                       const mapmos::VoxelHashMap &voxel_map) {
+    // Convert the point to voxel coordinates
+    const auto &voxel = voxel_map.PointToVoxel(point);
+    // Get nearby voxels on the map
+    const auto &query_voxels = GetAdjacentVoxels(voxel);
+    // Extract the points contained within the neighborhood voxels
+    const auto &neighbors = voxel_map.GetPoints(query_voxels);
+
+    // Find the nearest neighbor
+    Eigen::Vector3d closest_neighbor;
+    double closest_distance = std::numeric_limits<double>::max();
+    std::for_each(neighbors.cbegin(), neighbors.cend(), [&](const auto &neighbor) {
+        double distance = (neighbor - point).norm();
+        if (distance < closest_distance) {
+            closest_neighbor = neighbor;
+            closest_distance = distance;
+        }
+    });
+    return std::make_tuple(closest_neighbor, closest_distance);
+}
 
-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; });
+Correspondences DataAssociation(const std::vector<Eigen::Vector3d> &points,
+                                const mapmos::VoxelHashMap &voxel_map,
+                                const double max_correspondance_distance) {
+    using points_iterator = std::vector<Eigen::Vector3d>::const_iterator;
+    Correspondences correspondences;
+    correspondences.reserve(points.size());
+    correspondences = tbb::parallel_reduce(
+        // 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());
+            std::for_each(r.begin(), r.end(), [&](const auto &point) {
+                const auto &[closest_neighbor, distance] = GetClosestNeighbor(point, voxel_map);
+                if (distance < max_correspondance_distance) {
+                    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;
 }
 
-Sophus::SE3d AlignClouds(const std::vector<Eigen::Vector3d> &source,
-                         const std::vector<Eigen::Vector3d> &target,
-                         double th) {
-    auto compute_jacobian_and_residual = [&](auto i) {
-        const Eigen::Vector3d residual = source[i] - target[i];
+LinearSystem BuildLinearSystem(const Correspondences &correspondences, const double kernel_scale) {
+    auto compute_jacobian_and_residual = [](const auto &correspondence) {
+        const auto &[source, target] = correspondence;
+        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 = [&](const double &residual2) {
+        return square(kernel_scale) / square(kernel_scale + residual2);
+    };
+
+    using correspondence_iterator = Correspondences::const_iterator;
     const auto &[JTJ, JTr] = tbb::parallel_reduce(
         // Range
-        tbb::blocked_range<size_t>{0, source.size()},
+        tbb::blocked_range<correspondence_iterator>{correspondences.cbegin(),
+                                                    correspondences.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(th) / square(th + 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<correspondence_iterator> &r, LinearSystem J) -> LinearSystem {
+            return std::transform_reduce(
+                r.begin(), r.end(), J, sum_linear_systems, [&](const auto &correspondence) {
+                    const auto &[J_r, residual] = compute_jacobian_and_residual(correspondence);
+                    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);
 
-    const Eigen::Vector6d x = JTJ.ldlt().solve(-JTr);
-    return Sophus::SE3d::exp(x);
+    return {JTJ, JTr};
 }
-
-constexpr int MAX_NUM_ITERATIONS_ = 500;
-constexpr double ESTIMATION_THRESHOLD_ = 0.0001;
-
 }  // namespace
 
 namespace mapmos {
 
-Sophus::SE3d RegisterFrame(const std::vector<Eigen::Vector3d> &frame,
-                           const VoxelHashMap &voxel_map,
-                           const Sophus::SE3d &initial_guess,
-                           double max_correspondence_distance,
-                           double kernel) {
+Registration::Registration(int max_num_iteration, double convergence_criterion, int max_num_threads)
+    : max_num_iterations_(max_num_iteration),
+      convergence_criterion_(convergence_criterion),
+      // Only manipulate the number of threads if the user specifies something greater than 0
+      max_num_threads_(max_num_threads > 0 ? max_num_threads : tbb::info::default_concurrency()) {
+    // This global variable requires static duration storage to be able to manipulate the max
+    // concurrency from TBB across the entire class
+    static const auto tbb_control_settings = tbb::global_control(
+        tbb::global_control::max_allowed_parallelism, static_cast<size_t>(max_num_threads_));
+}
+
+Sophus::SE3d Registration::AlignPointsToMap(const std::vector<Eigen::Vector3d> &frame,
+                                            const VoxelHashMap &voxel_map,
+                                            const Sophus::SE3d &initial_guess,
+                                            const double max_distance,
+                                            const double kernel_scale) {
     if (voxel_map.Empty()) return initial_guess;
 
     // Equation (9)
@@ -117,17 +191,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 correspondences = DataAssociation(source, voxel_map, max_distance);
         // Equation (11)
-        auto estimation = AlignClouds(src, tgt, kernel);
+        const auto &[JTJ, JTr] = BuildLinearSystem(correspondences, kernel_scale);
+        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 (estimation.log().norm() < ESTIMATION_THRESHOLD_) break;
+        if (dx.norm() < convergence_criterion_) break;
     }
     // Spit the final transformation
     return T_icp * initial_guess;