Refactor KissICP pipeline to mantain just 1 pose instead of full trajectory. Fixes #122

cpp/kiss_icp/core/Deskew.cpp

@@ -38,7 +38,14 @@ std::vector<Eigen::Vector3d> DeSkewScan(const std::vector<Eigen::Vector3d> &fram
                                         const std::vector<double> &timestamps,
                                         const Sophus::SE3d &start_pose,
                                         const Sophus::SE3d &finish_pose) {
-    const auto delta_pose = (start_pose.inverse() * finish_pose).log();
+    const auto delta = start_pose.inverse() * finish_pose;
+    return DeSkewScan(frame, timestamps, delta);
+}
+
+std::vector<Eigen::Vector3d> DeSkewScan(const std::vector<Eigen::Vector3d> &frame,
+                                        const std::vector<double> &timestamps,
+                                        const Sophus::SE3d &delta) {
+    const auto delta_pose = delta.log();
     std::vector<Eigen::Vector3d> corrected_frame(frame.size());
     tbb::parallel_for(size_t(0), frame.size(), [&](size_t i) {
         const auto motion = Sophus::SE3d::exp((timestamps[i] - mid_pose_timestamp) * delta_pose);

cpp/kiss_icp/core/Deskew.hpp

@@ -34,4 +34,8 @@ std::vector<Eigen::Vector3d> DeSkewScan(const std::vector<Eigen::Vector3d> &fram
                                         const Sophus::SE3d &start_pose,
                                         const Sophus::SE3d &finish_pose);
 
+std::vector<Eigen::Vector3d> DeSkewScan(const std::vector<Eigen::Vector3d> &frame,
+                                        const std::vector<double> &timestamps,
+                                        const Sophus::SE3d &delta);
+
 }  // namespace kiss_icp

cpp/kiss_icp/core/Registration.cpp

@@ -41,7 +41,8 @@ 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 Correspondences = std::vector<std::pair<Eigen::Vector3d, Eigen::Vector3d>>;
 using LinearSystem = std::pair<Eigen::Matrix6d, Eigen::Vector6d>;
 
 namespace {
@@ -87,19 +88,19 @@ std::tuple<Eigen::Vector3d, double> GetClosestNeighbor(const Eigen::Vector3d &po
     return std::make_tuple(closest_neighbor, closest_distance);
 }
 
-Associations FindAssociations(const std::vector<Eigen::Vector3d> &points,
-                              const kiss_icp::VoxelHashMap &voxel_map,
-                              double max_correspondance_distance) {
+Correspondences DataAssociation(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(
+    Correspondences correspondences;
+    correspondences.reserve(points.size());
+    correspondences = tbb::parallel_reduce(
         // Range
         tbb::blocked_range<points_iterator>{points.cbegin(), points.cend()},
         // Identity
-        associations,
+        correspondences,
         // 1st lambda: Parallel computation
-        [&](const tbb::blocked_range<points_iterator> &r, Associations res) -> Associations {
+        [&](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);
@@ -110,17 +111,17 @@ Associations FindAssociations(const std::vector<Eigen::Vector3d> &points,
             return res;
         },
         // 2nd lambda: Parallel reduction
-        [](Associations a, const Associations &b) -> Associations {
+        [](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;
 }
 
-LinearSystem BuildLinearSystem(const Associations &associations, double kernel) {
+LinearSystem BuildLinearSystem(const Correspondences &correspondences, double kernel) {
     auto compute_jacobian_and_residual = [](auto association) {
         const auto &[source, target] = association;
         const Eigen::Vector3d residual = source - target;
@@ -138,14 +139,15 @@ LinearSystem BuildLinearSystem(const Associations &associations, double kernel)
 
     auto GM_weight = [&](double residual2) { return square(kernel) / square(kernel + residual2); };
 
-    using associations_iterator = Associations::const_iterator;
+    using correspondence_iterator = Correspondences::const_iterator;
     const auto &[JTJ, JTr] = tbb::parallel_reduce(
         // Range
-        tbb::blocked_range<associations_iterator>{associations.cbegin(), associations.cend()},
+        tbb::blocked_range<correspondence_iterator>{correspondences.cbegin(),
+                                                    correspondences.cend()},
         // Identity
         LinearSystem(Eigen::Matrix6d::Zero(), Eigen::Vector6d::Zero()),
         // 1st Lambda: Parallel computation
-        [&](const tbb::blocked_range<associations_iterator> &r, LinearSystem J) -> LinearSystem {
+        [&](const tbb::blocked_range<correspondence_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);
@@ -177,8 +179,8 @@ Registration::Registration(int max_num_iteration, double convergence_criterion,
 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) {
+                                            double max_distance,
+                                            double kernel_scale) {
     if (voxel_map.Empty()) return initial_guess;
 
     // Equation (9)
@@ -189,9 +191,9 @@ Sophus::SE3d Registration::AlignPointsToMap(const std::vector<Eigen::Vector3d> &
     Sophus::SE3d T_icp = Sophus::SE3d();
     for (int j = 0; j < max_num_iterations_; ++j) {
         // Equation (10)
-        const auto associations = FindAssociations(source, voxel_map, max_correspondence_distance);
+        const auto correspondences = DataAssociation(source, voxel_map, max_distance);
         // Equation (11)
-        const auto &[JTJ, JTr] = BuildLinearSystem(associations, 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)

cpp/kiss_icp/core/Threshold.cpp

@@ -25,28 +25,26 @@
 #include <Eigen/Core>
 #include <cmath>
 
-namespace {
-double ComputeModelError(const Sophus::SE3d &model_deviation, double max_range) {
-    const double theta = Eigen::AngleAxisd(model_deviation.rotationMatrix()).angle();
-    const double delta_rot = 2.0 * max_range * std::sin(theta / 2.0);
-    const double delta_trans = model_deviation.translation().norm();
-    return delta_trans + delta_rot;
-}
-}  // namespace
-
 namespace kiss_icp {
+AdaptiveThreshold::AdaptiveThreshold(double initial_threshold,
+                                     double min_motion_threshold,
+                                     double max_range)
+    : min_motion_threshold_(min_motion_threshold),
+      max_range_(max_range),
+      model_sse_(initial_threshold * initial_threshold),
+      num_samples_(1) {}
 
-double AdaptiveThreshold::ComputeThreshold() {
-    double model_error = ComputeModelError(model_deviation_, max_range_);
-    if (model_error > min_motion_th_) {
-        model_error_sse2_ += model_error * model_error;
+void AdaptiveThreshold::UpdateModelDeviation(const Sophus::SE3d &current_deviation) {
+    const double model_error = [&]() {
+        const double theta = Eigen::AngleAxisd(current_deviation.rotationMatrix()).angle();
+        const double delta_rot = 2.0 * max_range_ * std::sin(theta / 2.0);
+        const double delta_trans = current_deviation.translation().norm();
+        return delta_trans + delta_rot;
+    }();
+    if (model_error > min_motion_threshold_) {
+        model_sse_ += model_error * model_error;
         num_samples_++;
     }
-
-    if (num_samples_ < 1) {
-        return initial_threshold_;
-    }
-    return std::sqrt(model_error_sse2_ / num_samples_);
 }
 
 }  // namespace kiss_icp

cpp/kiss_icp/core/Threshold.hpp

@@ -27,28 +27,23 @@
 namespace kiss_icp {
 
 struct AdaptiveThreshold {
-    explicit AdaptiveThreshold(double initial_threshold, double min_motion_th, double max_range)
-        : initial_threshold_(initial_threshold),
-          min_motion_th_(min_motion_th),
-          max_range_(max_range) {}
+    explicit AdaptiveThreshold(double initial_threshold,
+                               double min_motion_threshold,
+                               double max_range);
 
     /// Update the current belief of the deviation from the prediction model
-    inline void UpdateModelDeviation(const Sophus::SE3d &current_deviation) {
-        model_deviation_ = current_deviation;
-    }
+    void UpdateModelDeviation(const Sophus::SE3d &current_deviation);
 
     /// Returns the KISS-ICP adaptive threshold used in registration
-    double ComputeThreshold();
+    constexpr double ComputeThreshold() const { return std::sqrt(model_sse_ / num_samples_); }
 
     // configurable parameters
-    double initial_threshold_;
-    double min_motion_th_;
+    double min_motion_threshold_;
     double max_range_;
 
     // Local cache for ccomputation
-    double model_error_sse2_ = 0;
-    int num_samples_ = 0;
-    Sophus::SE3d model_deviation_ = Sophus::SE3d();
+    double model_sse_;
+    int num_samples_;
 };
 
 }  // namespace kiss_icp

cpp/kiss_icp/pipeline/KissICP.cpp

@@ -24,7 +24,6 @@
 #include "KissICP.hpp"
 
 #include <Eigen/Core>
-#include <tuple>
 #include <vector>
 
 #include "kiss_icp/core/Deskew.hpp"
@@ -38,16 +37,7 @@ KissICP::Vector3dVectorTuple KissICP::RegisterFrame(const std::vector<Eigen::Vec
                                                     const std::vector<double> &timestamps) {
     const auto &deskew_frame = [&]() -> std::vector<Eigen::Vector3d> {
         if (!config_.deskew || timestamps.empty()) return frame;
-        // TODO(Nacho) Add some asserts here to sanitize the timestamps
-
-        //  If not enough poses for the estimation, do not de-skew
-        const size_t N = poses().size();
-        if (N <= 2) return frame;
-
-        // Estimate linear and angular velocities
-        const auto &start_pose = poses_[N - 2];
-        const auto &finish_pose = poses_[N - 1];
-        return DeSkewScan(frame, timestamps, start_pose, finish_pose);
+        return DeSkewScan(frame, timestamps, last_delta_);
     }();
     return RegisterFrame(deskew_frame);
 }
@@ -59,24 +49,29 @@ KissICP::Vector3dVectorTuple KissICP::RegisterFrame(const std::vector<Eigen::Vec
     // Voxelize
     const auto &[source, frame_downsample] = Voxelize(cropped_frame);
 
-    // Get motion prediction and adaptive_threshold
-    const double sigma = GetAdaptiveThreshold();
+    // Get adaptive_threshold
+    const double sigma = adaptive_threshold_.ComputeThreshold();
 
     // Compute initial_guess for ICP
-    const auto prediction = GetPredictionModel();
-    const auto last_pose = !poses_.empty() ? poses_.back() : Sophus::SE3d();
-    const auto initial_guess = last_pose * prediction;
+    const auto initial_guess = last_pose_ * last_delta_;
+
+    // Run ICP
+    const auto new_pose = registration_.AlignPointsToMap(source,         // frame
+                                                         local_map_,     // voxel_map
+                                                         initial_guess,  // initial_guess
+                                                         3.0 * sigma,    // max_correspondence_dist
+                                                         sigma / 3.0);   // kernel
 
-    // Run icp
-    const Sophus::SE3d new_pose = registration_.AlignPointsToMap(source,         //
-                                                                 local_map_,     //
-                                                                 initial_guess,  //
-                                                                 3.0 * sigma,    //
-                                                                 sigma / 3.0);
+    // Compute the difference between the prediction and the actual estimate
     const auto model_deviation = initial_guess.inverse() * new_pose;
+
+    // Update step: threshold, local map, delta, and the last pose
     adaptive_threshold_.UpdateModelDeviation(model_deviation);
     local_map_.Update(frame_downsample, new_pose);
-    poses_.push_back(new_pose);
+    last_delta_ = last_pose_.inverse() * new_pose;
+    last_pose_ = new_pose;
+
+    // Return the (deskew) input raw scan (frame) and the points used for registration (source)
     return {frame, source};
 }
 
@@ -87,24 +82,4 @@ KissICP::Vector3dVectorTuple KissICP::Voxelize(const std::vector<Eigen::Vector3d
     return {source, frame_downsample};
 }
 
-double KissICP::GetAdaptiveThreshold() {
-    if (!HasMoved()) {
-        return config_.initial_threshold;
-    }
-    return adaptive_threshold_.ComputeThreshold();
-}
-
-Sophus::SE3d KissICP::GetPredictionModel() const {
-    Sophus::SE3d pred = Sophus::SE3d();
-    const size_t N = poses_.size();
-    if (N < 2) return pred;
-    return poses_[N - 2].inverse() * poses_[N - 1];
-}
-
-bool KissICP::HasMoved() {
-    if (poses_.empty()) return false;
-    const double motion = (poses_.front().inverse() * poses_.back()).translation().norm();
-    return motion > 5.0 * config_.min_motion_th;
-}
-
 }  // namespace kiss_icp::pipeline

cpp/kiss_icp/pipeline/KissICP.hpp

@@ -22,10 +22,10 @@
 #pragma once
 
 #include <Eigen/Core>
+#include <sophus/se3.hpp>
 #include <tuple>
 #include <vector>
 
-#include "kiss_icp/core/Deskew.hpp"
 #include "kiss_icp/core/Registration.hpp"
 #include "kiss_icp/core/Threshold.hpp"
 #include "kiss_icp/core/VoxelHashMap.hpp"
@@ -70,18 +70,16 @@ class KissICP {
     Vector3dVectorTuple RegisterFrame(const std::vector<Eigen::Vector3d> &frame,
                                       const std::vector<double> &timestamps);
     Vector3dVectorTuple Voxelize(const std::vector<Eigen::Vector3d> &frame) const;
-    double GetAdaptiveThreshold();
-    Sophus::SE3d GetPredictionModel() const;
-    bool HasMoved();
 
-public:
-    // Extra C++ API to facilitate ROS debugging
     std::vector<Eigen::Vector3d> LocalMap() const { return local_map_.Pointcloud(); };
-    std::vector<Sophus::SE3d> poses() const { return poses_; };
+    Sophus::SE3d pose() const { return last_pose_; }
+    Sophus::SE3d delta() const { return last_delta_; }
 
 private:
+    Sophus::SE3d last_pose_;
+    Sophus::SE3d last_delta_;
+
     // KISS-ICP pipeline modules
-    std::vector<Sophus::SE3d> poses_;
     KISSConfig config_;
     Registration registration_;
     VoxelHashMap local_map_;

python/kiss_icp/deskew.py

@@ -31,18 +31,15 @@ def get_motion_compensator(config: KISSConfig):
 
 
 class StubCompensator:
-    def deskew_scan(self, frame, poses, timestamps):
+    def deskew_scan(self, frame, timestamps, delta):
         return frame
 
 
 class MotionCompensator:
-    def deskew_scan(self, frame, poses, timestamps):
-        if len(poses) < 2:
-            return frame
+    def deskew_scan(self, frame, timestamps, delta):
         deskew_frame = kiss_icp_pybind._deskew_scan(
             frame=kiss_icp_pybind._Vector3dVector(frame),
             timestamps=timestamps,
-            start_pose=poses[-2],
-            finish_pose=poses[-1],
+            delta=delta,
         )
         return np.asarray(deskew_frame)