From 59bc9fc447113214420b15c821a41df44e661b83 Mon Sep 17 00:00:00 2001 From: Tiziano Guadagnino <37455909+tizianoGuadagnino@users.noreply.github.com> Date: Tue, 19 Nov 2024 17:14:04 +0100 Subject: [PATCH] Kinematic ICP Threshold (#15) * We didnt have proper flagging in the online node * Clean launch common args * Add new threshold * Update CMakeLists.txt (#7) Make *USE_SYSTEM_SOPHUS* consistent with the rest of the options. * Update README.md (#6) More accurate statement in the readme, the system does not need to follow a unicycle motion model, just the pose correction needs to do that, which applies to much more robotics platforms. * Add python checks to pre-commit (#9) * Add black and isort * Format * use black for isort * Fixing ROS launch system (#8) * We didnt have proper flagging in the online node * Clean launch common args * Fix formatting python * Tiny modification, but makes sense * Renaming and add configuration for the correspondence threshold * Fix pipeline * Consistency in the CMakeLists * Remove ternary operator and make the two functions consistent * Update cpp/kinematic_icp/correspondence_threshold/CorrespondenceThreshold.hpp Co-authored-by: Benedikt Mersch * Renaming according to Ben's review * Why a ref * Odometry Regularization (#19) * Natural extension to make also the odometry regularization optional * Consistency in the CMakeLists * Remove ternary operator and make the two functions consistent * Update cpp/kinematic_icp/correspondence_threshold/CorrespondenceThreshold.hpp Co-authored-by: Benedikt Mersch * Renaming according to Ben's review * Why a ref * Use the same default --------- Co-authored-by: Benedikt Mersch Co-authored-by: Benedikt Mersch --------- Co-authored-by: Benedikt Mersch Co-authored-by: Benedikt Mersch --- cpp/kinematic_icp/CMakeLists.txt | 1 + .../correspondence_threshold/CMakeLists.txt | 26 ++++++++ .../CorrespondenceThreshold.cpp | 66 +++++++++++++++++++ .../CorrespondenceThreshold.hpp | 56 ++++++++++++++++ cpp/kinematic_icp/pipeline/CMakeLists.txt | 3 +- cpp/kinematic_icp/pipeline/KinematicICP.cpp | 8 +-- cpp/kinematic_icp/pipeline/KinematicICP.hpp | 51 ++++++++++---- .../registration/Registration.cpp | 21 ++++-- .../registration/Registration.hpp | 10 ++- ros/config/kinematic_icp_ros.yaml | 15 +++-- .../server/LidarOdometryServer.cpp | 20 ++++-- 11 files changed, 241 insertions(+), 36 deletions(-) create mode 100644 cpp/kinematic_icp/correspondence_threshold/CMakeLists.txt create mode 100644 cpp/kinematic_icp/correspondence_threshold/CorrespondenceThreshold.cpp create mode 100644 cpp/kinematic_icp/correspondence_threshold/CorrespondenceThreshold.hpp diff --git a/cpp/kinematic_icp/CMakeLists.txt b/cpp/kinematic_icp/CMakeLists.txt index 259cbba..93889b4 100644 --- a/cpp/kinematic_icp/CMakeLists.txt +++ b/cpp/kinematic_icp/CMakeLists.txt @@ -54,5 +54,6 @@ set(CMAKE_POSITION_INDEPENDENT_CODE ON) include(cmake/CompilerOptions.cmake) +add_subdirectory(correspondence_threshold) add_subdirectory(registration) add_subdirectory(pipeline) diff --git a/cpp/kinematic_icp/correspondence_threshold/CMakeLists.txt b/cpp/kinematic_icp/correspondence_threshold/CMakeLists.txt new file mode 100644 index 0000000..df30730 --- /dev/null +++ b/cpp/kinematic_icp/correspondence_threshold/CMakeLists.txt @@ -0,0 +1,26 @@ +# 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 STATIC) +target_sources(kinematic_icp_threshold PRIVATE CorrespondenceThreshold.cpp) +target_link_libraries(kinematic_icp_threshold PUBLIC Sophus::Sophus) +set_global_target_properties(kinematic_icp_threshold) diff --git a/cpp/kinematic_icp/correspondence_threshold/CorrespondenceThreshold.cpp b/cpp/kinematic_icp/correspondence_threshold/CorrespondenceThreshold.cpp new file mode 100644 index 0000000..1baf3d7 --- /dev/null +++ b/cpp/kinematic_icp/correspondence_threshold/CorrespondenceThreshold.cpp @@ -0,0 +1,66 @@ +// 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 "CorrespondenceThreshold.hpp" + +#include +#include + +namespace { +double OdometryErrorInPointSpace(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 { +CorrespondenceThreshold::CorrespondenceThreshold(const double map_discretization_error, + const double max_range, + const bool use_adaptive_threshold, + const double fixed_threshold) + : map_discretization_error_(map_discretization_error), + max_range_(max_range), + use_adaptive_threshold_(use_adaptive_threshold), + fixed_threshold_(fixed_threshold), + odom_sse_(0.0), + num_samples_(1e-8) {} + +double CorrespondenceThreshold::ComputeThreshold() const { + if (!use_adaptive_threshold_) return fixed_threshold_; + + const double sigma_odom = std::sqrt(odom_sse_ / num_samples_); + const double &sigma_map = map_discretization_error_; // <-- Renaming for clarity + const double adaptive_threshold = 3.0 * (sigma_map + sigma_odom); + return adaptive_threshold; +} + +void CorrespondenceThreshold::UpdateOdometryError(const Sophus::SE3d &odometry_error) { + if (!use_adaptive_threshold_) return; + + const double &odom_error_in_point_space = OdometryErrorInPointSpace(odometry_error, max_range_); + odom_sse_ += odom_error_in_point_space * odom_error_in_point_space; + num_samples_ += 1.0; +} + +} // namespace kinematic_icp diff --git a/cpp/kinematic_icp/correspondence_threshold/CorrespondenceThreshold.hpp b/cpp/kinematic_icp/correspondence_threshold/CorrespondenceThreshold.hpp new file mode 100644 index 0000000..d699ae8 --- /dev/null +++ b/cpp/kinematic_icp/correspondence_threshold/CorrespondenceThreshold.hpp @@ -0,0 +1,56 @@ +// 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 CorrespondenceThreshold { + explicit CorrespondenceThreshold(const double map_discretization_error, + const double max_range, + const bool use_adaptive_threshold, + const double fixed_threshold); + + void UpdateOdometryError(const Sophus::SE3d &odometry_error); + + double ComputeThreshold() const; + + inline void Reset() { + odom_sse_ = 0.0; + num_samples_ = 1e-8; + } + + // configurable parameters + double map_discretization_error_; // <-- Error introduced by the fact that we have a discrete + // set of points of the surface we are measuring + double max_range_; + bool use_adaptive_threshold_; + double fixed_threshold_; // <-- Ignored if use_adaptive_threshold_ = true + + // Local cache for computation + double odom_sse_; + double num_samples_; +}; +} // namespace kinematic_icp diff --git a/cpp/kinematic_icp/pipeline/CMakeLists.txt b/cpp/kinematic_icp/pipeline/CMakeLists.txt index 25fa5b3..3a95025 100644 --- a/cpp/kinematic_icp/pipeline/CMakeLists.txt +++ b/cpp/kinematic_icp/pipeline/CMakeLists.txt @@ -22,5 +22,6 @@ # SOFTWARE. add_library(kinematic_icp_pipeline STATIC) target_sources(kinematic_icp_pipeline PRIVATE KinematicICP.cpp) -target_link_libraries(kinematic_icp_pipeline PUBLIC kinematic_icp_registration kiss_icp_pipeline) +target_link_libraries(kinematic_icp_pipeline PUBLIC kinematic_icp_registration kinematic_icp_threshold + kiss_icp_pipeline) set_global_target_properties(kinematic_icp_pipeline) diff --git a/cpp/kinematic_icp/pipeline/KinematicICP.cpp b/cpp/kinematic_icp/pipeline/KinematicICP.cpp index c93e878..aece4c5 100644 --- a/cpp/kinematic_icp/pipeline/KinematicICP.cpp +++ b/cpp/kinematic_icp/pipeline/KinematicICP.cpp @@ -66,20 +66,20 @@ KinematicICP::Vector3dVectorTuple KinematicICP::RegisterFrame( const auto &[source, frame_downsample] = Voxelize(cropped_frame, config_.voxel_size); // Get adaptive_threshold - const double sigma = adaptive_threshold_.ComputeThreshold(); + const double tau = correspondence_threshold_.ComputeThreshold(); // Run ICP const auto new_pose = registration_.ComputeRobotMotion(source, // frame local_map_, // voxel_map last_pose_, // last_pose relative_odometry, // robot_motion - 3 * sigma); // max_correspondence_dist + tau); // max_correspondence_dist // Compute the difference between the prediction and the actual estimate - const auto model_deviation = (last_pose_ * relative_odometry).inverse() * new_pose; + const auto odometry_error = (last_pose_ * relative_odometry).inverse() * new_pose; // Update step: threshold, local map and the last pose - adaptive_threshold_.UpdateModelDeviation(model_deviation); + correspondence_threshold_.UpdateOdometryError(odometry_error); local_map_.Update(frame_downsample, new_pose); last_pose_ = new_pose; diff --git a/cpp/kinematic_icp/pipeline/KinematicICP.hpp b/cpp/kinematic_icp/pipeline/KinematicICP.hpp index 2aff12e..c17a62e 100644 --- a/cpp/kinematic_icp/pipeline/KinematicICP.hpp +++ b/cpp/kinematic_icp/pipeline/KinematicICP.hpp @@ -23,6 +23,7 @@ #pragma once #include +#include #include #include #include @@ -30,23 +31,52 @@ #include #include +#include "kinematic_icp/correspondence_threshold/CorrespondenceThreshold.hpp" #include "kinematic_icp/registration/Registration.hpp" namespace kinematic_icp::pipeline { -using Config = kiss_icp::pipeline::KISSConfig; +struct Config { + // Preprocessing + double max_range = 100.0; + double min_range = 0.0; + // Mapping parameters + double voxel_size = 1.0; + unsigned int max_points_per_voxel = 20; + // Derived parameter, will be computed from other parts of the configuration + constexpr double map_resolution() const { return voxel_size / std::sqrt(max_points_per_voxel); } + // Correspondence threshold parameters + bool use_adaptive_threshold = true; + double fixed_threshold = 1.0; // <-- Ignored if use_adaptive_threshold = true + + // Registration Parameters + int max_num_iterations = 10; + double convergence_criterion = 0.001; + int max_num_threads = 1; + bool use_adaptive_odometry_regularization = true; + double fixed_regularization = 0.0; // <-- Ignored if use_adaptive_threshold = true + + // Motion compensation + bool deskew = false; +}; class KinematicICP { public: using Vector3dVector = std::vector; using Vector3dVectorTuple = std::tuple; - explicit KinematicICP(const kiss_icp::pipeline::KISSConfig &config) - : registration_( - config.max_num_iterations, config.convergence_criterion, config.max_num_threads), + explicit KinematicICP(const Config &config) + : registration_(config.max_num_iterations, + config.convergence_criterion, + config.max_num_threads, + config.use_adaptive_odometry_regularization, + config.fixed_regularization), + correspondence_threshold_(config.map_resolution(), + config.max_range, + config.use_adaptive_threshold, + config.fixed_threshold), config_(config), - 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) {} + local_map_(config.voxel_size, config.max_range, config.max_points_per_voxel) {} Vector3dVectorTuple RegisterFrame(const std::vector &frame, const std::vector ×tamps, @@ -56,8 +86,7 @@ class KinematicICP { inline void SetPose(const Sophus::SE3d &pose) { last_pose_ = pose; local_map_.Clear(); - adaptive_threshold_ = kiss_icp::AdaptiveThreshold(config_.initial_threshold, - config_.min_motion_th, config_.max_range); + correspondence_threshold_.Reset(); }; std::vector LocalMap() const { return local_map_.Pointcloud(); }; @@ -68,14 +97,14 @@ class KinematicICP { const Sophus::SE3d &pose() const { return last_pose_; } Sophus::SE3d &pose() { return last_pose_; } -private: +protected: Sophus::SE3d last_pose_; // Kinematic module KinematicRegistration registration_; + 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/registration/Registration.cpp b/cpp/kinematic_icp/registration/Registration.cpp index 1cc95b0..b95286a 100644 --- a/cpp/kinematic_icp/registration/Registration.cpp +++ b/cpp/kinematic_icp/registration/Registration.cpp @@ -140,15 +140,19 @@ Eigen::Vector2d ComputePerturbation(const Correspondences &correspondences, namespace kinematic_icp { -KinematicRegistration::KinematicRegistration(int max_num_iteration, - double convergence_criterion, - int max_num_threads) +KinematicRegistration::KinematicRegistration(const int max_num_iteration, + const double convergence_criterion, + const int max_num_threads, + const bool use_adaptive_odometry_regularization, + const double fixed_regularization) : max_num_iterations_(max_num_iteration), convergence_criterion_(convergence_criterion), // Only manipulate the number of threads if the user specifies something // greater than 0 max_num_threads_(max_num_threads > 0 ? max_num_threads - : tbb::this_task_arena::max_concurrency()) { + : tbb::this_task_arena::max_concurrency()), + use_adaptive_odometry_regularization_(use_adaptive_odometry_regularization), + fixed_regularization_(fixed_regularization) { // 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( @@ -175,8 +179,13 @@ Sophus::SE3d KinematicRegistration::ComputeRobotMotion(const std::vector &frame, const kiss_icp::VoxelHashMap &voxel_map, @@ -43,5 +45,7 @@ struct KinematicRegistration { int max_num_iterations_; double convergence_criterion_; int max_num_threads_; + bool use_adaptive_odometry_regularization_; + double fixed_regularization_; }; } // namespace kinematic_icp diff --git a/ros/config/kinematic_icp_ros.yaml b/ros/config/kinematic_icp_ros.yaml index 6d4e0b6..b8d5ad3 100644 --- a/ros/config/kinematic_icp_ros.yaml +++ b/ros/config/kinematic_icp_ros.yaml @@ -3,20 +3,23 @@ # Preprocessing max_range: 100.0 min_range: 0.0 - deskew: true - # Mapping parameters voxel_size: 1.0 max_points_per_voxel: 20 - # Adaptive threshold - initial_threshold: 2.0 - min_motion_th: 0.1 + # Correspondence threshold parameters + use_adaptive_threshold: true + fixed_threshold: 1.0 # Ignored if use_adaptive_threshold=true - # Registration + # Registration Parameters max_num_iterations: 10 convergence_criterion: 0.001 max_num_threads: 1 + use_adaptive_odometry_regularization: true + fixed_regularization: 0.0 # Ignored if use_adaptive_odometry_regularization=true + + # Motion Compensation + deskew: true # Covariance diagonal values orientation_covariance: 0.1 diff --git a/ros/src/kinematic_icp_ros/server/LidarOdometryServer.cpp b/ros/src/kinematic_icp_ros/server/LidarOdometryServer.cpp index 8bffc9a..fadfc4a 100644 --- a/ros/src/kinematic_icp_ros/server/LidarOdometryServer.cpp +++ b/ros/src/kinematic_icp_ros/server/LidarOdometryServer.cpp @@ -70,21 +70,31 @@ LidarOdometryServer::LidarOdometryServer(rclcpp::Node::SharedPtr node) : node_(n duration_cast(seconds(node->declare_parameter("tf_timeout", 0.0))); kinematic_icp::pipeline::Config config; + // Preprocessing config.max_range = node->declare_parameter("max_range", config.max_range); config.min_range = node->declare_parameter("min_range", config.min_range); - config.deskew = node->declare_parameter("deskew", config.deskew); - config.voxel_size = node->declare_parameter("voxel_size", config.max_range / 100.0); + // Mapping parameters + config.voxel_size = node->declare_parameter("voxel_size", config.voxel_size); config.max_points_per_voxel = node->declare_parameter("max_points_per_voxel", config.max_points_per_voxel); - config.initial_threshold = - node->declare_parameter("initial_threshold", config.initial_threshold); - config.min_motion_th = node->declare_parameter("min_motion_th", config.min_motion_th); + // Correspondence threshold parameters + config.use_adaptive_threshold = + node->declare_parameter("use_adaptive_threshold", config.use_adaptive_threshold); + config.fixed_threshold = + node->declare_parameter("fixed_threshold", config.fixed_threshold); + // Registration Parameters config.max_num_iterations = node->declare_parameter("max_num_iterations", config.max_num_iterations); config.convergence_criterion = node->declare_parameter("convergence_criterion", config.convergence_criterion); config.max_num_threads = node->declare_parameter("max_num_threads", config.max_num_threads); + config.use_adaptive_odometry_regularization = node->declare_parameter( + "use_adaptive_odometry_regularization", config.use_adaptive_odometry_regularization); + config.fixed_regularization = + node->declare_parameter("fixed_regularization", config.fixed_regularization); + // Motion compensation + config.deskew = node->declare_parameter("deskew", config.deskew); if (config.max_range < config.min_range) { RCLCPP_WARN(node_->get_logger(), "[WARNING] max_range is smaller than min_range, settng min_range to 0.0");