From b69fe722a4589d7b0cc0eb7a1c9c2170bf294991 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 --- .../CorrespondenceThreshold.cpp | 15 ++++++------- .../CorrespondenceThreshold.hpp | 8 +++---- cpp/kinematic_icp/pipeline/KinematicICP.cpp | 6 +++--- cpp/kinematic_icp/pipeline/KinematicICP.hpp | 9 ++++++-- .../registration/Registration.cpp | 21 +++++++++++++------ .../registration/Registration.hpp | 10 ++++++--- ros/config/kinematic_icp_ros.yaml | 5 +++++ .../server/LidarOdometryServer.cpp | 4 ++++ 8 files changed, 53 insertions(+), 25 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 1646d75..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; } @@ -49,8 +49,8 @@ struct CorrespondenceThreshold { bool use_adaptive_threshold_; double fixed_threshold_; // <-- Ignored if use_adaptive_threshold_ = true - // Local cache for ccomputation - double model_sse_; + // Local cache for computation + 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..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 @@ -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; 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..74ae19a 100644 --- a/ros/config/kinematic_icp_ros.yaml +++ b/ros/config/kinematic_icp_ros.yaml @@ -15,6 +15,11 @@ 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 # 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 b36b63b..fadfc4a 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) {