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 <[email protected]> * 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 <[email protected]> * Renaming according to Ben's review * Why a ref * Use the same default --------- Co-authored-by: Benedikt Mersch <[email protected]> Co-authored-by: Benedikt Mersch <[email protected]> --------- Co-authored-by: Benedikt Mersch <[email protected]> Co-authored-by: Benedikt Mersch <[email protected]>
PRBonn · Nov 19, 2024 · 59bc9fc · 59bc9fc
1 parent 27d76e3
commit 59bc9fc
Show file tree

Hide file tree

Showing 11 changed files with 241 additions and 36 deletions.
diff --git 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
@@ -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
@@ -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 <cmath>
+#include <sophus/se3.hpp>
+
+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
@@ -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 <cmath>
+#include <sophus/se3.hpp>
+
+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
@@ -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
@@ -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
@@ -23,30 +23,60 @@
 #pragma once
 
 #include <Eigen/Core>
+#include <cmath>
 #include <kiss_icp/core/Threshold.hpp>
 #include <kiss_icp/core/VoxelHashMap.hpp>
 #include <kiss_icp/pipeline/KissICP.hpp>
 #include <sophus/se3.hpp>
 #include <tuple>
 #include <vector>
 
+#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<Eigen::Vector3d>;
     using Vector3dVectorTuple = std::tuple<Vector3dVector, Vector3dVector>;
 
-    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<Eigen::Vector3d> &frame,
                                       const std::vector<double> &timestamps,
@@ -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<Eigen::Vector3d> 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
@@ -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<Eigen::
     auto correspondences =
         DataAssociation(frame, voxel_map, current_estimate, max_correspondence_distance);
 
-    const double regularization_term =
-        ComputeOdometryRegularization(correspondences, current_estimate);
+    const double regularization_term = [&]() {
+        if (use_adaptive_odometry_regularization_) {
+            return ComputeOdometryRegularization(correspondences, current_estimate);
+        } else {
+            return fixed_regularization_;
+        }
+    }();
     // ICP-loop
     for (int j = 0; j < max_num_iterations_; ++j) {
         const auto dx = ComputePerturbation(correspondences, current_estimate, regularization_term);

diff --git a/cpp/kinematic_icp/registration/Registration.hpp b/cpp/kinematic_icp/registration/Registration.hpp
@@ -30,9 +30,11 @@
 namespace kinematic_icp {
 
 struct KinematicRegistration {
-    explicit KinematicRegistration(int max_num_iteration,
-                                   double convergence_criterion,
-                                   int max_num_threads);
+    explicit 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);
 
     Sophus::SE3d ComputeRobotMotion(const std::vector<Eigen::Vector3d> &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
@@ -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