From 6c2f6c5f6e477452242d9753fdbd54e24e0bcd7c Mon Sep 17 00:00:00 2001 From: Robofish <1683502971@qq.com> Date: Sat, 28 Feb 2026 11:35:30 +0800 Subject: [PATCH] add gicp --- .../small_gicp_relocalization.hpp | 1 + .../src/small_gicp_relocalization.cpp | 4 +++- .../config/reality/small_gicp_relocalization_real.yaml | 5 +++-- .../config/simulation/small_gicp_relocalization_sim.yaml | 9 +++++---- 4 files changed, 12 insertions(+), 7 deletions(-) diff --git a/src/rm_localization/small_gicp_relocalization/include/small_gicp_relocalization/small_gicp_relocalization.hpp b/src/rm_localization/small_gicp_relocalization/include/small_gicp_relocalization/small_gicp_relocalization.hpp index 391a137..e51b7ed 100644 --- a/src/rm_localization/small_gicp_relocalization/include/small_gicp_relocalization/small_gicp_relocalization.hpp +++ b/src/rm_localization/small_gicp_relocalization/include/small_gicp_relocalization/small_gicp_relocalization.hpp @@ -55,6 +55,7 @@ private: float global_leaf_size_; float registered_leaf_size_; float max_dist_sq_; + int max_iterations_; std::vector init_pose_; std::string map_frame_; diff --git a/src/rm_localization/small_gicp_relocalization/src/small_gicp_relocalization.cpp b/src/rm_localization/small_gicp_relocalization/src/small_gicp_relocalization.cpp index 44e499d..8dd0464 100644 --- a/src/rm_localization/small_gicp_relocalization/src/small_gicp_relocalization.cpp +++ b/src/rm_localization/small_gicp_relocalization/src/small_gicp_relocalization.cpp @@ -33,6 +33,7 @@ SmallGicpRelocalizationNode::SmallGicpRelocalizationNode(const rclcpp::NodeOptio this->declare_parameter("global_leaf_size", 0.25); this->declare_parameter("registered_leaf_size", 0.25); this->declare_parameter("max_dist_sq", 1.0); + this->declare_parameter("max_iterations", 50); this->declare_parameter("map_frame", "map"); this->declare_parameter("odom_frame", "odom"); this->declare_parameter("base_frame", ""); @@ -47,6 +48,7 @@ SmallGicpRelocalizationNode::SmallGicpRelocalizationNode(const rclcpp::NodeOptio this->get_parameter("global_leaf_size", global_leaf_size_); this->get_parameter("registered_leaf_size", registered_leaf_size_); this->get_parameter("max_dist_sq", max_dist_sq_); + this->get_parameter("max_iterations", max_iterations_); this->get_parameter("map_frame", map_frame_); this->get_parameter("odom_frame", odom_frame_); this->get_parameter("base_frame", base_frame_); @@ -167,7 +169,7 @@ void SmallGicpRelocalizationNode::performRegistration() register_->reduction.num_threads = num_threads_; register_->rejector.max_dist_sq = max_dist_sq_; - register_->optimizer.max_iterations = 10; + register_->optimizer.max_iterations = max_iterations_; auto result = register_->align(*target_, *source_, *target_tree_, previous_result_t_); diff --git a/src/rm_nav_bringup/config/reality/small_gicp_relocalization_real.yaml b/src/rm_nav_bringup/config/reality/small_gicp_relocalization_real.yaml index e547378..74bd37a 100644 --- a/src/rm_nav_bringup/config/reality/small_gicp_relocalization_real.yaml +++ b/src/rm_nav_bringup/config/reality/small_gicp_relocalization_real.yaml @@ -5,10 +5,11 @@ num_neighbors: 20 global_leaf_size: 0.25 registered_leaf_size: 0.25 - max_dist_sq: 1.0 + max_dist_sq: 25.0 + max_iterations: 50 map_frame: "map" odom_frame: "odom" - base_frame: "base_link" + base_frame: "livox_frame" robot_base_frame: "base_link" lidar_frame: "livox_frame" prior_pcd_file: "" diff --git a/src/rm_nav_bringup/config/simulation/small_gicp_relocalization_sim.yaml b/src/rm_nav_bringup/config/simulation/small_gicp_relocalization_sim.yaml index e582272..cb1fb0b 100644 --- a/src/rm_nav_bringup/config/simulation/small_gicp_relocalization_sim.yaml +++ b/src/rm_nav_bringup/config/simulation/small_gicp_relocalization_sim.yaml @@ -3,12 +3,13 @@ use_sim_time: true num_threads: 4 num_neighbors: 20 - global_leaf_size: 0.25 - registered_leaf_size: 0.25 - max_dist_sq: 1.0 + global_leaf_size: 0.1 + registered_leaf_size: 0.05 + max_dist_sq: 3.0 + max_iterations: 50 map_frame: "map" odom_frame: "odom" - base_frame: "base_link" + base_frame: "livox_frame" robot_base_frame: "base_link" lidar_frame: "livox_frame" prior_pcd_file: ""