From 51c302f2b8b6bdedd06fbc885ebe8e937daf216c Mon Sep 17 00:00:00 2001 From: tizianoGuadagnino Date: Wed, 16 Oct 2024 15:10:07 +0200 Subject: [PATCH 01/16] We didnt have proper flagging in the online node --- ros/launch/online_node.launch.py | 25 +++++++++++++++++-------- 1 file changed, 17 insertions(+), 8 deletions(-) diff --git a/ros/launch/online_node.launch.py b/ros/launch/online_node.launch.py index 01c0a2a..1d3a198 100644 --- a/ros/launch/online_node.launch.py +++ b/ros/launch/online_node.launch.py @@ -5,13 +5,16 @@ from launch.launch_description_sources import ( get_launch_description_from_python_launch_file, ) +from launch.conditions import IfCondition from launch.substitutions import LaunchConfiguration, PythonExpression from launch_ros.actions import Node from launch_ros.substitutions.find_package import get_package_share_directory def generate_launch_description(): - use_sim_time = LaunchConfiguration("use_sim_time", default=(os.getenv("SIMULATION") == "true")) + use_sim_time = LaunchConfiguration( + "use_sim_time", default=(os.getenv("SIMULATION") == "true") + ) tf_timeout = LaunchConfiguration( "tf_timeout", default=PythonExpression(["'0.1' if ", use_sim_time, " else '0.0'"]), @@ -31,7 +34,8 @@ def generate_launch_description(): ], parameters=[ # KISS-ICP configuration - get_package_share_directory("kinematic_icp") + "/config/kinematic_icp_ros.yaml", + get_package_share_directory("kinematic_icp") + + "/config/kinematic_icp_ros.yaml", { # Input topic, is not a remap to marry API with offline node "input": LaunchConfiguration("lidar_topic"), @@ -47,10 +51,15 @@ def generate_launch_description(): }, ], ) - - return LaunchDescription( - [ - common_launch_args, - kinematic_icp_online_node, - ] + rviz_node = Node( + package="rviz2", + executable="rviz2", + output="screen", + arguments=[ + "-d", + get_package_share_directory("kinematic_icp") + "/rviz/kinematic_icp.rviz", + ], + condition=IfCondition(LaunchConfiguration("visualize")), ) + + return LaunchDescription([common_launch_args, kinematic_icp_online_node, rviz_node]) From 925d05284397ce1d916e6e628a69e73f9a62bc39 Mon Sep 17 00:00:00 2001 From: tizianoGuadagnino Date: Wed, 16 Oct 2024 15:11:54 +0200 Subject: [PATCH 02/16] Clean launch common args --- ros/launch/common_args.launch.py | 7 +------ 1 file changed, 1 insertion(+), 6 deletions(-) diff --git a/ros/launch/common_args.launch.py b/ros/launch/common_args.launch.py index 2d276dc..9ff7c4c 100644 --- a/ros/launch/common_args.launch.py +++ b/ros/launch/common_args.launch.py @@ -43,14 +43,9 @@ def generate_launch_description(): ), DeclareLaunchArgument( "visualize", - default_value="true", + default_value="false", description="", choices=["true", "false"], ), - DeclareLaunchArgument( - "bag_filenames", - default_value="", - description="Comma-separated list of file paths", - ), ] ) From 458694828f5aa6193d5bc7b3042fa40ea133523a Mon Sep 17 00:00:00 2001 From: tizianoGuadagnino Date: Wed, 16 Oct 2024 15:06:16 +0200 Subject: [PATCH 03/16] Add new threshold --- cpp/kinematic_icp/CMakeLists.txt | 1 + cpp/kinematic_icp/pipeline/CMakeLists.txt | 2 +- cpp/kinematic_icp/pipeline/KinematicICP.cpp | 6 +-- cpp/kinematic_icp/pipeline/KinematicICP.hpp | 12 +++-- .../threshold/AdaptiveThreshold.cpp | 51 ++++++++++++++++++ .../threshold/AdaptiveThreshold.hpp | 52 +++++++++++++++++++ cpp/kinematic_icp/threshold/CMakeLists.txt | 25 +++++++++ 7 files changed, 140 insertions(+), 9 deletions(-) create mode 100644 cpp/kinematic_icp/threshold/AdaptiveThreshold.cpp create mode 100644 cpp/kinematic_icp/threshold/AdaptiveThreshold.hpp create mode 100644 cpp/kinematic_icp/threshold/CMakeLists.txt diff --git a/cpp/kinematic_icp/CMakeLists.txt b/cpp/kinematic_icp/CMakeLists.txt index 718f731..a3d0eb8 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(threshold) add_subdirectory(registration) add_subdirectory(pipeline) diff --git a/cpp/kinematic_icp/pipeline/CMakeLists.txt b/cpp/kinematic_icp/pipeline/CMakeLists.txt index 25fa5b3..0411df1 100644 --- a/cpp/kinematic_icp/pipeline/CMakeLists.txt +++ b/cpp/kinematic_icp/pipeline/CMakeLists.txt @@ -22,5 +22,5 @@ # 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..dbe3cb6 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 = adaptive_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; // Update step: threshold, local map and the last pose - adaptive_threshold_.UpdateModelDeviation(model_deviation); + adaptive_threshold_.UpdateModelError(model_deviation); 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..6b40cb1 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 @@ -31,6 +32,7 @@ #include #include "kinematic_icp/registration/Registration.hpp" +#include "kinematic_icp/threshold/AdaptiveThreshold.hpp" namespace kinematic_icp::pipeline { @@ -44,9 +46,10 @@ class KinematicICP { explicit KinematicICP(const kiss_icp::pipeline::KISSConfig &config) : registration_( config.max_num_iterations, config.convergence_criterion, config.max_num_threads), + adaptive_threshold_(config.voxel_size / std::sqrt(config.max_points_per_voxel), + config.max_range), 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 +59,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); + adaptive_threshold_.Reset(); }; std::vector LocalMap() const { return local_map_.Pointcloud(); }; @@ -72,10 +74,10 @@ class KinematicICP { Sophus::SE3d last_pose_; // Kinematic module KinematicRegistration registration_; + AdaptiveThreshold adaptive_threshold_; // 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 new file mode 100644 index 0000000..0801929 --- /dev/null +++ b/cpp/kinematic_icp/threshold/AdaptiveThreshold.cpp @@ -0,0 +1,51 @@ +// 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 new file mode 100644 index 0000000..60d504a --- /dev/null +++ b/cpp/kinematic_icp/threshold/AdaptiveThreshold.hpp @@ -0,0 +1,52 @@ +// 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 new file mode 100644 index 0000000..01d5f57 --- /dev/null +++ b/cpp/kinematic_icp/threshold/CMakeLists.txt @@ -0,0 +1,25 @@ +# 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) From f2b394a7b0d7b40587c1e71b6d0ba7996af27a33 Mon Sep 17 00:00:00 2001 From: Tiziano Guadagnino <37455909+tizianoGuadagnino@users.noreply.github.com> Date: Wed, 16 Oct 2024 14:22:54 +0200 Subject: [PATCH 04/16] Update CMakeLists.txt (#7) Make *USE_SYSTEM_SOPHUS* consistent with the rest of the options. --- cpp/kinematic_icp/CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/cpp/kinematic_icp/CMakeLists.txt b/cpp/kinematic_icp/CMakeLists.txt index a3d0eb8..2bf3da4 100644 --- a/cpp/kinematic_icp/CMakeLists.txt +++ b/cpp/kinematic_icp/CMakeLists.txt @@ -26,7 +26,7 @@ project(kinematic_icp_cpp VERSION 0.0.1 LANGUAGES CXX) # Setup build options for the underlying kiss dependency option(USE_CCACHE "Build using Ccache if found on the path" ON) option(USE_SYSTEM_EIGEN3 "Use system pre-installed Eigen" ON) -option(USE_SYSTEM_SOPHUS "Use system pre-installed Sophus" OFF) +option(USE_SYSTEM_SOPHUS "Use system pre-installed Sophus" ON) option(USE_SYSTEM_TSL-ROBIN-MAP "Use system pre-installed tsl_robin" ON) option(USE_SYSTEM_TBB "Use system pre-installed oneAPI/tbb" ON) include(kiss_icp/kiss-icp.cmake) From 4fcbbbed294374eeebf01cea4434f485a3a33e27 Mon Sep 17 00:00:00 2001 From: Tiziano Guadagnino <37455909+tizianoGuadagnino@users.noreply.github.com> Date: Wed, 16 Oct 2024 14:23:07 +0200 Subject: [PATCH 05/16] 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. --- README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/README.md b/README.md index a6c1f64..7f9f688 100644 --- a/README.md +++ b/README.md @@ -46,7 +46,7 @@ Our system operates on ROS2, supporting **ROS Humble**, **Iron**, and **Jazzy**. # TF Requirements -Kinematic ICP can enhance existing odometry using a 3D LiDAR. However, there are specific requirements regarding motion and transformations due to the assumption that the robot operates on a unicycle kinematic model. Below are the key requirements: +Kinematic ICP can enhance existing odometry using a 3D LiDAR. However, there are specific requirements regarding motion and transformations since we use a kinematic motion model for the pose correction. Below are the key requirements: 1. **Planar Movement**: The robot is expected to move primarily on a planar surface. From e787a532de7dfcdec783be68fe81dcc705fe9bf3 Mon Sep 17 00:00:00 2001 From: Benedikt Mersch Date: Wed, 16 Oct 2024 15:35:18 +0200 Subject: [PATCH 06/16] Add python checks to pre-commit (#9) * Add black and isort * Format * use black for isort --- .pre-commit-config.yaml | 9 +++++++++ ros/launch/offline_node.launch.py | 3 +-- 2 files changed, 10 insertions(+), 2 deletions(-) diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index 37cfe77..304ae26 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -12,7 +12,16 @@ repos: rev: v14.0.0 hooks: - id: clang-format +- repo: https://github.com/psf/black + rev: 23.1.0 + hooks: + - id: black - repo: https://github.com/ahans/cmake-format-precommit rev: 8e52fb6506f169dddfaa87f88600d765fca48386 hooks: - id: cmake-format +- repo: https://github.com/pycqa/isort + rev: 5.12.0 + hooks: + - id: isort + args: ["--profile", "black", "--filter-files"] diff --git a/ros/launch/offline_node.launch.py b/ros/launch/offline_node.launch.py index 371d8bb..bd66551 100644 --- a/ros/launch/offline_node.launch.py +++ b/ros/launch/offline_node.launch.py @@ -4,14 +4,13 @@ import launch from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription -from launch.actions import OpaqueFunction +from launch.actions import DeclareLaunchArgument, OpaqueFunction from launch.conditions import IfCondition from launch.launch_description_sources import ( get_launch_description_from_python_launch_file, ) from launch.substitutions import LaunchConfiguration from launch_ros.actions import Node -from launch.actions import DeclareLaunchArgument def _generate_launch_description(context: launch.LaunchContext, *args, **kwargs): From 6c134d57bc23fe006b32e36cb96b3fbb5a46b009 Mon Sep 17 00:00:00 2001 From: Tiziano Guadagnino <37455909+tizianoGuadagnino@users.noreply.github.com> Date: Wed, 16 Oct 2024 15:43:52 +0200 Subject: [PATCH 07/16] Fixing ROS launch system (#8) * We didnt have proper flagging in the online node * Clean launch common args * Fix formatting python --- ros/launch/online_node.launch.py | 1 + 1 file changed, 1 insertion(+) diff --git a/ros/launch/online_node.launch.py b/ros/launch/online_node.launch.py index 1d3a198..383f741 100644 --- a/ros/launch/online_node.launch.py +++ b/ros/launch/online_node.launch.py @@ -2,6 +2,7 @@ from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription +from launch.conditions import IfCondition from launch.launch_description_sources import ( get_launch_description_from_python_launch_file, ) From 197a2bb96d538e86bab0c948906e9492062665fe Mon Sep 17 00:00:00 2001 From: tizianoGuadagnino Date: Wed, 23 Oct 2024 10:11:39 +0200 Subject: [PATCH 08/16] Tiny modification, but makes sense --- cpp/kinematic_icp/threshold/AdaptiveThreshold.cpp | 10 ++++++---- 1 file changed, 6 insertions(+), 4 deletions(-) diff --git a/cpp/kinematic_icp/threshold/AdaptiveThreshold.cpp b/cpp/kinematic_icp/threshold/AdaptiveThreshold.cpp index 0801929..4bc3767 100644 --- a/cpp/kinematic_icp/threshold/AdaptiveThreshold.cpp +++ b/cpp/kinematic_icp/threshold/AdaptiveThreshold.cpp @@ -42,10 +42,12 @@ AdaptiveThreshold::AdaptiveThreshold(const double map_discretization_error, cons 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_++; + const double &model_error = ModelError(current_deviation, max_range_); + if (model_error > map_discretization_error_) { + const double ¢ered_model_error = model_error - map_discretization_error_; + model_sse_ += centered_model_error * centered_model_error; + num_samples_++; + } } } // namespace kinematic_icp From 35b36b65db860a580b5f29903010726687ae8fe1 Mon Sep 17 00:00:00 2001 From: tizianoGuadagnino Date: Fri, 8 Nov 2024 13:59:00 +0100 Subject: [PATCH 09/16] Renaming and add configuration for the correspondence threshold --- cpp/kinematic_icp/CMakeLists.txt | 2 +- .../CMakeLists.txt | 2 +- .../CorrespondenceThreshold.cpp} | 28 ++++++++----- .../CorrespondenceThreshold.hpp} | 20 ++++++---- cpp/kinematic_icp/pipeline/KinematicICP.cpp | 4 +- cpp/kinematic_icp/pipeline/KinematicICP.hpp | 40 ++++++++++++++----- ros/config/kinematic_icp_ros.yaml | 13 +++--- .../server/LidarOdometryServer.cpp | 16 +++++--- 8 files changed, 84 insertions(+), 41 deletions(-) rename cpp/kinematic_icp/{threshold => correspondence_threshold}/CMakeLists.txt (95%) rename cpp/kinematic_icp/{threshold/AdaptiveThreshold.cpp => correspondence_threshold/CorrespondenceThreshold.cpp} (64%) rename cpp/kinematic_icp/{threshold/AdaptiveThreshold.hpp => correspondence_threshold/CorrespondenceThreshold.hpp} (68%) diff --git a/cpp/kinematic_icp/CMakeLists.txt b/cpp/kinematic_icp/CMakeLists.txt index 2bf3da4..93889b4 100644 --- a/cpp/kinematic_icp/CMakeLists.txt +++ b/cpp/kinematic_icp/CMakeLists.txt @@ -54,6 +54,6 @@ set(CMAKE_POSITION_INDEPENDENT_CODE ON) include(cmake/CompilerOptions.cmake) -add_subdirectory(threshold) +add_subdirectory(correspondence_threshold) add_subdirectory(registration) add_subdirectory(pipeline) diff --git a/cpp/kinematic_icp/threshold/CMakeLists.txt b/cpp/kinematic_icp/correspondence_threshold/CMakeLists.txt similarity index 95% rename from cpp/kinematic_icp/threshold/CMakeLists.txt rename to cpp/kinematic_icp/correspondence_threshold/CMakeLists.txt index 01d5f57..155d79b 100644 --- a/cpp/kinematic_icp/threshold/CMakeLists.txt +++ b/cpp/kinematic_icp/correspondence_threshold/CMakeLists.txt @@ -20,6 +20,6 @@ # 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) +add_library(kinematic_icp_threshold CorrespondenceThreshold.cpp) target_link_libraries(kinematic_icp_threshold Sophus::Sophus) set_global_target_properties(kinematic_icp_threshold) diff --git a/cpp/kinematic_icp/threshold/AdaptiveThreshold.cpp b/cpp/kinematic_icp/correspondence_threshold/CorrespondenceThreshold.cpp similarity index 64% rename from cpp/kinematic_icp/threshold/AdaptiveThreshold.cpp rename to cpp/kinematic_icp/correspondence_threshold/CorrespondenceThreshold.cpp index 4bc3767..73f3bf8 100644 --- a/cpp/kinematic_icp/threshold/AdaptiveThreshold.cpp +++ b/cpp/kinematic_icp/correspondence_threshold/CorrespondenceThreshold.cpp @@ -20,7 +20,7 @@ // 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 "CorrespondenceThreshold.hpp" #include #include @@ -35,19 +35,29 @@ double ModelError(const Sophus::SE3d &pose, const double max_range) { } // namespace namespace kinematic_icp { -AdaptiveThreshold::AdaptiveThreshold(const double map_discretization_error, const double max_range) +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), model_sse_(0.0), - num_samples_(1) {} + num_samples_(1e-8) {} + +double CorrespondenceThreshold::ComputeThreshold() const { + const double moder_error_variance = std::sqrt(model_sse_ / num_samples_); + const double adaptive_threshold = 3.0 * (map_discretization_error_ + moder_error_variance); + return use_adaptive_threshold_ ? adaptive_threshold : fixed_threshold_; +} + +void CorrespondenceThreshold::UpdateModelError(const Sophus::SE3d ¤t_deviation) { + if (!use_adaptive_threshold_) return; -void AdaptiveThreshold::UpdateModelError(const Sophus::SE3d ¤t_deviation) { const double &model_error = ModelError(current_deviation, max_range_); - if (model_error > map_discretization_error_) { - const double ¢ered_model_error = model_error - map_discretization_error_; - model_sse_ += centered_model_error * centered_model_error; - num_samples_++; - } + model_sse_ += model_error * model_error; + num_samples_ += 1.0; } } // namespace kinematic_icp diff --git a/cpp/kinematic_icp/threshold/AdaptiveThreshold.hpp b/cpp/kinematic_icp/correspondence_threshold/CorrespondenceThreshold.hpp similarity index 68% rename from cpp/kinematic_icp/threshold/AdaptiveThreshold.hpp rename to cpp/kinematic_icp/correspondence_threshold/CorrespondenceThreshold.hpp index 60d504a..1646d75 100644 --- a/cpp/kinematic_icp/threshold/AdaptiveThreshold.hpp +++ b/cpp/kinematic_icp/correspondence_threshold/CorrespondenceThreshold.hpp @@ -27,26 +27,30 @@ namespace kinematic_icp { -struct AdaptiveThreshold { - explicit AdaptiveThreshold(const double map_discretization_error, const double max_range); +struct CorrespondenceThreshold { + explicit CorrespondenceThreshold(const double map_discretization_error, + const double max_range, + const bool use_adaptive_threshold, + const double fixed_threshold); void UpdateModelError(const Sophus::SE3d ¤t_deviation); - inline double ComputeThreshold() const { - return map_discretization_error_ + 3.0 * std::sqrt(model_sse_ / num_samples_); - } + double ComputeThreshold() const; inline void Reset() { model_sse_ = 0.0; - num_samples_ = 1; + num_samples_ = 1e-8; } // configurable parameters - double map_discretization_error_; + 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 ccomputation double model_sse_; - int num_samples_; + double num_samples_; }; } // namespace kinematic_icp diff --git a/cpp/kinematic_icp/pipeline/KinematicICP.cpp b/cpp/kinematic_icp/pipeline/KinematicICP.cpp index dbe3cb6..9f05339 100644 --- a/cpp/kinematic_icp/pipeline/KinematicICP.cpp +++ b/cpp/kinematic_icp/pipeline/KinematicICP.cpp @@ -66,7 +66,7 @@ KinematicICP::Vector3dVectorTuple KinematicICP::RegisterFrame( const auto &[source, frame_downsample] = Voxelize(cropped_frame, config_.voxel_size); // Get adaptive_threshold - const double &tau = adaptive_threshold_.ComputeThreshold(); + const double &tau = correspondence_threshold_.ComputeThreshold(); // Run ICP const auto new_pose = registration_.ComputeRobotMotion(source, // frame @@ -79,7 +79,7 @@ KinematicICP::Vector3dVectorTuple KinematicICP::RegisterFrame( const auto model_deviation = (last_pose_ * relative_odometry).inverse() * new_pose; // Update step: threshold, local map and the last pose - adaptive_threshold_.UpdateModelError(model_deviation); + correspondence_threshold_.UpdateModelError(model_deviation); 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 6b40cb1..5d1602a 100644 --- a/cpp/kinematic_icp/pipeline/KinematicICP.hpp +++ b/cpp/kinematic_icp/pipeline/KinematicICP.hpp @@ -31,23 +31,45 @@ #include #include +#include "kinematic_icp/correspondence_threshold/CorrespondenceThreshold.hpp" #include "kinematic_icp/registration/Registration.hpp" -#include "kinematic_icp/threshold/AdaptiveThreshold.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; + + // Motion compensation + bool deskew = false; +}; class KinematicICP { public: using Vector3dVector = std::vector; using Vector3dVectorTuple = std::tuple; - explicit KinematicICP(const kiss_icp::pipeline::KISSConfig &config) + explicit KinematicICP(const Config &config) : registration_( config.max_num_iterations, config.convergence_criterion, config.max_num_threads), - adaptive_threshold_(config.voxel_size / std::sqrt(config.max_points_per_voxel), - config.max_range), + 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) {} @@ -59,7 +81,7 @@ class KinematicICP { inline void SetPose(const Sophus::SE3d &pose) { last_pose_ = pose; local_map_.Clear(); - adaptive_threshold_.Reset(); + correspondence_threshold_.Reset(); }; std::vector LocalMap() const { return local_map_.Pointcloud(); }; @@ -70,13 +92,13 @@ 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_; - AdaptiveThreshold adaptive_threshold_; + CorrespondenceThreshold correspondence_threshold_; + Config config_; // KISS-ICP pipeline modules - kiss_icp::pipeline::KISSConfig config_; kiss_icp::VoxelHashMap local_map_; }; diff --git a/ros/config/kinematic_icp_ros.yaml b/ros/config/kinematic_icp_ros.yaml index 6d4e0b6..1ef703d 100644 --- a/ros/config/kinematic_icp_ros.yaml +++ b/ros/config/kinematic_icp_ros.yaml @@ -3,21 +3,22 @@ # 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 + # Motion Compensation + deskew: true + # Covariance diagonal values orientation_covariance: 0.1 position_covariance: 0.1 diff --git a/ros/src/kinematic_icp_ros/server/LidarOdometryServer.cpp b/ros/src/kinematic_icp_ros/server/LidarOdometryServer.cpp index b1c9bb9..3e578b9 100644 --- a/ros/src/kinematic_icp_ros/server/LidarOdometryServer.cpp +++ b/ros/src/kinematic_icp_ros/server/LidarOdometryServer.cpp @@ -70,21 +70,27 @@ 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); + // 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"); From 8200e362640a02e00e1a3522a7a65ca53374577a Mon Sep 17 00:00:00 2001 From: tizianoGuadagnino Date: Fri, 8 Nov 2024 15:57:04 +0100 Subject: [PATCH 10/16] Fix pipeline --- cpp/kinematic_icp/pipeline/CMakeLists.txt | 3 ++- ros/launch/online_node.launch.py | 1 - 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/cpp/kinematic_icp/pipeline/CMakeLists.txt b/cpp/kinematic_icp/pipeline/CMakeLists.txt index 0411df1..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 kinematic_icp_threshold 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/ros/launch/online_node.launch.py b/ros/launch/online_node.launch.py index 383f741..603e973 100644 --- a/ros/launch/online_node.launch.py +++ b/ros/launch/online_node.launch.py @@ -6,7 +6,6 @@ from launch.launch_description_sources import ( get_launch_description_from_python_launch_file, ) -from launch.conditions import IfCondition from launch.substitutions import LaunchConfiguration, PythonExpression from launch_ros.actions import Node from launch_ros.substitutions.find_package import get_package_share_directory From 0fa8ecf8049a24292b3c70bb79b8792085a9f3a2 Mon Sep 17 00:00:00 2001 From: tizianoGuadagnino Date: Tue, 12 Nov 2024 12:07:49 +0100 Subject: [PATCH 11/16] Consistency in the CMakeLists --- cpp/kinematic_icp/correspondence_threshold/CMakeLists.txt | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/cpp/kinematic_icp/correspondence_threshold/CMakeLists.txt b/cpp/kinematic_icp/correspondence_threshold/CMakeLists.txt index 155d79b..df30730 100644 --- a/cpp/kinematic_icp/correspondence_threshold/CMakeLists.txt +++ b/cpp/kinematic_icp/correspondence_threshold/CMakeLists.txt @@ -20,6 +20,7 @@ # 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 CorrespondenceThreshold.cpp) -target_link_libraries(kinematic_icp_threshold Sophus::Sophus) +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) From 947cae335b287c5da6858f6de6901cf8dbcc88d0 Mon Sep 17 00:00:00 2001 From: tizianoGuadagnino Date: Tue, 12 Nov 2024 12:10:40 +0100 Subject: [PATCH 12/16] Remove ternary operator and make the two functions consistent --- .../correspondence_threshold/CorrespondenceThreshold.cpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/cpp/kinematic_icp/correspondence_threshold/CorrespondenceThreshold.cpp b/cpp/kinematic_icp/correspondence_threshold/CorrespondenceThreshold.cpp index 73f3bf8..a1fa59c 100644 --- a/cpp/kinematic_icp/correspondence_threshold/CorrespondenceThreshold.cpp +++ b/cpp/kinematic_icp/correspondence_threshold/CorrespondenceThreshold.cpp @@ -47,9 +47,11 @@ CorrespondenceThreshold::CorrespondenceThreshold(const double map_discretization num_samples_(1e-8) {} double CorrespondenceThreshold::ComputeThreshold() const { + if (!use_adaptive_threshold_) return fixed_threshold_; + const double moder_error_variance = std::sqrt(model_sse_ / num_samples_); const double adaptive_threshold = 3.0 * (map_discretization_error_ + moder_error_variance); - return use_adaptive_threshold_ ? adaptive_threshold : fixed_threshold_; + return adaptive_threshold; } void CorrespondenceThreshold::UpdateModelError(const Sophus::SE3d ¤t_deviation) { From c4e34e347c5325ce9f6f9fbcd2994cec2f95953e Mon Sep 17 00:00:00 2001 From: Tiziano Guadagnino <37455909+tizianoGuadagnino@users.noreply.github.com> Date: Tue, 12 Nov 2024 15:18:49 +0100 Subject: [PATCH 13/16] Update cpp/kinematic_icp/correspondence_threshold/CorrespondenceThreshold.hpp Co-authored-by: Benedikt Mersch --- .../correspondence_threshold/CorrespondenceThreshold.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/cpp/kinematic_icp/correspondence_threshold/CorrespondenceThreshold.hpp b/cpp/kinematic_icp/correspondence_threshold/CorrespondenceThreshold.hpp index 1646d75..e2b5c07 100644 --- a/cpp/kinematic_icp/correspondence_threshold/CorrespondenceThreshold.hpp +++ b/cpp/kinematic_icp/correspondence_threshold/CorrespondenceThreshold.hpp @@ -49,7 +49,7 @@ struct CorrespondenceThreshold { bool use_adaptive_threshold_; double fixed_threshold_; // <-- Ignored if use_adaptive_threshold_ = true - // Local cache for ccomputation + // Local cache for computation double model_sse_; double num_samples_; }; From 1a5a94c99c9d8c9d1ed4d2fdf9e137c00dc2baee Mon Sep 17 00:00:00 2001 From: tizianoGuadagnino Date: Tue, 12 Nov 2024 15:27:54 +0100 Subject: [PATCH 14/16] Renaming according to Ben's review --- .../CorrespondenceThreshold.cpp | 15 ++++++++------- .../CorrespondenceThreshold.hpp | 6 +++--- cpp/kinematic_icp/pipeline/KinematicICP.cpp | 4 ++-- 3 files changed, 13 insertions(+), 12 deletions(-) diff --git a/cpp/kinematic_icp/correspondence_threshold/CorrespondenceThreshold.cpp b/cpp/kinematic_icp/correspondence_threshold/CorrespondenceThreshold.cpp index a1fa59c..1baf3d7 100644 --- a/cpp/kinematic_icp/correspondence_threshold/CorrespondenceThreshold.cpp +++ b/cpp/kinematic_icp/correspondence_threshold/CorrespondenceThreshold.cpp @@ -26,7 +26,7 @@ #include namespace { -double ModelError(const Sophus::SE3d &pose, const double max_range) { +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(); @@ -43,22 +43,23 @@ CorrespondenceThreshold::CorrespondenceThreshold(const double map_discretization max_range_(max_range), use_adaptive_threshold_(use_adaptive_threshold), fixed_threshold_(fixed_threshold), - model_sse_(0.0), + odom_sse_(0.0), num_samples_(1e-8) {} double CorrespondenceThreshold::ComputeThreshold() const { if (!use_adaptive_threshold_) return fixed_threshold_; - const double moder_error_variance = std::sqrt(model_sse_ / num_samples_); - const double adaptive_threshold = 3.0 * (map_discretization_error_ + moder_error_variance); + 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::UpdateModelError(const Sophus::SE3d ¤t_deviation) { +void CorrespondenceThreshold::UpdateOdometryError(const Sophus::SE3d &odometry_error) { if (!use_adaptive_threshold_) return; - const double &model_error = ModelError(current_deviation, max_range_); - model_sse_ += model_error * model_error; + 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; } diff --git a/cpp/kinematic_icp/correspondence_threshold/CorrespondenceThreshold.hpp b/cpp/kinematic_icp/correspondence_threshold/CorrespondenceThreshold.hpp index e2b5c07..d699ae8 100644 --- a/cpp/kinematic_icp/correspondence_threshold/CorrespondenceThreshold.hpp +++ b/cpp/kinematic_icp/correspondence_threshold/CorrespondenceThreshold.hpp @@ -33,12 +33,12 @@ struct CorrespondenceThreshold { const bool use_adaptive_threshold, const double fixed_threshold); - void UpdateModelError(const Sophus::SE3d ¤t_deviation); + void UpdateOdometryError(const Sophus::SE3d &odometry_error); double ComputeThreshold() const; inline void Reset() { - model_sse_ = 0.0; + odom_sse_ = 0.0; num_samples_ = 1e-8; } @@ -50,7 +50,7 @@ struct CorrespondenceThreshold { double fixed_threshold_; // <-- Ignored if use_adaptive_threshold_ = true // Local cache for computation - double model_sse_; + double odom_sse_; double num_samples_; }; } // namespace kinematic_icp diff --git a/cpp/kinematic_icp/pipeline/KinematicICP.cpp b/cpp/kinematic_icp/pipeline/KinematicICP.cpp index 9f05339..68e651b 100644 --- a/cpp/kinematic_icp/pipeline/KinematicICP.cpp +++ b/cpp/kinematic_icp/pipeline/KinematicICP.cpp @@ -76,10 +76,10 @@ KinematicICP::Vector3dVectorTuple KinematicICP::RegisterFrame( 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 - correspondence_threshold_.UpdateModelError(model_deviation); + correspondence_threshold_.UpdateOdometryError(odometry_error); local_map_.Update(frame_downsample, new_pose); last_pose_ = new_pose; From 83ec040bfc958ae007f2d84fbcebddaa8291ac07 Mon Sep 17 00:00:00 2001 From: tizianoGuadagnino Date: Tue, 12 Nov 2024 15:32:55 +0100 Subject: [PATCH 15/16] Why a ref --- cpp/kinematic_icp/pipeline/KinematicICP.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/cpp/kinematic_icp/pipeline/KinematicICP.cpp b/cpp/kinematic_icp/pipeline/KinematicICP.cpp index 68e651b..aece4c5 100644 --- a/cpp/kinematic_icp/pipeline/KinematicICP.cpp +++ b/cpp/kinematic_icp/pipeline/KinematicICP.cpp @@ -66,7 +66,7 @@ KinematicICP::Vector3dVectorTuple KinematicICP::RegisterFrame( const auto &[source, frame_downsample] = Voxelize(cropped_frame, config_.voxel_size); // Get adaptive_threshold - const double &tau = correspondence_threshold_.ComputeThreshold(); + const double tau = correspondence_threshold_.ComputeThreshold(); // Run ICP const auto new_pose = registration_.ComputeRobotMotion(source, // frame From 26a34506d8ea4a5a0ca672a5801dd3bba89a981b Mon Sep 17 00:00:00 2001 From: Tiziano Guadagnino <37455909+tizianoGuadagnino@users.noreply.github.com> Date: Tue, 19 Nov 2024 14:15:37 +0100 Subject: [PATCH 16/16] 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 --- cpp/kinematic_icp/pipeline/KinematicICP.hpp | 9 ++++++-- .../registration/Registration.cpp | 21 +++++++++++++------ .../registration/Registration.hpp | 10 ++++++--- ros/config/kinematic_icp_ros.yaml | 2 ++ .../server/LidarOdometryServer.cpp | 4 ++++ 5 files changed, 35 insertions(+), 11 deletions(-) diff --git a/cpp/kinematic_icp/pipeline/KinematicICP.hpp b/cpp/kinematic_icp/pipeline/KinematicICP.hpp index 5d1602a..c17a62e 100644 --- a/cpp/kinematic_icp/pipeline/KinematicICP.hpp +++ b/cpp/kinematic_icp/pipeline/KinematicICP.hpp @@ -53,6 +53,8 @@ struct Config { 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; @@ -64,8 +66,11 @@ class KinematicICP { using Vector3dVectorTuple = std::tuple; explicit KinematicICP(const Config &config) - : registration_( - config.max_num_iterations, config.convergence_criterion, config.max_num_threads), + : 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, 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 1ef703d..b8d5ad3 100644 --- a/ros/config/kinematic_icp_ros.yaml +++ b/ros/config/kinematic_icp_ros.yaml @@ -15,6 +15,8 @@ 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 diff --git a/ros/src/kinematic_icp_ros/server/LidarOdometryServer.cpp b/ros/src/kinematic_icp_ros/server/LidarOdometryServer.cpp index 3e578b9..ab39990 100644 --- a/ros/src/kinematic_icp_ros/server/LidarOdometryServer.cpp +++ b/ros/src/kinematic_icp_ros/server/LidarOdometryServer.cpp @@ -89,6 +89,10 @@ LidarOdometryServer::LidarOdometryServer(rclcpp::Node::SharedPtr node) : node_(n 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) {