From d0c387e03c7b0fc2ca7f42ffef15867893c00978 Mon Sep 17 00:00:00 2001 From: Florian Vahl <7vahl@informatik.uni-hamburg.de> Date: Thu, 2 Nov 2023 14:08:26 +0100 Subject: [PATCH] Make out of map value a parameter --- bitbots_localization/config/config.yaml | 12 +++-- bitbots_localization/src/ObservationModel.cpp | 8 ++-- bitbots_localization/src/localization.cpp | 25 +++++----- bitbots_localization/src/parameters.yml | 48 +++++++++++++------ 4 files changed, 59 insertions(+), 34 deletions(-) diff --git a/bitbots_localization/config/config.yaml b/bitbots_localization/config/config.yaml index 279bf55..bbf901d 100644 --- a/bitbots_localization/config/config.yaml +++ b/bitbots_localization/config/config.yaml @@ -38,9 +38,15 @@ bitbots_localization: out_of_field_weight_decrease: 0.01 out_of_field_range: 0.5 scoring: - lines_factor: 1.0 - goal_factor: 0.0 - field_boundary_factor: 0.0 + lines: + factor: 1.0 + out_of_field_score: 0.0 + goal: + factor: 0.0 + out_of_field_score: 0.0 + field_boundary: + factor: 0.0 + out_of_field_score: 0.0 confidences: line_element: 0.01 goal_element: 0.0 diff --git a/bitbots_localization/src/ObservationModel.cpp b/bitbots_localization/src/ObservationModel.cpp index d6fcdf9..fd2e03a 100644 --- a/bitbots_localization/src/ObservationModel.cpp +++ b/bitbots_localization/src/ObservationModel.cpp @@ -45,10 +45,10 @@ double RobotPoseObservationModel::measure(const RobotState &state) const { auto scoring_config = config_.particle_filter.scoring; // Calculate weight for the particle - double weight = (((1 - scoring_config.lines_factor) + scoring_config.lines_factor * particle_weight_lines) * - ((1 - scoring_config.goal_factor) + scoring_config.goal_factor * particle_weight_goal) * - ((1 - scoring_config.field_boundary_factor) + - scoring_config.field_boundary_factor * particle_weight_field_boundary)); + double weight = (((1 - scoring_config.lines.factor) + scoring_config.lines.factor * particle_weight_lines) * + ((1 - scoring_config.goal.factor) + scoring_config.goal.factor * particle_weight_goal) * + ((1 - scoring_config.field_boundary.factor) + + scoring_config.field_boundary.factor * particle_weight_field_boundary)); if (weight < min_weight_) { weight = min_weight_; diff --git a/bitbots_localization/src/localization.cpp b/bitbots_localization/src/localization.cpp index 376ede9..0e86191 100644 --- a/bitbots_localization/src/localization.cpp +++ b/bitbots_localization/src/localization.cpp @@ -91,16 +91,17 @@ void Localization::updateParams(bool force_reload) { config_ = param_listener_.get_params(); // Check if measurement type is used and load the correct map for that - if (config_.particle_filter.scoring.lines_factor) { - lines_.reset(new Map(config_.field.name, "lines.png", -10.0)); + if (config_.particle_filter.scoring.lines.factor) { + lines_.reset(new Map(config_.field.name, "lines.png", config_.particle_filter.scoring.lines.out_of_field_score)); // Publish the line map once field_publisher_->publish(lines_->get_map_msg(config_.ros.map_frame)); } - if (config_.particle_filter.scoring.goal_factor) { - goals_.reset(new Map(config_.field.name, "goals.png", -10.0)); + if (config_.particle_filter.scoring.goal.factor) { + goals_.reset(new Map(config_.field.name, "goals.png", config_.particle_filter.scoring.goal.out_of_field_score)); } - if (config_.particle_filter.scoring.field_boundary_factor) { - field_boundary_.reset(new Map(config_.field.name, "field_boundary.png", -10.0)); + if (config_.particle_filter.scoring.field_boundary.factor) { + field_boundary_.reset(new Map(config_.field.name, "field_boundary.png", + config_.particle_filter.scoring.field_boundary.out_of_field_score)); } // Init observation model @@ -297,13 +298,13 @@ void Localization::reset_filter(int distribution, double x, double y, double ang void Localization::updateMeasurements() { // Sets the measurements in the observation model - if (line_pointcloud_relative_.header.stamp != last_stamp_lines_pc && config_.particle_filter.scoring.lines_factor) { + if (line_pointcloud_relative_.header.stamp != last_stamp_lines_pc && config_.particle_filter.scoring.lines.factor) { robot_pose_observation_model_->set_measurement_lines_pc(line_pointcloud_relative_); } - if (config_.particle_filter.scoring.goal_factor && goal_posts_relative_.header.stamp != last_stamp_goals) { + if (config_.particle_filter.scoring.goal.factor && goal_posts_relative_.header.stamp != last_stamp_goals) { robot_pose_observation_model_->set_measurement_goalposts(goal_posts_relative_); } - if (config_.particle_filter.scoring.field_boundary_factor && + if (config_.particle_filter.scoring.field_boundary.factor && fieldboundary_relative_.header.stamp != last_stamp_fb_points) { robot_pose_observation_model_->set_measurement_field_boundary(fieldboundary_relative_); } @@ -499,17 +500,17 @@ void Localization::publish_particle_markers() { } void Localization::publish_ratings() { - if (config_.particle_filter.scoring.lines_factor) { + if (config_.particle_filter.scoring.lines.factor) { // Publish line ratings publish_debug_rating(robot_pose_observation_model_->get_measurement_lines(), 0.1, "line_ratings", lines_, line_ratings_publisher_); } - if (config_.particle_filter.scoring.goal_factor) { + if (config_.particle_filter.scoring.goal.factor) { // Publish goal ratings publish_debug_rating(robot_pose_observation_model_->get_measurement_goals(), 0.2, "goal_ratings", goals_, goal_ratings_publisher_); } - if (config_.particle_filter.scoring.field_boundary_factor) { + if (config_.particle_filter.scoring.field_boundary.factor) { // Publish field boundary ratings publish_debug_rating(robot_pose_observation_model_->get_measurement_field_boundary(), 0.2, "field_boundary_ratings", field_boundary_, fieldboundary_ratings_publisher_); diff --git a/bitbots_localization/src/parameters.yml b/bitbots_localization/src/parameters.yml index 252516a..f31f193 100644 --- a/bitbots_localization/src/parameters.yml +++ b/bitbots_localization/src/parameters.yml @@ -171,21 +171,39 @@ bitbots_localization: validation: bounds<>: [-5.0, 5.0] scoring: - lines_factor: - type: double - description: "Weighing how much the line information is considered for the scoring of a particle" - validation: - bounds<>: [0.0, 1.0] - goal_factor: - type: double - description: "Weighing how much the goal information is considered for the scoring of a particle" - validation: - bounds<>: [0.0, 1.0] - field_boundary_factor: - type: double - description: "Weighing how much the field boundary information is considered for the scoring of a particle" - validation: - bounds<>: [0.0, 1.0] + lines: + factor: + type: double + description: "Weighing how much the line information is considered for the scoring of a particle" + validation: + bounds<>: [0.0, 1.0] + out_of_field_score: + type: double + description: "Score which is given to a measurement (e.g. projected line pixel) if it is out of the field" + validation: + bounds<>: [0.0, 100.0] + goal: + factor: + type: double + description: "Weighing how much the goal information is considered for the scoring of a particle" + validation: + bounds<>: [0.0, 1.0] + out_of_field_score: + type: double + description: "Score which is given to a measurement (e.g. projected goal post) if it is out of the field" + validation: + bounds<>: [0.0, 100.0] + field_boundary: + factor: + type: double + description: "Weighing how much the field boundary information is considered for the scoring of a particle" + validation: + bounds<>: [0.0, 1.0] + out_of_field_score: + type: double + description: "Score which is given to a measurement (e.g. projected field boundary segment) if it is out of the field" + validation: + bounds<>: [0.0, 100.0] confidences: line_element: type: double