From 1a5a94c99c9d8c9d1ed4d2fdf9e137c00dc2baee Mon Sep 17 00:00:00 2001
From: tizianoGuadagnino <frevo93@gmail.com>
Date: Tue, 12 Nov 2024 15:27:54 +0100
Subject: [PATCH] Renaming according to Ben's review

---
 .../CorrespondenceThreshold.cpp                   | 15 ++++++++-------
 .../CorrespondenceThreshold.hpp                   |  6 +++---
 cpp/kinematic_icp/pipeline/KinematicICP.cpp       |  4 ++--
 3 files changed, 13 insertions(+), 12 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 <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
index e2b5c07..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 &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;
     }
 
@@ -50,7 +50,7 @@ struct CorrespondenceThreshold {
     double fixed_threshold_;  // <-- Ignored if use_adaptive_threshold_ = true
 
     // Local cache for computation
-    double model_sse_;
+    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..68e651b 100644
--- a/cpp/kinematic_icp/pipeline/KinematicICP.cpp
+++ b/cpp/kinematic_icp/pipeline/KinematicICP.cpp
@@ -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;