From 19219c1a224b5b7fc9dd8fc0058370bfc7f03d7c 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 --- .../correspondence_threshold/CMakeLists.txt | 5 +++-- .../CorrespondenceThreshold.cpp | 19 +++++++++++-------- .../CorrespondenceThreshold.hpp | 13 +++++++++++++ cpp/kinematic_icp/pipeline/CMakeLists.txt | 2 +- 4 files changed, 28 insertions(+), 11 deletions(-) diff --git a/cpp/kinematic_icp/correspondence_threshold/CMakeLists.txt b/cpp/kinematic_icp/correspondence_threshold/CMakeLists.txt index 155d79b..df30730 100644 --- a/cpp/kinematic_icp/correspondence_threshold/CMakeLists.txt +++ b/cpp/kinematic_icp/correspondence_threshold/CMakeLists.txt @@ -20,6 +20,7 @@ # 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 CorrespondenceThreshold.cpp) -target_link_libraries(kinematic_icp_threshold Sophus::Sophus) +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 index 73f3bf8..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,20 +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 { - const double moder_error_variance = std::sqrt(model_sse_ / num_samples_); - const double adaptive_threshold = 3.0 * (map_discretization_error_ + moder_error_variance); - return use_adaptive_threshold_ ? adaptive_threshold : fixed_threshold_; + 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::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..d8480b2 100644 --- a/cpp/kinematic_icp/correspondence_threshold/CorrespondenceThreshold.hpp +++ b/cpp/kinematic_icp/correspondence_threshold/CorrespondenceThreshold.hpp @@ -33,12 +33,20 @@ struct CorrespondenceThreshold { const bool use_adaptive_threshold, const double fixed_threshold); +<<<<<<< HEAD void UpdateModelError(const Sophus::SE3d ¤t_deviation); +======= + void UpdateOdometryError(const Sophus::SE3d &odometry_error); +>>>>>>> 59bc9fc (Kinematic ICP Threshold (#15)) double ComputeThreshold() const; inline void Reset() { +<<<<<<< HEAD model_sse_ = 0.0; +======= + odom_sse_ = 0.0; +>>>>>>> 59bc9fc (Kinematic ICP Threshold (#15)) num_samples_ = 1e-8; } @@ -49,8 +57,13 @@ struct CorrespondenceThreshold { bool use_adaptive_threshold_; double fixed_threshold_; // <-- Ignored if use_adaptive_threshold_ = true +<<<<<<< HEAD // Local cache for ccomputation double model_sse_; +======= + // Local cache for computation + double odom_sse_; +>>>>>>> 59bc9fc (Kinematic ICP Threshold (#15)) double num_samples_; }; } // namespace kinematic_icp diff --git a/cpp/kinematic_icp/pipeline/CMakeLists.txt b/cpp/kinematic_icp/pipeline/CMakeLists.txt index a6f32aa..3a95025 100644 --- a/cpp/kinematic_icp/pipeline/CMakeLists.txt +++ b/cpp/kinematic_icp/pipeline/CMakeLists.txt @@ -23,5 +23,5 @@ add_library(kinematic_icp_pipeline STATIC) target_sources(kinematic_icp_pipeline PRIVATE KinematicICP.cpp) target_link_libraries(kinematic_icp_pipeline PUBLIC kinematic_icp_registration kinematic_icp_threshold - kiss_icp_pipeline Eigen3::Eigen Sophus::Sophus) + kiss_icp_pipeline) set_global_target_properties(kinematic_icp_pipeline)