Preprocessing Pipeline

cpp/kiss_icp/core/CMakeLists.txt

@@ -21,7 +21,6 @@
 # OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
 # SOFTWARE.
 add_library(kiss_icp_core STATIC)
-target_sources(kiss_icp_core PRIVATE Registration.cpp Deskew.cpp VoxelHashMap.cpp VoxelUtils.cpp Preprocessing.cpp
-                                     Threshold.cpp)
+target_sources(kiss_icp_core PRIVATE Registration.cpp VoxelHashMap.cpp VoxelUtils.cpp Preprocessing.cpp Threshold.cpp)
 target_link_libraries(kiss_icp_core PUBLIC Eigen3::Eigen tsl::robin_map TBB::tbb Sophus::Sophus)
 set_global_target_properties(kiss_icp_core)

cpp/kiss_icp/core/Preprocessing.cpp

@@ -22,23 +22,85 @@
 // SOFTWARE.
 #include "Preprocessing.hpp"
 
-#include <tsl/robin_map.h>
+#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/task_arena.h>
 
 #include <Eigen/Core>
 #include <Eigen/Geometry>
 #include <algorithm>
+#include <functional>
 #include <vector>
 
+namespace {
+constexpr double mid_pose_timestamp{0.5};
+struct StubDeskewer {
+    StubDeskewer(const std::vector<double> &timestamps, const Sophus::SE3d &relative_motion)
+        : stamps_(timestamps), motion_(relative_motion) {}
+
+    Eigen::Vector3d operator()(const Eigen::Vector3d &point, const size_t &) const { return point; }
+
+    const std::vector<double> &stamps_;
+    const Sophus::SE3d motion_;
+};
+
+struct MotionDeskewer : public StubDeskewer {
+    MotionDeskewer(const std::vector<double> &timestamps, const Sophus::SE3d &relative_motion)
+        : StubDeskewer(timestamps, relative_motion) {}
+
+    Eigen::Vector3d operator()(const Eigen::Vector3d &point, const size_t &idx) const {
+        const auto delta_pose = motion_.log();
+        const auto motion = Sophus::SE3d::exp((stamps_.at(idx) - mid_pose_timestamp) * delta_pose);
+        return motion * point;
+    }
+};
+}  // namespace
+
 namespace kiss_icp {
-std::vector<Eigen::Vector3d> Preprocess(const std::vector<Eigen::Vector3d> &frame,
-                                        double max_range,
-                                        double min_range) {
-    std::vector<Eigen::Vector3d> inliers;
-    std::copy_if(frame.cbegin(), frame.cend(), std::back_inserter(inliers), [&](const auto &pt) {
-        const double norm = pt.norm();
-        return norm < max_range && norm > min_range;
-    });
-    return inliers;
+
+Preprocessor::Preprocessor(const double max_range,
+                           const double min_range,
+                           const bool deskew,
+                           const int max_num_threads)
+    : max_range_(max_range),
+      min_range_(min_range),
+      deskew_(deskew),
+      max_num_threads_(max_num_threads > 0 ? max_num_threads
+                                           : tbb::this_task_arena::max_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_));
 }
 
+std::vector<Eigen::Vector3d> Preprocessor::Preprocess(const std::vector<Eigen::Vector3d> &frame,
+                                                      const std::vector<double> &timestamps,
+                                                      const Sophus::SE3d &relative_motion) const {
+    using DeskewerType = std::function<Eigen::Vector3d(const Eigen::Vector3d &, const size_t &)>;
+    const DeskewerType deskewer = (deskew_ && !timestamps.empty())
+                                      ? MotionDeskewer(timestamps, relative_motion)
+                                      : StubDeskewer(timestamps, relative_motion);
+    tbb::concurrent_vector<Eigen::Vector3d> preprocessed_frame;
+    preprocessed_frame.reserve(frame.size());
+    tbb::parallel_for(
+        // Index Range
+        tbb::blocked_range<size_t>(0, frame.size()),
+        // Parallel Compute
+        [&](const tbb::blocked_range<size_t> &r) {
+            for (size_t idx = r.begin(); idx < r.end(); ++idx) {
+                const auto &point = frame.at(idx);
+                const double point_range = point.norm();
+                if (point_range < max_range_ && point_range > min_range_) {
+                    const auto &deskewed_point = deskewer(point, idx);
+                    preprocessed_frame.emplace_back(deskewed_point);
+                }
+            };
+        });
+    std::vector<Eigen::Vector3d> returned(std::make_move_iterator(preprocessed_frame.cbegin()),  //
+                                          std::make_move_iterator(preprocessed_frame.cend()));
+    return returned;
+}
 }  // namespace kiss_icp

cpp/kiss_icp/core/Preprocessing.hpp

@@ -23,12 +23,24 @@
 #pragma once
 
 #include <Eigen/Core>
+#include <functional>
+#include <sophus/se3.hpp>
 #include <vector>
 
 namespace kiss_icp {
 
-/// Crop the frame with max/min ranges
-std::vector<Eigen::Vector3d> Preprocess(const std::vector<Eigen::Vector3d> &frame,
-                                        double max_range,
-                                        double min_range);
+struct Preprocessor {
+    Preprocessor(const double max_range,
+                 const double min_range,
+                 const bool deskew,
+                 const int max_num_threads);
+
+    std::vector<Eigen::Vector3d> Preprocess(const std::vector<Eigen::Vector3d> &frame,
+                                            const std::vector<double> &timestamps,
+                                            const Sophus::SE3d &relative_motion) const;
+    double max_range_;
+    double min_range_;
+    bool deskew_;
+    int max_num_threads_;
+};
 }  // namespace kiss_icp

cpp/kiss_icp/pipeline/KissICP.cpp

@@ -26,7 +26,6 @@
 #include <Eigen/Core>
 #include <vector>
 
-#include "kiss_icp/core/Deskew.hpp"
 #include "kiss_icp/core/Preprocessing.hpp"
 #include "kiss_icp/core/Registration.hpp"
 #include "kiss_icp/core/VoxelHashMap.hpp"
@@ -35,19 +34,11 @@ namespace kiss_icp::pipeline {
 
 KissICP::Vector3dVectorTuple KissICP::RegisterFrame(const std::vector<Eigen::Vector3d> &frame,
                                                     const std::vector<double> &timestamps) {
-    const auto &deskew_frame = [&]() -> std::vector<Eigen::Vector3d> {
-        if (!config_.deskew || timestamps.empty()) return frame;
-        return DeSkewScan(frame, timestamps, last_delta_);
-    }();
-    return RegisterFrame(deskew_frame);
-}
-
-KissICP::Vector3dVectorTuple KissICP::RegisterFrame(const std::vector<Eigen::Vector3d> &frame) {
     // Preprocess the input cloud
-    const auto &cropped_frame = Preprocess(frame, config_.max_range, config_.min_range);
+    const auto &preprocessed_frame = preprocessor_.Preprocess(frame, timestamps, last_delta_);
 
     // Voxelize
-    const auto &[source, frame_downsample] = Voxelize(cropped_frame);
+    const auto &[source, frame_downsample] = Voxelize(preprocessed_frame);
 
     // Get adaptive_threshold
     const double sigma = adaptive_threshold_.ComputeThreshold();

cpp/kiss_icp/pipeline/KissICP.hpp

@@ -26,6 +26,7 @@
 #include <tuple>
 #include <vector>
 
+#include "kiss_icp/core/Preprocessing.hpp"
 #include "kiss_icp/core/Registration.hpp"
 #include "kiss_icp/core/Threshold.hpp"
 #include "kiss_icp/core/VoxelHashMap.hpp"
@@ -60,13 +61,13 @@ class KissICP {
 public:
     explicit KissICP(const KISSConfig &config)
         : config_(config),
+          preprocessor_(config.max_range, config.min_range, config.deskew, config.max_num_threads),
           registration_(
               config.max_num_iterations, config.convergence_criterion, config.max_num_threads),
           local_map_(config.voxel_size, config.max_range, config.max_points_per_voxel),
           adaptive_threshold_(config.initial_threshold, config.min_motion_th, config.max_range) {}
 
 public:
-    Vector3dVectorTuple RegisterFrame(const std::vector<Eigen::Vector3d> &frame);
     Vector3dVectorTuple RegisterFrame(const std::vector<Eigen::Vector3d> &frame,
                                       const std::vector<double> &timestamps);
     Vector3dVectorTuple Voxelize(const std::vector<Eigen::Vector3d> &frame) const;
@@ -88,6 +89,7 @@ class KissICP {
 
     // KISS-ICP pipeline modules
     KISSConfig config_;
+    Preprocessor preprocessor_;
     Registration registration_;
     VoxelHashMap local_map_;
     AdaptiveThreshold adaptive_threshold_;

python/kiss_icp/kiss_icp.py

@@ -23,7 +23,6 @@
 import numpy as np
 
 from kiss_icp.config import KISSConfig
-from kiss_icp.deskew import get_motion_compensator
 from kiss_icp.mapping import get_voxel_hash_map
 from kiss_icp.preprocess import get_preprocessor
 from kiss_icp.registration import get_registration
@@ -36,18 +35,15 @@ def __init__(self, config: KISSConfig):
         self.last_pose = np.eye(4)
         self.last_delta = np.eye(4)
         self.config = config
-        self.compensator = get_motion_compensator(config)
         self.adaptive_threshold = get_threshold_estimator(self.config)
+        self.preprocessor = get_preprocessor(self.config)
         self.registration = get_registration(self.config)
         self.local_map = get_voxel_hash_map(self.config)
         self.preprocess = get_preprocessor(self.config)
 
     def register_frame(self, frame, timestamps):
         # Apply motion compensation
-        frame = self.compensator.deskew_scan(frame, timestamps, self.last_delta)
-
-        # Preprocess the input cloud
-        frame = self.preprocess(frame)
+        frame = self.preprocessor.preprocess(frame, timestamps, self.last_delta)
 
         # Voxelize
         source, frame_downsample = self.voxelize(frame)

python/kiss_icp/preprocess.py

@@ -27,18 +27,25 @@
 
 
 def get_preprocessor(config: KISSConfig):
-    return Preprocessor(config)
+    return Preprocessor(
+        max_range=config.data.max_range,
+        min_range=config.data.min_range,
+        deskew=config.data.deskew,
+        max_num_threads=config.registration.max_num_threads,
+    )
 
 
 class Preprocessor:
-    def __init__(self, config: KISSConfig):
-        self.config = config
+    def __init__(self, max_range, min_range, deskew, max_num_threads):
+        self._preprocessor = kiss_icp_pybind._Preprocessor(
+            max_range, min_range, deskew, max_num_threads
+        )
 
-    def __call__(self, frame: np.ndarray):
+    def preprocess(self, frame: np.ndarray, timestamps: np.ndarray, relative_motion: np.ndarray):
         return np.asarray(
-            kiss_icp_pybind._preprocess(
+            self._preprocessor._preprocess(
                 kiss_icp_pybind._Vector3dVector(frame),
-                self.config.data.max_range,
-                self.config.data.min_range,
+                timestamps,
+                relative_motion,
             )
         )