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]>
PRBonn · Nov 19, 2024 · 26a3450 · 26a3450
1 parent 83ec040
commit 26a3450
Show file tree

Hide file tree

Showing 5 changed files with 35 additions and 11 deletions.
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,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
@@ -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) {