robofish/MOVE_AI
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

add gicp

Browse Source
This commit is contained in:
Robofish 2026-02-28 11:35:30 +08:00
parent 70e68e2098
commit 6c2f6c5f6e
4 changed files with 12 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
src/rm_localization/small_gicp_relocalization/include/small_gicp_relocalization/small_gicp_relocalization.hpp
View File

@ -55,6 +55,7 @@ private:
float global_leaf_size_; float global_leaf_size_;
float registered_leaf_size_; float registered_leaf_size_;
float max_dist_sq_; float max_dist_sq_;
int max_iterations_;
std::vector<double> init_pose_; std::vector<double> init_pose_;
std::string map_frame_; std::string map_frame_;

4
src/rm_localization/small_gicp_relocalization/src/small_gicp_relocalization.cpp
View File

@ -33,6 +33,7 @@ SmallGicpRelocalizationNode::SmallGicpRelocalizationNode(const rclcpp::NodeOptio
this->declare_parameter("global_leaf_size", 0.25); this->declare_parameter("global_leaf_size", 0.25);
this->declare_parameter("registered_leaf_size", 0.25); this->declare_parameter("registered_leaf_size", 0.25);
this->declare_parameter("max_dist_sq", 1.0); this->declare_parameter("max_dist_sq", 1.0);
this->declare_parameter("max_iterations", 50);
this->declare_parameter("map_frame", "map"); this->declare_parameter("map_frame", "map");
this->declare_parameter("odom_frame", "odom"); this->declare_parameter("odom_frame", "odom");
this->declare_parameter("base_frame", ""); 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("global_leaf_size", global_leaf_size_);
this->get_parameter("registered_leaf_size", registered_leaf_size_); this->get_parameter("registered_leaf_size", registered_leaf_size_);
this->get_parameter("max_dist_sq", max_dist_sq_); 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("map_frame", map_frame_);
this->get_parameter("odom_frame", odom_frame_); this->get_parameter("odom_frame", odom_frame_);
this->get_parameter("base_frame", base_frame_); this->get_parameter("base_frame", base_frame_);
@ -167,7 +169,7 @@ void SmallGicpRelocalizationNode::performRegistration()
register_->reduction.num_threads = num_threads_; register_->reduction.num_threads = num_threads_;
register_->rejector.max_dist_sq = max_dist_sq_; 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_); auto result = register_->align(*target_, *source_, *target_tree_, previous_result_t_);

5
src/rm_nav_bringup/config/reality/small_gicp_relocalization_real.yaml
View File

@ -5,10 +5,11 @@
num_neighbors: 20 num_neighbors: 20
global_leaf_size: 0.25 global_leaf_size: 0.25
registered_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" map_frame: "map"
odom_frame: "odom" odom_frame: "odom"
base_frame: "base_link" base_frame: "livox_frame"
robot_base_frame: "base_link" robot_base_frame: "base_link"
lidar_frame: "livox_frame" lidar_frame: "livox_frame"
prior_pcd_file: "" prior_pcd_file: ""

9
src/rm_nav_bringup/config/simulation/small_gicp_relocalization_sim.yaml
View File

@ -3,12 +3,13 @@
use_sim_time: true use_sim_time: true
num_threads: 4 num_threads: 4
num_neighbors: 20 num_neighbors: 20
global_leaf_size: 0.25 global_leaf_size: 0.1
registered_leaf_size: 0.25 registered_leaf_size: 0.05
max_dist_sq: 1.0 max_dist_sq: 3.0
max_iterations: 50
map_frame: "map" map_frame: "map"
odom_frame: "odom" odom_frame: "odom"
base_frame: "base_link" base_frame: "livox_frame"
robot_base_frame: "base_link" robot_base_frame: "base_link"
lidar_frame: "livox_frame" lidar_frame: "livox_frame"
prior_pcd_file: "" prior_pcd_file: ""