From 73aa68ab4c4fd75c36fdfe6a56bde97c164cccf7 Mon Sep 17 00:00:00 2001 From: tizianoGuadagnino Date: Fri, 8 Nov 2024 13:59:00 +0100 Subject: [PATCH] Renaming and add configuration for the correspondence threshold --- cpp/kinematic_icp/pipeline/KinematicICP.hpp | 2 - .../threshold/AdaptiveThreshold.cpp | 51 ------------------ .../threshold/AdaptiveThreshold.hpp | 52 ------------------- cpp/kinematic_icp/threshold/CMakeLists.txt | 25 --------- 4 files changed, 130 deletions(-) delete mode 100644 cpp/kinematic_icp/threshold/AdaptiveThreshold.cpp delete mode 100644 cpp/kinematic_icp/threshold/AdaptiveThreshold.hpp delete mode 100644 cpp/kinematic_icp/threshold/CMakeLists.txt diff --git a/cpp/kinematic_icp/pipeline/KinematicICP.hpp b/cpp/kinematic_icp/pipeline/KinematicICP.hpp index ddad2f4..c17a62e 100644 --- a/cpp/kinematic_icp/pipeline/KinematicICP.hpp +++ b/cpp/kinematic_icp/pipeline/KinematicICP.hpp @@ -104,9 +104,7 @@ class KinematicICP { CorrespondenceThreshold correspondence_threshold_; Config config_; // KISS-ICP pipeline modules - kiss_icp::pipeline::KISSConfig config_; kiss_icp::VoxelHashMap local_map_; - kiss_icp::AdaptiveThreshold adaptive_threshold_; }; } // namespace kinematic_icp::pipeline diff --git a/cpp/kinematic_icp/threshold/AdaptiveThreshold.cpp b/cpp/kinematic_icp/threshold/AdaptiveThreshold.cpp deleted file mode 100644 index 0801929..0000000 --- a/cpp/kinematic_icp/threshold/AdaptiveThreshold.cpp +++ /dev/null @@ -1,51 +0,0 @@ -// MIT License -// -// Copyright (c) 2024 Tiziano Guadagnino, Benedikt Mersch, Ignacio Vizzo, 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 "AdaptiveThreshold.hpp" - -#include -#include - -namespace { -double ModelError(const Sophus::SE3d &pose, const double max_range) { - const double &theta = pose.so3().logAndTheta().theta; - const double &delta_rot = 2.0 * max_range * std::sin(theta / 2.0); - const double &delta_trans = pose.translation().norm(); - return delta_trans + delta_rot; -}; -} // namespace - -namespace kinematic_icp { -AdaptiveThreshold::AdaptiveThreshold(const double map_discretization_error, const double max_range) - : map_discretization_error_(map_discretization_error), - max_range_(max_range), - model_sse_(0.0), - num_samples_(1) {} - -void AdaptiveThreshold::UpdateModelError(const Sophus::SE3d ¤t_deviation) { - const double centered_model_error = - ModelError(current_deviation, max_range_) - map_discretization_error_; - model_sse_ += centered_model_error * centered_model_error; - num_samples_++; -} - -} // namespace kinematic_icp diff --git a/cpp/kinematic_icp/threshold/AdaptiveThreshold.hpp b/cpp/kinematic_icp/threshold/AdaptiveThreshold.hpp deleted file mode 100644 index 60d504a..0000000 --- a/cpp/kinematic_icp/threshold/AdaptiveThreshold.hpp +++ /dev/null @@ -1,52 +0,0 @@ -// MIT License -// -// Copyright (c) 2024 Tiziano Guadagnino, Benedikt Mersch, Ignacio Vizzo, 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 - -namespace kinematic_icp { - -struct AdaptiveThreshold { - explicit AdaptiveThreshold(const double map_discretization_error, const double max_range); - - void UpdateModelError(const Sophus::SE3d ¤t_deviation); - - inline double ComputeThreshold() const { - return map_discretization_error_ + 3.0 * std::sqrt(model_sse_ / num_samples_); - } - - inline void Reset() { - model_sse_ = 0.0; - num_samples_ = 1; - } - - // configurable parameters - double map_discretization_error_; - double max_range_; - - // Local cache for ccomputation - double model_sse_; - int num_samples_; -}; -} // namespace kinematic_icp diff --git a/cpp/kinematic_icp/threshold/CMakeLists.txt b/cpp/kinematic_icp/threshold/CMakeLists.txt deleted file mode 100644 index 01d5f57..0000000 --- a/cpp/kinematic_icp/threshold/CMakeLists.txt +++ /dev/null @@ -1,25 +0,0 @@ -# MIT License -# -# Copyright (c) 2024 Tiziano Guadagnino, Benedikt Mersch, Ignacio Vizzo, 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. -add_library(kinematic_icp_threshold AdaptiveThreshold.cpp) -target_link_libraries(kinematic_icp_threshold Sophus::Sophus) -set_global_target_properties(kinematic_icp_threshold)