Preprocessing Pipeline (#412)

* Preprocessing as a module

* Now C++ compile, need to work on the python API

* Fix formatting cmake

* Now also in the python API

* Remove completely unnused function

* Still need to open the PR, good job

* Zero additional allocations

* We know this shifts

* Revert VoxelHashMap change -> Allocations go in a separate PR

* Revert "Revert VoxelHashMap change -> Allocations go in a separate PR"

This reverts commit 5ae3283.

* Revert concurrent vector change

* Parallel for and concurrent vector

* Include the learnings from other PRs

* Huglier solution but it works

* Better

* Midpose

* Actually, lets deskew at the end of the scan as it should be

* Revert "Actually, lets deskew at the end of the scan as it should be"

This reverts commit c450246.

* Fix pipeline for 20.04

* Const correctness just in case

* Debug string

* Simplify

* Remove debug string

* Remove redundancy

* Deskew with abstract classes...bleh

* empty array like in ros

* bla

* Now it matches main

* Rename variable

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,73 @@
 // 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_stamp{0.5};
+}  // 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 {
+    const std::vector<Eigen::Vector3d> &deskewed_frame = [&]() {
+        if (!deskew_ || timestamps.empty()) {
+            return frame;
+        } else {
+            const auto motion = relative_motion.log();
+            std::vector<Eigen::Vector3d> deskewed_frame(frame.size());
+            tbb::parallel_for(
+                // Index Range
+                tbb::blocked_range<size_t>{0, deskewed_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 auto &stamp = timestamps.at(idx);
+                        const auto pose = Sophus::SE3d::exp((stamp - mid_pose_stamp) * motion);
+                        deskewed_frame.at(idx) = pose * point;
+                    };
+                });
+            return deskewed_frame;
+        }
+    }();
+    std::vector<Eigen::Vector3d> preprocessed_frame;
+    preprocessed_frame.reserve(deskewed_frame.size());
+    std::for_each(deskewed_frame.cbegin(), deskewed_frame.cend(), [&](const auto &point) {
+        const double point_range = point.norm();
+        if (point_range < max_range_ && point_range > min_range_) {
+            preprocessed_frame.emplace_back(point);
+        }
+    });
+    preprocessed_frame.shrink_to_fit();
+    return preprocessed_frame;
+}
 }  // 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,14 @@ 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/pipeline.py

@@ -121,7 +121,7 @@ def _next(self, idx):
             frame, timestamps = dataframe
         except ValueError:
             frame = dataframe
-            timestamps = np.zeros(frame.shape[0])
+            timestamps = np.array([])
         return frame, timestamps
 
     @staticmethod

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,
             )
         )