diff --git a/cpp/kiss_icp/core/CMakeLists.txt b/cpp/kiss_icp/core/CMakeLists.txt index fc627bf4..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 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) 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..d8e60da1 100644 --- a/cpp/kiss_icp/core/Preprocessing.cpp +++ b/cpp/kiss_icp/core/Preprocessing.cpp @@ -22,23 +22,73 @@ // SOFTWARE. #include "Preprocessing.hpp" -#include +#include +#include +#include +#include +#include +#include #include #include #include +#include #include +namespace { +constexpr double mid_pose_stamp{0.5}; +} // 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 Sophus::SE3d &relative_motion) const { + const std::vector &deskewed_frame = [&]() { + if (!deskew_ || timestamps.empty()) { + return frame; + } else { + const auto motion = relative_motion.log(); + std::vector deskewed_frame(frame.size()); + tbb::parallel_for( + // Index Range + tbb::blocked_range{0, deskewed_frame.size()}, + // Parallel Compute + [&](const tbb::blocked_range &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 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 diff --git a/cpp/kiss_icp/core/Preprocessing.hpp b/cpp/kiss_icp/core/Preprocessing.hpp index d161177d..ad2bdd81 100644 --- a/cpp/kiss_icp/core/Preprocessing.hpp +++ b/cpp/kiss_icp/core/Preprocessing.hpp @@ -23,12 +23,24 @@ #pragma once #include +#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); + + std::vector Preprocess(const std::vector &frame, + const std::vector ×tamps, + const Sophus::SE3d &relative_motion) const; + double max_range_; + double min_range_; + bool deskew_; + int max_num_threads_; +}; } // 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..0ca3e52e 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,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 &frame); Vector3dVectorTuple RegisterFrame(const std::vector &frame, const std::vector ×tamps); Vector3dVectorTuple Voxelize(const std::vector &frame) const; @@ -88,6 +89,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/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..f53349c8 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 @@ -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) diff --git a/python/kiss_icp/pipeline.py b/python/kiss_icp/pipeline.py index aa45d9a3..27a7bbc2 100644 --- a/python/kiss_icp/pipeline.py +++ b/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 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 2291c04f..ab1c1435 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" @@ -74,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 @@ -107,19 +119,8 @@ 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); /// 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)