From 27d76e35ae13a58a0d28981046703a831433ac81 Mon Sep 17 00:00:00 2001 From: Benedikt Mersch Date: Tue, 19 Nov 2024 11:18:36 +0100 Subject: [PATCH 1/2] Revise timestamping (#18) * Query from last stamp in case of frame drops * Rename * WIP * Fix conversions * Fix build * Make CI happy * Add TimeStampHandler module to handle this madness * Minor renaming * Try to take care of absolute timestamping of points + scan stamped at the end * Comments * Should fix all cases that came to my mind at this time * Some comments to make stuff more clear * Remove useless case, we need to fix deskewing * Fix condition for clarity --------- Co-authored-by: tizianoGuadagnino --- ros/CMakeLists.txt | 4 +- .../server/LidarOdometryServer.hpp | 4 +- .../kinematic_icp_ros/utils/RosUtils.hpp | 9 -- .../utils/TimeStampHandler.hpp | 52 +++++++ .../kinematic_icp_ros/nodes/offline_node.cpp | 2 +- .../server/LidarOdometryServer.cpp | 36 ++--- ros/src/kinematic_icp_ros/utils/RosUtils.cpp | 76 ---------- .../utils/TimeStampHandler.cpp | 140 ++++++++++++++++++ 8 files changed, 209 insertions(+), 114 deletions(-) create mode 100644 ros/include/kinematic_icp_ros/utils/TimeStampHandler.hpp create mode 100644 ros/src/kinematic_icp_ros/utils/TimeStampHandler.cpp diff --git a/ros/CMakeLists.txt b/ros/CMakeLists.txt index d4e2678..932dc5c 100644 --- a/ros/CMakeLists.txt +++ b/ros/CMakeLists.txt @@ -58,8 +58,8 @@ set(dependencies add_library( kinematic_icp_ros SHARED src/kinematic_icp_ros/server/LidarOdometryServer.cpp src/kinematic_icp_ros/utils/RosUtils.cpp - src/kinematic_icp_ros/utils/RosbagUtils.cpp src/kinematic_icp_ros/nodes/online_node.cpp -)# Adding it here for composition + src/kinematic_icp_ros/utils/RosbagUtils.cpp src/kinematic_icp_ros/utils/TimeStampHandler.cpp + src/kinematic_icp_ros/nodes/online_node.cpp) # Adding it here for composition target_compile_features(kinematic_icp_ros PUBLIC cxx_std_17) target_include_directories(kinematic_icp_ros PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}/include) target_link_libraries(kinematic_icp_ros kinematic_icp_pipeline "${cpp_typesupport_target}") diff --git a/ros/include/kinematic_icp_ros/server/LidarOdometryServer.hpp b/ros/include/kinematic_icp_ros/server/LidarOdometryServer.hpp index 5bbdbf9..2b709f1 100644 --- a/ros/include/kinematic_icp_ros/server/LidarOdometryServer.hpp +++ b/ros/include/kinematic_icp_ros/server/LidarOdometryServer.hpp @@ -23,6 +23,7 @@ #pragma once #include "kinematic_icp/pipeline/KinematicICP.hpp" +#include "kinematic_icp_ros/utils/TimeStampHandler.hpp" // ROS 2 C #include @@ -30,7 +31,6 @@ #include // ROS 2 C++ -#include #include #include #include @@ -57,7 +57,7 @@ class LidarOdometryServer { /// Register new frame void RegisterFrame(const sensor_msgs::msg::PointCloud2::ConstSharedPtr &msg); std::unique_ptr kinematic_icp_; - builtin_interfaces::msg::Time current_stamp_; + utils::TimeStampHandler timestamps_handler_; private: /// Stream the estimated pose to ROS diff --git a/ros/include/kinematic_icp_ros/utils/RosUtils.hpp b/ros/include/kinematic_icp_ros/utils/RosUtils.hpp index 5001fba..5610d00 100644 --- a/ros/include/kinematic_icp_ros/utils/RosUtils.hpp +++ b/ros/include/kinematic_icp_ros/utils/RosUtils.hpp @@ -129,15 +129,6 @@ inline Sophus::SE3d LookupDeltaTransform(const std::string &target_frame, } } -std::optional GetTimestampField(const PointCloud2::ConstSharedPtr msg); - -std::vector NormalizeTimestamps(const std::vector ×tamps); - -auto ExtractTimestampsFromMsg(const PointCloud2::ConstSharedPtr msg, - const PointField ×tamp_field); - -std::vector GetTimestamps(const PointCloud2::ConstSharedPtr msg); - std::vector PointCloud2ToEigen(const PointCloud2::ConstSharedPtr msg, const Sophus::SE3d &T = {}); diff --git a/ros/include/kinematic_icp_ros/utils/TimeStampHandler.hpp b/ros/include/kinematic_icp_ros/utils/TimeStampHandler.hpp new file mode 100644 index 0000000..7ec6a1e --- /dev/null +++ b/ros/include/kinematic_icp_ros/utils/TimeStampHandler.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 +#include +#include +#include +#include +#include + +namespace kinematic_icp_ros::utils { + +using StampType = builtin_interfaces::msg::Time; + +struct TimeStampHandler { + std::tuple> ProcessTimestamps( + const sensor_msgs::msg::PointCloud2::ConstSharedPtr msg); + // From double to ROS TimeStamp + inline StampType toStamp(const double &time_in_seconds) const { + return rclcpp::Time(tf2::durationFromSec(time_in_seconds).count()); + }; + // From ROS TimeStamp to double + inline double toTime(const StampType &stamp) const { + return rclcpp::Time(stamp).nanoseconds() * 1e-9; + }; + + StampType last_processed_stamp_; +}; + +} // namespace kinematic_icp_ros::utils diff --git a/ros/src/kinematic_icp_ros/nodes/offline_node.cpp b/ros/src/kinematic_icp_ros/nodes/offline_node.cpp index 898ac00..a7c9577 100644 --- a/ros/src/kinematic_icp_ros/nodes/offline_node.cpp +++ b/ros/src/kinematic_icp_ros/nodes/offline_node.cpp @@ -113,7 +113,7 @@ void OfflineNode::Run() { while (rclcpp::ok() && bag_multiplexer_.IsMessageAvailable()) { const auto msg = GetNextMsg(); odometry_server_->RegisterFrame(msg); - const auto &stamp = odometry_server_->current_stamp_; + const auto &stamp = odometry_server_->timestamps_handler_.last_processed_stamp_; poses_with_timestamps_.emplace_back(stamp.sec + stamp.nanosec * 1e-9, odometry_server_->kinematic_icp_->pose()); bar.tick(); diff --git a/ros/src/kinematic_icp_ros/server/LidarOdometryServer.cpp b/ros/src/kinematic_icp_ros/server/LidarOdometryServer.cpp index b1c9bb9..8bffc9a 100644 --- a/ros/src/kinematic_icp_ros/server/LidarOdometryServer.cpp +++ b/ros/src/kinematic_icp_ros/server/LidarOdometryServer.cpp @@ -169,7 +169,7 @@ void LidarOdometryServer::InitializePoseAndExtrinsic( // Initialization finished RCLCPP_INFO(node_->get_logger(), "KISS-ICP ROS 2 odometry node initialized"); - current_stamp_ = msg->header.stamp; + timestamps_handler_.last_processed_stamp_ = msg->header.stamp; initialize_odom_node = true; } @@ -179,39 +179,27 @@ void LidarOdometryServer::RegisterFrame(const sensor_msgs::msg::PointCloud2::Con } // Buffer the last state This will be used for computing the veloicty - const auto last_stamp = current_stamp_; const auto last_pose = kinematic_icp_->pose(); - // Extract timestamps - const auto timestamps = GetTimestamps(msg); - const auto &[min_it, max_it] = std::minmax_element(timestamps.cbegin(), timestamps.cend()); - // From double to ROS TimeStamp - auto toStamp = [](const double &time) -> builtin_interfaces::msg::Time { - return rclcpp::Time(tf2::durationFromSec(time).count()); - }; - // Update what is the current stamp of this iteration - const auto begin_scan_stamp = min_it != timestamps.cend() ? toStamp(*min_it) : last_stamp; - const auto end_scan_stamp = max_it != timestamps.cend() ? toStamp(*max_it) : msg->header.stamp; - current_stamp_ = end_scan_stamp; + const auto &[begin_odom_query, end_odom_query, timestamps] = + timestamps_handler_.ProcessTimestamps(msg); + // Get the initial guess from the wheel odometry const auto delta = - LookupDeltaTransform(base_frame_, begin_scan_stamp, base_frame_, end_scan_stamp, + LookupDeltaTransform(base_frame_, begin_odom_query, base_frame_, end_odom_query, wheel_odom_frame_, tf_timeout_, tf2_buffer_); // Run kinematic ICP if (delta.log().norm() > 1e-3) { const auto &extrinsic = sensor_to_base_footprint_; const auto points = PointCloud2ToEigen(msg, {}); - const auto normalized_timestamps = NormalizeTimestamps(timestamps); const auto &[frame, kpoints] = - kinematic_icp_->RegisterFrame(points, normalized_timestamps, extrinsic, delta); + kinematic_icp_->RegisterFrame(points, timestamps, extrinsic, delta); PublishClouds(frame, kpoints); } - auto toTime = [](const builtin_interfaces::msg::Time &stamp) -> double { - return rclcpp::Time(stamp).nanoseconds() * 1e-9; - }; // Compute velocities, use the elapsed time between the current msg and the last received - const double elapsed_time = toTime(current_stamp_) - toTime(last_stamp); + const double elapsed_time = + timestamps_handler_.toTime(end_odom_query) - timestamps_handler_.toTime(begin_odom_query); const Sophus::SE3d::Tangent delta_twist = (last_pose.inverse() * kinematic_icp_->pose()).log(); const Sophus::SE3d::Tangent velocity = delta_twist / elapsed_time; @@ -227,7 +215,7 @@ void LidarOdometryServer::PublishOdometryMsg(const Sophus::SE3d &pose, if (invert_odom_tf_) return tf2::sophusToTransform(pose.inverse()); return tf2::sophusToTransform(pose); }(); - tf_msg_.header.stamp = current_stamp_; + tf_msg_.header.stamp = timestamps_handler_.last_processed_stamp_; tf_broadcaster_->sendTransform(tf_msg_); } @@ -235,7 +223,7 @@ void LidarOdometryServer::PublishOdometryMsg(const Sophus::SE3d &pose, odom_msg_.pose.pose = tf2::sophusToPose(pose); odom_msg_.twist.twist.linear.x = velocity[0]; odom_msg_.twist.twist.angular.z = velocity[5]; - odom_msg_.header.stamp = current_stamp_; + odom_msg_.header.stamp = timestamps_handler_.last_processed_stamp_; odom_publisher_->publish(odom_msg_); } @@ -244,12 +232,12 @@ void LidarOdometryServer::PublishClouds(const std::vector frame // For re-publishing the input frame and keypoints, we do it in the LiDAR coordinate frames std_msgs::msg::Header lidar_header; lidar_header.frame_id = base_frame_; - lidar_header.stamp = current_stamp_; + lidar_header.stamp = timestamps_handler_.last_processed_stamp_; // The internal map representation is in the lidar_odom_frame_ std_msgs::msg::Header map_header; map_header.frame_id = lidar_odom_frame_; - map_header.stamp = current_stamp_; + map_header.stamp = timestamps_handler_.last_processed_stamp_; // Check for subscriptions before publishing to avoid unnecesary CPU usage if (frame_publisher_->get_subscription_count() > 0) { diff --git a/ros/src/kinematic_icp_ros/utils/RosUtils.cpp b/ros/src/kinematic_icp_ros/utils/RosUtils.cpp index 3f840e6..a952e8f 100644 --- a/ros/src/kinematic_icp_ros/utils/RosUtils.cpp +++ b/ros/src/kinematic_icp_ros/utils/RosUtils.cpp @@ -27,82 +27,6 @@ namespace kinematic_icp_ros::utils { -std::optional GetTimestampField(const PointCloud2::ConstSharedPtr msg) { - PointField timestamp_field; - for (const auto &field : msg->fields) { - if ((field.name == "t" || field.name == "timestamp" || field.name == "time")) { - timestamp_field = field; - } - } - if (timestamp_field.count) return timestamp_field; - RCLCPP_WARN_ONCE(rclcpp::get_logger("kinematic_icp_ros"), - "Field 't', 'timestamp', or 'time' does not exist. " - "Disabling scan deskewing"); - return {}; -} - -// Normalize timestamps from 0.0 to 1.0 -std::vector NormalizeTimestamps(const std::vector ×tamps) { - const auto [min_it, max_it] = std::minmax_element(timestamps.cbegin(), timestamps.cend()); - const double min_timestamp = min_it != timestamps.cend() ? *min_it : 0.0; - const double max_timestamp = max_it != timestamps.cend() ? *max_it : 1.0; - - std::vector timestamps_normalized(timestamps.size()); - std::transform(timestamps.cbegin(), timestamps.cend(), timestamps_normalized.begin(), - [&](const auto ×tamp) { - return (timestamp - min_timestamp) / (max_timestamp - min_timestamp); - }); - return timestamps_normalized; -} - -auto ExtractTimestampsFromMsg(const PointCloud2::ConstSharedPtr msg, - const PointField ×tamp_field) { - auto number_of_digits_decimal_part = [](const auto &stamp) { - const uint64_t number_of_seconds = static_cast(std::round(stamp)); - return number_of_seconds > 0 ? std::floor(std::log10(number_of_seconds) + 1) : 1; - }; - auto extract_timestamps = - [&](sensor_msgs::PointCloud2ConstIterator &&it) -> std::vector { - const size_t n_points = msg->height * msg->width; - std::vector timestamps; - timestamps.reserve(n_points); - for (size_t i = 0; i < n_points; ++i, ++it) { - double stampd = static_cast(*it); - // If the number of digits is greater than 10 (which is the maximum number of digits - // that can be represented with a 32 bits integer), the stamp is in nanoseconds instead - // of seconds, perform conversion - if (number_of_digits_decimal_part(stampd) > 10) { - stampd *= 1e-9; - } - timestamps.emplace_back(stampd); - } - return timestamps; - }; - - // According to the type of the timestamp == type, return a PointCloud2ConstIterator - using sensor_msgs::PointCloud2ConstIterator; - if (timestamp_field.datatype == PointField::UINT32) { - return extract_timestamps(PointCloud2ConstIterator(*msg, timestamp_field.name)); - } else if (timestamp_field.datatype == PointField::FLOAT32) { - return extract_timestamps(PointCloud2ConstIterator(*msg, timestamp_field.name)); - } else if (timestamp_field.datatype == PointField::FLOAT64) { - return extract_timestamps(PointCloud2ConstIterator(*msg, timestamp_field.name)); - } - - // timestamp type not supported, please open an issue :) - throw std::runtime_error("timestamp field type not supported"); -} - -std::vector GetTimestamps(const PointCloud2::ConstSharedPtr msg) { - auto timestamp_field = GetTimestampField(msg); - if (!timestamp_field.has_value()) return {}; - - // Extract timestamps from cloud_msg - std::vector timestamps = ExtractTimestampsFromMsg(msg, timestamp_field.value()); - - return timestamps; -} - std::vector PointCloud2ToEigen(const PointCloud2::ConstSharedPtr msg, const Sophus::SE3d &T) { std::vector points; diff --git a/ros/src/kinematic_icp_ros/utils/TimeStampHandler.cpp b/ros/src/kinematic_icp_ros/utils/TimeStampHandler.cpp new file mode 100644 index 0000000..d8ce8bb --- /dev/null +++ b/ros/src/kinematic_icp_ros/utils/TimeStampHandler.cpp @@ -0,0 +1,140 @@ +// 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 "kinematic_icp_ros/utils/TimeStampHandler.hpp" + +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +namespace { + +using PointCloud2 = sensor_msgs::msg::PointCloud2; +using PointField = sensor_msgs::msg::PointField; +using Header = std_msgs::msg::Header; + +std::optional GetTimestampField(const PointCloud2::ConstSharedPtr msg) { + PointField timestamp_field; + for (const auto &field : msg->fields) { + if ((field.name == "t" || field.name == "timestamp" || field.name == "time")) { + timestamp_field = field; + } + } + if (timestamp_field.count) return timestamp_field; + RCLCPP_WARN_ONCE(rclcpp::get_logger("kinematic_icp_ros"), + "Field 't', 'timestamp', or 'time' does not exist. " + "Disabling scan deskewing"); + return {}; +} + +auto ExtractTimestampsFromMsg(const PointCloud2::ConstSharedPtr msg, + const PointField ×tamp_field) { + auto number_of_digits_decimal_part = [](const auto &stamp) { + const uint64_t number_of_seconds = static_cast(std::round(stamp)); + return number_of_seconds > 0 ? std::floor(std::log10(number_of_seconds) + 1) : 1; + }; + auto extract_timestamps = + [&](sensor_msgs::PointCloud2ConstIterator &&it) -> std::vector { + const size_t n_points = msg->height * msg->width; + std::vector timestamps; + timestamps.reserve(n_points); + for (size_t i = 0; i < n_points; ++i, ++it) { + double stampd = static_cast(*it); + // If the number of digits is greater than 10 (which is the maximum number of digits + // that can be represented with a 32 bits integer), the stamp is in nanoseconds instead + // of seconds, perform conversion + if (number_of_digits_decimal_part(stampd) > 10) { + stampd *= 1e-9; + } + timestamps.emplace_back(stampd); + } + return timestamps; + }; + + // According to the type of the timestamp == type, return a PointCloud2ConstIterator + using sensor_msgs::PointCloud2ConstIterator; + if (timestamp_field.datatype == PointField::UINT32) { + return extract_timestamps(PointCloud2ConstIterator(*msg, timestamp_field.name)); + } else if (timestamp_field.datatype == PointField::FLOAT32) { + return extract_timestamps(PointCloud2ConstIterator(*msg, timestamp_field.name)); + } else if (timestamp_field.datatype == PointField::FLOAT64) { + return extract_timestamps(PointCloud2ConstIterator(*msg, timestamp_field.name)); + } + + // timestamp type not supported, please open an issue :) + throw std::runtime_error("timestamp field type not supported"); +} + +std::vector GetTimestamps(const PointCloud2::ConstSharedPtr msg) { + auto timestamp_field = GetTimestampField(msg); + if (!timestamp_field.has_value()) return {}; + + // Extract timestamps from cloud_msg + std::vector timestamps = ExtractTimestampsFromMsg(msg, timestamp_field.value()); + + return timestamps; +} +} // namespace + +namespace kinematic_icp_ros::utils { + +std::tuple> TimeStampHandler::ProcessTimestamps( + const sensor_msgs::msg::PointCloud2::ConstSharedPtr msg) { + std::vector timestamps = GetTimestamps(msg); + const auto &[min_it, max_it] = std::minmax_element(timestamps.cbegin(), timestamps.cend()); + const StampType msg_stamp = msg->header.stamp; + const StampType begin_stamp = last_processed_stamp_; + StampType end_stamp = msg_stamp; + if (max_it != timestamps.cend()) { + const double &max_stamp_in_seconds = *max_it; + const double &min_stamp_in_seconds = *min_it; + const double msg_stamp_in_seconds = this->toTime(msg_stamp); + + // Check if stamping happens and the beginning or the end of scan + const bool is_stamped_at_the_beginning = + std::abs(msg_stamp_in_seconds - max_stamp_in_seconds) > 1e-8; + if (is_stamped_at_the_beginning) { + // begin-stamping -> add scan duration to the stamp + const auto scan_duration = + tf2::durationFromSec(max_stamp_in_seconds - min_stamp_in_seconds); + end_stamp = StampType(rclcpp::Time(end_stamp) + scan_duration); + } + + // Normalize timestamps + std::transform(timestamps.cbegin(), timestamps.cend(), timestamps.begin(), + [&](const auto ×tamp) { + return (timestamp - min_stamp_in_seconds) / + (max_stamp_in_seconds - min_stamp_in_seconds); + }); + } + last_processed_stamp_ = end_stamp; + return std::make_tuple(begin_stamp, end_stamp, timestamps); +} + +} // namespace kinematic_icp_ros::utils 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 2/2] 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");