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 · b69fe72 · b69fe72
1 parent 214c45d
commit b69fe72
Show file tree

Hide file tree

Showing 8 changed files with 53 additions and 25 deletions.
diff --git a/cpp/kinematic_icp/correspondence_threshold/CorrespondenceThreshold.cpp b/cpp/kinematic_icp/correspondence_threshold/CorrespondenceThreshold.cpp
@@ -26,7 +26,7 @@
 #include <sophus/se3.hpp>
 
 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 &current_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
@@ -33,12 +33,12 @@ struct CorrespondenceThreshold {
                                      const bool use_adaptive_threshold,
                                      const double fixed_threshold);
 
-    void UpdateModelError(const Sophus::SE3d &current_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
@@ -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
@@ -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<Vector3dVector, Vector3dVector>;
 
     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
@@ -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
@@ -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
@@ -89,6 +89,10 @@ LidarOdometryServer::LidarOdometryServer(rclcpp::Node::SharedPtr node) : node_(n
         node->declare_parameter<double>("convergence_criterion", config.convergence_criterion);
     config.max_num_threads =
         node->declare_parameter<int>("max_num_threads", config.max_num_threads);
+    config.use_adaptive_odometry_regularization = node->declare_parameter<bool>(
+        "use_adaptive_odometry_regularization", config.use_adaptive_odometry_regularization);
+    config.fixed_regularization =
+        node->declare_parameter<double>("fixed_regularization", config.fixed_regularization);
     // Motion compensation
     config.deskew = node->declare_parameter<bool>("deskew", config.deskew);
     if (config.max_range < config.min_range) {