From 576dda93a3b9f4e8bc7dcdaf581580f67908f138 Mon Sep 17 00:00:00 2001 From: tizianoGuadagnino Date: Fri, 18 Oct 2024 14:13:39 +0200 Subject: [PATCH 01/14] Preprocessing as a module --- cpp/kiss_icp/core/CMakeLists.txt | 2 +- cpp/kiss_icp/core/Deskew.cpp | 49 -------------------- cpp/kiss_icp/core/Deskew.hpp | 36 --------------- cpp/kiss_icp/core/Preprocessing.cpp | 72 +++++++++++++++++++++++++---- cpp/kiss_icp/core/Preprocessing.hpp | 23 +++++++-- 5 files changed, 82 insertions(+), 100 deletions(-) delete mode 100644 cpp/kiss_icp/core/Deskew.cpp delete mode 100644 cpp/kiss_icp/core/Deskew.hpp diff --git a/cpp/kiss_icp/core/CMakeLists.txt b/cpp/kiss_icp/core/CMakeLists.txt index fc627bf4..fbaa3b27 100644 --- a/cpp/kiss_icp/core/CMakeLists.txt +++ b/cpp/kiss_icp/core/CMakeLists.txt @@ -21,7 +21,7 @@ # 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 +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) diff --git a/cpp/kiss_icp/core/Deskew.cpp b/cpp/kiss_icp/core/Deskew.cpp deleted file mode 100644 index fe20b77e..00000000 --- a/cpp/kiss_icp/core/Deskew.cpp +++ /dev/null @@ -1,49 +0,0 @@ -// MIT License -// -// Copyright (c) 2022 Ignacio Vizzo, Tiziano Guadagnino, Benedikt Mersch, Cyrill -// Stachniss. -// -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: -// -// The above copyright notice and this permission notice shall be included in all -// copies or substantial portions of the Software. -// -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -// SOFTWARE. -#include "Deskew.hpp" - -#include - -#include -#include -#include - -namespace { -/// TODO(Nacho) Explain what is the very important meaning of this param -constexpr double mid_pose_timestamp{0.5}; -} // namespace - -namespace kiss_icp { -std::vector DeSkewScan(const std::vector &frame, - const std::vector ×tamps, - const Sophus::SE3d &delta) { - const auto delta_pose = delta.log(); - std::vector corrected_frame(frame.size()); - // TODO(All): This tbb execution is ignoring the max_n_threads config value - tbb::parallel_for(size_t(0), frame.size(), [&](size_t i) { - const auto motion = Sophus::SE3d::exp((timestamps[i] - mid_pose_timestamp) * delta_pose); - corrected_frame[i] = motion * frame[i]; - }); - return corrected_frame; -} -} // namespace kiss_icp diff --git a/cpp/kiss_icp/core/Deskew.hpp b/cpp/kiss_icp/core/Deskew.hpp deleted file mode 100644 index e229063a..00000000 --- a/cpp/kiss_icp/core/Deskew.hpp +++ /dev/null @@ -1,36 +0,0 @@ -// MIT License -// -// Copyright (c) 2022 Ignacio Vizzo, Tiziano Guadagnino, Benedikt Mersch, Cyrill -// Stachniss. -// -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: -// -// The above copyright notice and this permission notice shall be included in all -// copies or substantial portions of the Software. -// -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -// SOFTWARE. -#pragma once - -#include -#include -#include - -namespace kiss_icp { - -/// Compensate the frame by interpolating the delta pose -std::vector DeSkewScan(const std::vector &frame, - const std::vector ×tamps, - const Sophus::SE3d &delta); - -} // namespace kiss_icp diff --git a/cpp/kiss_icp/core/Preprocessing.cpp b/cpp/kiss_icp/core/Preprocessing.cpp index d626de45..169012d5 100644 --- a/cpp/kiss_icp/core/Preprocessing.cpp +++ b/cpp/kiss_icp/core/Preprocessing.cpp @@ -22,23 +22,75 @@ // SOFTWARE. #include "Preprocessing.hpp" -#include +#include +#include +#include #include #include #include +#include #include +namespace { +constexpr double mid_pose_timestamp{0.5}; + +Eigen::Vector3d DeSkewPoint(const Eigen::Vector3d &point, + const double timestamp, + const Sophus::SE3d &delta) { + const auto delta_pose = delta.log(); + const auto motion = Sophus::SE3d::exp((timestamp - mid_pose_timestamp) * delta_pose); + return motion * point; +} + +} // namespace + namespace kiss_icp { -std::vector Preprocess(const std::vector &frame, - double max_range, - double min_range) { - std::vector 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(max_num_threads_)); } +std::vector Preprocessor::Preprocess(const std::vector &frame, + const std::vector ×tamps) const { + std::vector preprocessed_frame; + preprocessed_frame.reserve(frame.size()); + preprocessed_frame = tbb::parallel_reduce( + // Index Range + tbb::blocked_range(0, frame.size()), + // Initial value + preprocessed_frame, + // Parallel Compute + [&](const tbb::blocked_range &r, + std::vector preprocessed_block) -> std::vector { + preprocessed_block.reserve(r.size()); + std::for_each(r.begin(), r.end(), [&](const size_t &idx) { + const auto &point = deskew_ ? DeskewPoint(frame.at(i), timestamps.at(i), + relative_motion_for_deskewing_) + : frame.at(i); + const double point_range = point.norm(); + if (point_range < max_range_ && point_range > min_range_) { + preprocessed_block.emplace_back(point); + } + }) + }, + // Reduction + [](std::vector a, + const std::vector &b) -> std::vector { + a.insert(a.end(), // + std::make_move_iterator(b.cbegin()), // + std::make_move_iterator(b.cend())); + return a; + }); } // namespace kiss_icp diff --git a/cpp/kiss_icp/core/Preprocessing.hpp b/cpp/kiss_icp/core/Preprocessing.hpp index d161177d..f37de155 100644 --- a/cpp/kiss_icp/core/Preprocessing.hpp +++ b/cpp/kiss_icp/core/Preprocessing.hpp @@ -23,12 +23,27 @@ #pragma once #include +#include #include namespace kiss_icp { -/// Crop the frame with max/min ranges -std::vector Preprocess(const std::vector &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); + + inline void SetPlatformMotion(const Sophus::SE3d &motion) { + relative_motion_for_deskewing_ = motion; + } + + std::vector Preprocess(const std::vector &frame, + const std::vector ×tamps) const; + double max_range_; + double min_range_; + bool deskew_; + int max_num_threads_; + Sophus::SE3d relative_motion_for_deskewing_; +}; } // namespace kiss_icp From a0ec02938a41122328fa7eea37037f468bfc28f6 Mon Sep 17 00:00:00 2001 From: tizianoGuadagnino Date: Fri, 18 Oct 2024 15:08:57 +0200 Subject: [PATCH 02/14] Now C++ compile, need to work on the python API --- cpp/kiss_icp/core/Preprocessing.cpp | 30 ++++++++++++++-------- cpp/kiss_icp/core/Preprocessing.hpp | 8 ++---- cpp/kiss_icp/pipeline/KissICP.cpp | 13 ++-------- cpp/kiss_icp/pipeline/KissICP.hpp | 3 +++ python/kiss_icp/pybind/kiss_icp_pybind.cpp | 11 -------- 5 files changed, 26 insertions(+), 39 deletions(-) diff --git a/cpp/kiss_icp/core/Preprocessing.cpp b/cpp/kiss_icp/core/Preprocessing.cpp index 169012d5..4bb229b2 100644 --- a/cpp/kiss_icp/core/Preprocessing.cpp +++ b/cpp/kiss_icp/core/Preprocessing.cpp @@ -22,14 +22,16 @@ // SOFTWARE. #include "Preprocessing.hpp" -#include -#include -#include +#include +#include +#include +#include +#include #include #include #include -#include +#include #include namespace { @@ -62,8 +64,10 @@ Preprocessor::Preprocessor(const double max_range, tbb::global_control::max_allowed_parallelism, static_cast(max_num_threads_)); } -std::vector Preprocessor::Preprocess(const std::vector &frame, - const std::vector ×tamps) const { +std::vector Preprocessor::Preprocess( + const std::vector &frame, + const std::vector ×tamps, + const Sophus::SE3d &relative_platform_motion) const { std::vector preprocessed_frame; preprocessed_frame.reserve(frame.size()); preprocessed_frame = tbb::parallel_reduce( @@ -75,15 +79,17 @@ std::vector Preprocessor::Preprocess(const std::vector &r, std::vector preprocessed_block) -> std::vector { preprocessed_block.reserve(r.size()); - std::for_each(r.begin(), r.end(), [&](const size_t &idx) { - const auto &point = deskew_ ? DeskewPoint(frame.at(i), timestamps.at(i), - relative_motion_for_deskewing_) - : frame.at(i); + for (size_t idx = r.begin(); idx < r.end(); ++idx) { + const auto &point = + (deskew_ && !timestamps.empty()) + ? DeSkewPoint(frame.at(idx), timestamps.at(idx), relative_platform_motion) + : frame.at(idx); const double point_range = point.norm(); if (point_range < max_range_ && point_range > min_range_) { preprocessed_block.emplace_back(point); } - }) + }; + return preprocessed_block; }, // Reduction [](std::vector a, @@ -93,4 +99,6 @@ std::vector Preprocessor::Preprocess(const std::vector Preprocess(const std::vector &frame, - const std::vector ×tamps) const; + const std::vector ×tamps, + const Sophus::SE3d &relative_platform_motion) const; double max_range_; double min_range_; bool deskew_; int max_num_threads_; - Sophus::SE3d relative_motion_for_deskewing_; }; } // namespace kiss_icp diff --git a/cpp/kiss_icp/pipeline/KissICP.cpp b/cpp/kiss_icp/pipeline/KissICP.cpp index 6c8f168a..6886a5d6 100644 --- a/cpp/kiss_icp/pipeline/KissICP.cpp +++ b/cpp/kiss_icp/pipeline/KissICP.cpp @@ -26,7 +26,6 @@ #include #include -#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 &frame, const std::vector ×tamps) { - const auto &deskew_frame = [&]() -> std::vector { - if (!config_.deskew || timestamps.empty()) return frame; - return DeSkewScan(frame, timestamps, last_delta_); - }(); - return RegisterFrame(deskew_frame); -} - -KissICP::Vector3dVectorTuple KissICP::RegisterFrame(const std::vector &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(); diff --git a/cpp/kiss_icp/pipeline/KissICP.hpp b/cpp/kiss_icp/pipeline/KissICP.hpp index 2a5be57c..488fbe5e 100644 --- a/cpp/kiss_icp/pipeline/KissICP.hpp +++ b/cpp/kiss_icp/pipeline/KissICP.hpp @@ -26,6 +26,7 @@ #include #include +#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,6 +61,7 @@ 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), @@ -88,6 +90,7 @@ class KissICP { // KISS-ICP pipeline modules KISSConfig config_; + Preprocessor preprocessor_; Registration registration_; VoxelHashMap local_map_; AdaptiveThreshold adaptive_threshold_; diff --git a/python/kiss_icp/pybind/kiss_icp_pybind.cpp b/python/kiss_icp/pybind/kiss_icp_pybind.cpp index 2291c04f..b428abbd 100644 --- a/python/kiss_icp/pybind/kiss_icp_pybind.cpp +++ b/python/kiss_icp/pybind/kiss_icp_pybind.cpp @@ -32,7 +32,6 @@ #include #include -#include "kiss_icp/core/Deskew.hpp" #include "kiss_icp/core/Preprocessing.hpp" #include "kiss_icp/core/Registration.hpp" #include "kiss_icp/core/Threshold.hpp" @@ -107,16 +106,6 @@ PYBIND11_MODULE(kiss_icp_pybind, m) { }, "model_deviation"_a); - // DeSkewScan - m.def( - "_deskew_scan", - [](const std::vector &frame, const std::vector ×tamps, - const Eigen::Matrix4d &T_delta) { - Sophus::SE3d delta(T_delta); - return DeSkewScan(frame, timestamps, delta); - }, - "frame"_a, "timestamps"_a, "delta"_a); - // prerpocessing modules m.def("_voxel_down_sample", &VoxelDownsample, "frame"_a, "voxel_size"_a); m.def("_preprocess", &Preprocess, "frame"_a, "max_range"_a, "min_range"_a); From 597a1aa0517ce1ce1417c6b27652f1eb866d2f19 Mon Sep 17 00:00:00 2001 From: tizianoGuadagnino Date: Fri, 18 Oct 2024 15:09:25 +0200 Subject: [PATCH 03/14] Fix formatting cmake --- cpp/kiss_icp/core/CMakeLists.txt | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/cpp/kiss_icp/core/CMakeLists.txt b/cpp/kiss_icp/core/CMakeLists.txt index fbaa3b27..d439cdb1 100644 --- a/cpp/kiss_icp/core/CMakeLists.txt +++ b/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 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) From e0c9e6dbd374d794524c0db22364b49deb26698b Mon Sep 17 00:00:00 2001 From: tizianoGuadagnino Date: Fri, 18 Oct 2024 15:33:25 +0200 Subject: [PATCH 04/14] Now also in the python API --- cpp/kiss_icp/core/Preprocessing.cpp | 9 ++--- cpp/kiss_icp/core/Preprocessing.hpp | 2 +- python/kiss_icp/deskew.py | 45 ---------------------- python/kiss_icp/kiss_icp.py | 7 +--- python/kiss_icp/preprocess.py | 21 ++++++---- python/kiss_icp/pybind/kiss_icp_pybind.cpp | 14 ++++++- 6 files changed, 34 insertions(+), 64 deletions(-) delete mode 100644 python/kiss_icp/deskew.py diff --git a/cpp/kiss_icp/core/Preprocessing.cpp b/cpp/kiss_icp/core/Preprocessing.cpp index 4bb229b2..61268180 100644 --- a/cpp/kiss_icp/core/Preprocessing.cpp +++ b/cpp/kiss_icp/core/Preprocessing.cpp @@ -64,10 +64,9 @@ Preprocessor::Preprocessor(const double max_range, tbb::global_control::max_allowed_parallelism, static_cast(max_num_threads_)); } -std::vector Preprocessor::Preprocess( - const std::vector &frame, - const std::vector ×tamps, - const Sophus::SE3d &relative_platform_motion) const { +std::vector Preprocessor::Preprocess(const std::vector &frame, + const std::vector ×tamps, + const Sophus::SE3d &relative_motion) const { std::vector preprocessed_frame; preprocessed_frame.reserve(frame.size()); preprocessed_frame = tbb::parallel_reduce( @@ -82,7 +81,7 @@ std::vector Preprocessor::Preprocess( for (size_t idx = r.begin(); idx < r.end(); ++idx) { const auto &point = (deskew_ && !timestamps.empty()) - ? DeSkewPoint(frame.at(idx), timestamps.at(idx), relative_platform_motion) + ? DeSkewPoint(frame.at(idx), timestamps.at(idx), relative_motion) : frame.at(idx); const double point_range = point.norm(); if (point_range < max_range_ && point_range > min_range_) { diff --git a/cpp/kiss_icp/core/Preprocessing.hpp b/cpp/kiss_icp/core/Preprocessing.hpp index bc6d29ef..9443ed86 100644 --- a/cpp/kiss_icp/core/Preprocessing.hpp +++ b/cpp/kiss_icp/core/Preprocessing.hpp @@ -36,7 +36,7 @@ struct Preprocessor { std::vector Preprocess(const std::vector &frame, const std::vector ×tamps, - const Sophus::SE3d &relative_platform_motion) const; + const Sophus::SE3d &relative_motion) const; double max_range_; double min_range_; bool deskew_; diff --git a/python/kiss_icp/deskew.py b/python/kiss_icp/deskew.py deleted file mode 100644 index a61c138f..00000000 --- a/python/kiss_icp/deskew.py +++ /dev/null @@ -1,45 +0,0 @@ -# MIT License -# -# Copyright (c) 2022 Ignacio Vizzo, Tiziano Guadagnino, Benedikt Mersch, Cyrill -# Stachniss. -# -# Permission is hereby granted, free of charge, to any person obtaining a copy -# of this software and associated documentation files (the "Software"), to deal -# in the Software without restriction, including without limitation the rights -# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -# copies of the Software, and to permit persons to whom the Software is -# furnished to do so, subject to the following conditions: -# -# The above copyright notice and this permission notice shall be included in all -# copies or substantial portions of the Software. -# -# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -# AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -# SOFTWARE. -import numpy as np - -from kiss_icp.config import KISSConfig -from kiss_icp.pybind import kiss_icp_pybind - - -def get_motion_compensator(config: KISSConfig): - return MotionCompensator() if config.data.deskew else StubCompensator() - - -class StubCompensator: - def deskew_scan(self, frame, timestamps, delta): - return frame - - -class MotionCompensator: - def deskew_scan(self, frame, timestamps, delta): - deskew_frame = kiss_icp_pybind._deskew_scan( - frame=kiss_icp_pybind._Vector3dVector(frame), - timestamps=timestamps, - delta=delta, - ) - return np.asarray(deskew_frame) diff --git a/python/kiss_icp/kiss_icp.py b/python/kiss_icp/kiss_icp.py index e7087dc8..239c7845 100644 --- a/python/kiss_icp/kiss_icp.py +++ b/python/kiss_icp/kiss_icp.py @@ -36,18 +36,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) diff --git a/python/kiss_icp/preprocess.py b/python/kiss_icp/preprocess.py index 0a1cdcd5..60f8cb23 100644 --- a/python/kiss_icp/preprocess.py +++ b/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, ) ) diff --git a/python/kiss_icp/pybind/kiss_icp_pybind.cpp b/python/kiss_icp/pybind/kiss_icp_pybind.cpp index b428abbd..ab1c1435 100644 --- a/python/kiss_icp/pybind/kiss_icp_pybind.cpp +++ b/python/kiss_icp/pybind/kiss_icp_pybind.cpp @@ -73,6 +73,19 @@ PYBIND11_MODULE(kiss_icp_pybind, m) { .def("_remove_far_away_points", &VoxelHashMap::RemovePointsFarFromLocation, "origin"_a) .def("_point_cloud", &VoxelHashMap::Pointcloud); + py::class_ internal_preprocessor(m, "_Preprocessor", "Don't use this"); + internal_preprocessor + .def(py::init(), "max_range"_a, "min_range"_a, "deskew"_a, + "max_num_threads"_a) + .def( + "_preprocess", + [](Preprocessor &self, const std::vector &points, + const std::vector ×tamps, const Eigen::Matrix4d &relative_motion) { + Sophus::SE3d motion(relative_motion); + return self.Preprocess(points, timestamps, motion); + }, + "points"_a, "timestamps"_a, "relative_motion"_a); + // Point Cloud registration py::class_ internal_registration(m, "_Registration", "Don't use this"); internal_registration @@ -108,7 +121,6 @@ PYBIND11_MODULE(kiss_icp_pybind, m) { // prerpocessing modules m.def("_voxel_down_sample", &VoxelDownsample, "frame"_a, "voxel_size"_a); - m.def("_preprocess", &Preprocess, "frame"_a, "max_range"_a, "min_range"_a); /// This function only applies for the KITTI dataset, and should NOT be used by any other /// dataset, the original idea and part of the implementation is taking from CT-ICP(Although /// IMLS-SLAM Originally introduced the calibration factor) From de4c492fbc22122bea03c45f2d42bf09b0772d3f Mon Sep 17 00:00:00 2001 From: tizianoGuadagnino Date: Fri, 18 Oct 2024 15:34:38 +0200 Subject: [PATCH 05/14] Remove completely unnused function --- cpp/kiss_icp/pipeline/KissICP.hpp | 1 - 1 file changed, 1 deletion(-) diff --git a/cpp/kiss_icp/pipeline/KissICP.hpp b/cpp/kiss_icp/pipeline/KissICP.hpp index 488fbe5e..0ca3e52e 100644 --- a/cpp/kiss_icp/pipeline/KissICP.hpp +++ b/cpp/kiss_icp/pipeline/KissICP.hpp @@ -68,7 +68,6 @@ class KissICP { adaptive_threshold_(config.initial_threshold, config.min_motion_th, config.max_range) {} public: - Vector3dVectorTuple RegisterFrame(const std::vector &frame); Vector3dVectorTuple RegisterFrame(const std::vector &frame, const std::vector ×tamps); Vector3dVectorTuple Voxelize(const std::vector &frame) const; From aa6838dc3bc8d6aa479d12d957753b3ed414cf44 Mon Sep 17 00:00:00 2001 From: tizianoGuadagnino Date: Fri, 29 Nov 2024 10:13:00 +0100 Subject: [PATCH 06/14] Still need to open the PR, good job --- python/kiss_icp/kiss_icp.py | 1 - 1 file changed, 1 deletion(-) diff --git a/python/kiss_icp/kiss_icp.py b/python/kiss_icp/kiss_icp.py index 239c7845..a8e62beb 100644 --- a/python/kiss_icp/kiss_icp.py +++ b/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 From d4862095b2431a657c0ffcbd3e38452593b4f6c6 Mon Sep 17 00:00:00 2001 From: tizianoGuadagnino Date: Thu, 5 Dec 2024 13:29:35 +0100 Subject: [PATCH 07/14] Include the learnings from other PRs --- cpp/kiss_icp/core/Preprocessing.cpp | 60 +++++++++++++---------------- cpp/kiss_icp/core/Preprocessing.hpp | 5 ++- 2 files changed, 31 insertions(+), 34 deletions(-) diff --git a/cpp/kiss_icp/core/Preprocessing.cpp b/cpp/kiss_icp/core/Preprocessing.cpp index 61268180..cdee67d3 100644 --- a/cpp/kiss_icp/core/Preprocessing.cpp +++ b/cpp/kiss_icp/core/Preprocessing.cpp @@ -22,10 +22,12 @@ // SOFTWARE. #include "Preprocessing.hpp" +#include #include +#include #include #include -#include +#include #include #include @@ -37,14 +39,15 @@ namespace { constexpr double mid_pose_timestamp{0.5}; -Eigen::Vector3d DeSkewPoint(const Eigen::Vector3d &point, - const double timestamp, - const Sophus::SE3d &delta) { - const auto delta_pose = delta.log(); - const auto motion = Sophus::SE3d::exp((timestamp - mid_pose_timestamp) * delta_pose); - return motion * point; -} - +struct MotionDeskewer { + Eigen::Vector3d operator()(const Eigen::Vector3d &point, + const double timestamp, + const Sophus::SE3d &delta) { + const auto delta_pose = delta.log(); + const auto motion = Sophus::SE3d::exp((timestamp - mid_pose_timestamp) * delta_pose); + return motion * point; + } +}; } // namespace namespace kiss_icp { @@ -55,9 +58,13 @@ Preprocessor::Preprocessor(const double max_range, const int max_num_threads) : max_range_(max_range), min_range_(min_range), - deskew_(deskew), + deskewer_( + [](const Eigen::Vector3d &point, const double, const Sophus::SE3d &) { return point; }), max_num_threads_(max_num_threads > 0 ? max_num_threads : tbb::this_task_arena::max_concurrency()) { + if (deskew) { + deskewer_ = MotionDeskewer(); + } // 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( @@ -67,37 +74,24 @@ Preprocessor::Preprocessor(const double max_range, std::vector Preprocessor::Preprocess(const std::vector &frame, const std::vector ×tamps, const Sophus::SE3d &relative_motion) const { - std::vector preprocessed_frame; + tbb::concurrent_vector preprocessed_frame; preprocessed_frame.reserve(frame.size()); - preprocessed_frame = tbb::parallel_reduce( + tbb::parallel_for( // Index Range tbb::blocked_range(0, frame.size()), - // Initial value - preprocessed_frame, // Parallel Compute - [&](const tbb::blocked_range &r, - std::vector preprocessed_block) -> std::vector { - preprocessed_block.reserve(r.size()); + [&](const tbb::blocked_range &r) { for (size_t idx = r.begin(); idx < r.end(); ++idx) { - const auto &point = - (deskew_ && !timestamps.empty()) - ? DeSkewPoint(frame.at(idx), timestamps.at(idx), relative_motion) - : frame.at(idx); - const double point_range = point.norm(); + const double point_range = frame.at(idx).norm(); if (point_range < max_range_ && point_range > min_range_) { - preprocessed_block.emplace_back(point); + const auto &point = + deskewer_(frame.at(idx), timestamps.at(idx), relative_motion); + preprocessed_frame.emplace_back(point); } }; - return preprocessed_block; - }, - // Reduction - [](std::vector a, - const std::vector &b) -> std::vector { - a.insert(a.end(), // - std::make_move_iterator(b.cbegin()), // - std::make_move_iterator(b.cend())); - return a; }); - return preprocessed_frame; + std::vector returned(std::make_move_iterator(preprocessed_frame.cbegin()), // + std::make_move_iterator(preprocessed_frame.cend())); + return returned; } } // namespace kiss_icp diff --git a/cpp/kiss_icp/core/Preprocessing.hpp b/cpp/kiss_icp/core/Preprocessing.hpp index 9443ed86..352f2d75 100644 --- a/cpp/kiss_icp/core/Preprocessing.hpp +++ b/cpp/kiss_icp/core/Preprocessing.hpp @@ -23,6 +23,7 @@ #pragma once #include +#include #include #include @@ -39,7 +40,9 @@ struct Preprocessor { const Sophus::SE3d &relative_motion) const; double max_range_; double min_range_; - bool deskew_; + using DeskewerType = + std::function; + DeskewerType deskewer_; int max_num_threads_; }; } // namespace kiss_icp From 54fa150562c225f8161e25ae374d497f30a87577 Mon Sep 17 00:00:00 2001 From: tizianoGuadagnino Date: Thu, 5 Dec 2024 16:03:21 +0100 Subject: [PATCH 08/14] Huglier solution but it works --- cpp/kiss_icp/core/Preprocessing.cpp | 44 +++++++++++++++++++---------- cpp/kiss_icp/core/Preprocessing.hpp | 4 +-- 2 files changed, 30 insertions(+), 18 deletions(-) diff --git a/cpp/kiss_icp/core/Preprocessing.cpp b/cpp/kiss_icp/core/Preprocessing.cpp index cdee67d3..e7af85ca 100644 --- a/cpp/kiss_icp/core/Preprocessing.cpp +++ b/cpp/kiss_icp/core/Preprocessing.cpp @@ -37,15 +37,30 @@ #include namespace { -constexpr double mid_pose_timestamp{0.5}; +static constexpr double end_pose_stamp{1.0}; +struct StubDeskewer { + StubDeskewer(const std::vector &frame, + const std::vector ×tamps, + const Sophus::SE3d &relative_motion) + : points_(&frame), stamps_(×tamps), motion_(relative_motion) {} -struct MotionDeskewer { - Eigen::Vector3d operator()(const Eigen::Vector3d &point, - const double timestamp, - const Sophus::SE3d &delta) { - const auto delta_pose = delta.log(); - const auto motion = Sophus::SE3d::exp((timestamp - mid_pose_timestamp) * delta_pose); - return motion * point; + Eigen::Vector3d operator()(const size_t idx) { return points_->at(idx); } + + const std::vector *points_; + const std::vector *stamps_; + const Sophus::SE3d motion_; +}; + +struct MotionDeskewer : public StubDeskewer { + MotionDeskewer(const std::vector &frame, + const std::vector ×tamps, + const Sophus::SE3d &relative_motion) + : StubDeskewer(frame, timestamps, relative_motion) {} + + Eigen::Vector3d operator()(const size_t idx) { + const auto delta_pose = motion_.log(); + const auto motion = Sophus::SE3d::exp((stamps_->at(idx) - end_pose_stamp) * delta_pose); + return motion * points_->at(idx); } }; } // namespace @@ -58,13 +73,9 @@ Preprocessor::Preprocessor(const double max_range, const int max_num_threads) : max_range_(max_range), min_range_(min_range), - deskewer_( - [](const Eigen::Vector3d &point, const double, const Sophus::SE3d &) { return point; }), + deskew_(deskew), max_num_threads_(max_num_threads > 0 ? max_num_threads : tbb::this_task_arena::max_concurrency()) { - if (deskew) { - deskewer_ = MotionDeskewer(); - } // 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( @@ -74,6 +85,10 @@ Preprocessor::Preprocessor(const double max_range, std::vector Preprocessor::Preprocess(const std::vector &frame, const std::vector ×tamps, const Sophus::SE3d &relative_motion) const { + using DeskewerType = std::function; + DeskewerType compute_deskewed_point = (deskew_ && !timestamps.empty()) + ? MotionDeskewer(frame, timestamps, relative_motion) + : StubDeskewer(frame, timestamps, relative_motion); tbb::concurrent_vector preprocessed_frame; preprocessed_frame.reserve(frame.size()); tbb::parallel_for( @@ -84,8 +99,7 @@ std::vector Preprocessor::Preprocess(const std::vector min_range_) { - const auto &point = - deskewer_(frame.at(idx), timestamps.at(idx), relative_motion); + const auto &point = compute_deskewed_point(idx); preprocessed_frame.emplace_back(point); } }; diff --git a/cpp/kiss_icp/core/Preprocessing.hpp b/cpp/kiss_icp/core/Preprocessing.hpp index 352f2d75..ad2bdd81 100644 --- a/cpp/kiss_icp/core/Preprocessing.hpp +++ b/cpp/kiss_icp/core/Preprocessing.hpp @@ -40,9 +40,7 @@ struct Preprocessor { const Sophus::SE3d &relative_motion) const; double max_range_; double min_range_; - using DeskewerType = - std::function; - DeskewerType deskewer_; + bool deskew_; int max_num_threads_; }; } // namespace kiss_icp From bbe87aa458920115574e4677e1657faa77c91895 Mon Sep 17 00:00:00 2001 From: tizianoGuadagnino Date: Thu, 5 Dec 2024 16:13:58 +0100 Subject: [PATCH 09/14] Better --- cpp/kiss_icp/core/Preprocessing.cpp | 38 +++++++++++++---------------- 1 file changed, 17 insertions(+), 21 deletions(-) diff --git a/cpp/kiss_icp/core/Preprocessing.cpp b/cpp/kiss_icp/core/Preprocessing.cpp index e7af85ca..a3b33474 100644 --- a/cpp/kiss_icp/core/Preprocessing.cpp +++ b/cpp/kiss_icp/core/Preprocessing.cpp @@ -39,28 +39,23 @@ namespace { static constexpr double end_pose_stamp{1.0}; struct StubDeskewer { - StubDeskewer(const std::vector &frame, - const std::vector ×tamps, - const Sophus::SE3d &relative_motion) - : points_(&frame), stamps_(×tamps), motion_(relative_motion) {} + StubDeskewer(const std::vector ×tamps, const Sophus::SE3d &relative_motion) + : stamps_(timestamps), motion_(relative_motion) {} - Eigen::Vector3d operator()(const size_t idx) { return points_->at(idx); } + Eigen::Vector3d operator()(const Eigen::Vector3d &point, const size_t &) { return point; } - const std::vector *points_; - const std::vector *stamps_; + const std::vector &stamps_; const Sophus::SE3d motion_; }; struct MotionDeskewer : public StubDeskewer { - MotionDeskewer(const std::vector &frame, - const std::vector ×tamps, - const Sophus::SE3d &relative_motion) - : StubDeskewer(frame, timestamps, relative_motion) {} + MotionDeskewer(const std::vector ×tamps, const Sophus::SE3d &relative_motion) + : StubDeskewer(timestamps, relative_motion) {} - Eigen::Vector3d operator()(const size_t idx) { + Eigen::Vector3d operator()(const Eigen::Vector3d &point, const size_t &idx) { const auto delta_pose = motion_.log(); - const auto motion = Sophus::SE3d::exp((stamps_->at(idx) - end_pose_stamp) * delta_pose); - return motion * points_->at(idx); + const auto motion = Sophus::SE3d::exp((stamps_.at(idx) - end_pose_stamp) * delta_pose); + return motion * point; } }; } // namespace @@ -85,10 +80,10 @@ Preprocessor::Preprocessor(const double max_range, std::vector Preprocessor::Preprocess(const std::vector &frame, const std::vector ×tamps, const Sophus::SE3d &relative_motion) const { - using DeskewerType = std::function; - DeskewerType compute_deskewed_point = (deskew_ && !timestamps.empty()) - ? MotionDeskewer(frame, timestamps, relative_motion) - : StubDeskewer(frame, timestamps, relative_motion); + using DeskewerType = std::function; + DeskewerType deskewer = (deskew_ && !timestamps.empty()) + ? MotionDeskewer(timestamps, relative_motion) + : StubDeskewer(timestamps, relative_motion); tbb::concurrent_vector preprocessed_frame; preprocessed_frame.reserve(frame.size()); tbb::parallel_for( @@ -97,10 +92,11 @@ std::vector Preprocessor::Preprocess(const std::vector &r) { for (size_t idx = r.begin(); idx < r.end(); ++idx) { - const double point_range = frame.at(idx).norm(); + const auto &point = frame.at(idx); + const double point_range = point.norm(); if (point_range < max_range_ && point_range > min_range_) { - const auto &point = compute_deskewed_point(idx); - preprocessed_frame.emplace_back(point); + const auto &deskewed_point = deskewer(point, idx); + preprocessed_frame.emplace_back(deskewed_point); } }; }); From f8951bc9b8365f943283a51488785ce819619feb Mon Sep 17 00:00:00 2001 From: tizianoGuadagnino Date: Thu, 5 Dec 2024 17:14:19 +0100 Subject: [PATCH 10/14] Midpose --- cpp/kiss_icp/core/Preprocessing.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/cpp/kiss_icp/core/Preprocessing.cpp b/cpp/kiss_icp/core/Preprocessing.cpp index a3b33474..186ee174 100644 --- a/cpp/kiss_icp/core/Preprocessing.cpp +++ b/cpp/kiss_icp/core/Preprocessing.cpp @@ -37,7 +37,7 @@ #include namespace { -static constexpr double end_pose_stamp{1.0}; +constexpr double mid_pose_timestamp{0.5}; struct StubDeskewer { StubDeskewer(const std::vector ×tamps, const Sophus::SE3d &relative_motion) : stamps_(timestamps), motion_(relative_motion) {} @@ -54,7 +54,7 @@ struct MotionDeskewer : public StubDeskewer { Eigen::Vector3d operator()(const Eigen::Vector3d &point, const size_t &idx) { const auto delta_pose = motion_.log(); - const auto motion = Sophus::SE3d::exp((stamps_.at(idx) - end_pose_stamp) * delta_pose); + const auto motion = Sophus::SE3d::exp((stamps_.at(idx) - mid_pose_timestamp) * delta_pose); return motion * point; } }; From c45024655eee440d4b44d5639b6dead8e972289f Mon Sep 17 00:00:00 2001 From: tizianoGuadagnino Date: Thu, 5 Dec 2024 17:30:25 +0100 Subject: [PATCH 11/14] Actually, lets deskew at the end of the scan as it should be --- cpp/kiss_icp/core/Preprocessing.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/cpp/kiss_icp/core/Preprocessing.cpp b/cpp/kiss_icp/core/Preprocessing.cpp index 186ee174..63ff0b3c 100644 --- a/cpp/kiss_icp/core/Preprocessing.cpp +++ b/cpp/kiss_icp/core/Preprocessing.cpp @@ -37,7 +37,7 @@ #include namespace { -constexpr double mid_pose_timestamp{0.5}; +constexpr double final_pose_stamp{1.0}; struct StubDeskewer { StubDeskewer(const std::vector ×tamps, const Sophus::SE3d &relative_motion) : stamps_(timestamps), motion_(relative_motion) {} @@ -54,7 +54,7 @@ struct MotionDeskewer : public StubDeskewer { Eigen::Vector3d operator()(const Eigen::Vector3d &point, const size_t &idx) { const auto delta_pose = motion_.log(); - const auto motion = Sophus::SE3d::exp((stamps_.at(idx) - mid_pose_timestamp) * delta_pose); + const auto motion = Sophus::SE3d::exp((stamps_.at(idx) - final_pose_stamp) * delta_pose); return motion * point; } }; From 2953d9f26c9b112465326f1494ecf04c9b28dd87 Mon Sep 17 00:00:00 2001 From: tizianoGuadagnino Date: Thu, 5 Dec 2024 17:30:54 +0100 Subject: [PATCH 12/14] Revert "Actually, lets deskew at the end of the scan as it should be" This reverts commit c45024655eee440d4b44d5639b6dead8e972289f. --- cpp/kiss_icp/core/Preprocessing.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/cpp/kiss_icp/core/Preprocessing.cpp b/cpp/kiss_icp/core/Preprocessing.cpp index 63ff0b3c..186ee174 100644 --- a/cpp/kiss_icp/core/Preprocessing.cpp +++ b/cpp/kiss_icp/core/Preprocessing.cpp @@ -37,7 +37,7 @@ #include namespace { -constexpr double final_pose_stamp{1.0}; +constexpr double mid_pose_timestamp{0.5}; struct StubDeskewer { StubDeskewer(const std::vector ×tamps, const Sophus::SE3d &relative_motion) : stamps_(timestamps), motion_(relative_motion) {} @@ -54,7 +54,7 @@ struct MotionDeskewer : public StubDeskewer { Eigen::Vector3d operator()(const Eigen::Vector3d &point, const size_t &idx) { const auto delta_pose = motion_.log(); - const auto motion = Sophus::SE3d::exp((stamps_.at(idx) - final_pose_stamp) * delta_pose); + const auto motion = Sophus::SE3d::exp((stamps_.at(idx) - mid_pose_timestamp) * delta_pose); return motion * point; } }; From 036240aff6d97b55822f83f82cda88fbf487bb6a Mon Sep 17 00:00:00 2001 From: tizianoGuadagnino Date: Thu, 5 Dec 2024 17:35:06 +0100 Subject: [PATCH 13/14] Fix pipeline for 20.04 --- cpp/kiss_icp/core/Preprocessing.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/cpp/kiss_icp/core/Preprocessing.cpp b/cpp/kiss_icp/core/Preprocessing.cpp index 186ee174..ebf1ef38 100644 --- a/cpp/kiss_icp/core/Preprocessing.cpp +++ b/cpp/kiss_icp/core/Preprocessing.cpp @@ -22,7 +22,6 @@ // SOFTWARE. #include "Preprocessing.hpp" -#include #include #include #include From b548d3006133f6343e146e05b0e6612254c4eba3 Mon Sep 17 00:00:00 2001 From: tizianoGuadagnino Date: Fri, 6 Dec 2024 10:31:50 +0100 Subject: [PATCH 14/14] Const correctness just in case --- cpp/kiss_icp/core/Preprocessing.cpp | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/cpp/kiss_icp/core/Preprocessing.cpp b/cpp/kiss_icp/core/Preprocessing.cpp index ebf1ef38..e9c3eb2b 100644 --- a/cpp/kiss_icp/core/Preprocessing.cpp +++ b/cpp/kiss_icp/core/Preprocessing.cpp @@ -41,7 +41,7 @@ struct StubDeskewer { StubDeskewer(const std::vector ×tamps, const Sophus::SE3d &relative_motion) : stamps_(timestamps), motion_(relative_motion) {} - Eigen::Vector3d operator()(const Eigen::Vector3d &point, const size_t &) { return point; } + Eigen::Vector3d operator()(const Eigen::Vector3d &point, const size_t &) const { return point; } const std::vector &stamps_; const Sophus::SE3d motion_; @@ -51,7 +51,7 @@ struct MotionDeskewer : public StubDeskewer { MotionDeskewer(const std::vector ×tamps, const Sophus::SE3d &relative_motion) : StubDeskewer(timestamps, relative_motion) {} - Eigen::Vector3d operator()(const Eigen::Vector3d &point, const size_t &idx) { + 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; @@ -80,9 +80,9 @@ std::vector Preprocessor::Preprocess(const std::vector ×tamps, const Sophus::SE3d &relative_motion) const { using DeskewerType = std::function; - DeskewerType deskewer = (deskew_ && !timestamps.empty()) - ? MotionDeskewer(timestamps, relative_motion) - : StubDeskewer(timestamps, relative_motion); + const DeskewerType deskewer = (deskew_ && !timestamps.empty()) + ? MotionDeskewer(timestamps, relative_motion) + : StubDeskewer(timestamps, relative_motion); tbb::concurrent_vector preprocessed_frame; preprocessed_frame.reserve(frame.size()); tbb::parallel_for(