From b34ce99b3d0e09c3a0d6d5b145dd16ca88da22cd Mon Sep 17 00:00:00 2001 From: mbsaloka Date: Mon, 19 Jan 2026 19:23:03 +0700 Subject: [PATCH 01/33] docs: add readme --- README.md | 2 ++ 1 file changed, 2 insertions(+) create mode 100644 README.md diff --git a/README.md b/README.md new file mode 100644 index 0000000..b541d6f --- /dev/null +++ b/README.md @@ -0,0 +1,2 @@ +# basho +Localization Package for ROS 2 Project. From 0d1928a87212ad636b23c68efa947863e5be9f7f Mon Sep 17 00:00:00 2001 From: mbsaloka Date: Tue, 20 Jan 2026 19:04:58 +0700 Subject: [PATCH 02/33] feat: add ros node and MCL logic --- .github/pull_request_template.md | 27 ++ .github/workflows/ci.yml | 21 ++ .github/workflows/dispatch-pr-bot.yml | 21 ++ CMakeLists.txt | 90 ++++- include/basho/basho.hpp | 27 ++ include/basho/localization/mcl.hpp | 136 +++++++ include/basho/model/field.hpp | 81 ++++ include/basho/node/basho_node.hpp | 78 ++++ include/basho/process/localization.hpp | 69 ++++ package.xml | 14 +- src/basho/localization/mcl.cpp | 500 +++++++++++++++++++++++++ src/basho/node/basho_node.cpp | 166 ++++++++ src/basho/process/localization.cpp | 121 ++++++ src/basho_main.cpp | 57 +++ src/basho_node.cpp | 10 - 15 files changed, 1387 insertions(+), 31 deletions(-) create mode 100644 .github/pull_request_template.md create mode 100644 .github/workflows/ci.yml create mode 100644 .github/workflows/dispatch-pr-bot.yml create mode 100644 include/basho/basho.hpp create mode 100644 include/basho/localization/mcl.hpp create mode 100644 include/basho/model/field.hpp create mode 100644 include/basho/node/basho_node.hpp create mode 100644 include/basho/process/localization.hpp create mode 100644 src/basho/localization/mcl.cpp create mode 100644 src/basho/node/basho_node.cpp create mode 100644 src/basho/process/localization.cpp create mode 100644 src/basho_main.cpp delete mode 100644 src/basho_node.cpp diff --git a/.github/pull_request_template.md b/.github/pull_request_template.md new file mode 100644 index 0000000..a85b791 --- /dev/null +++ b/.github/pull_request_template.md @@ -0,0 +1,27 @@ +## Jira Link: + +## Description + +Describe problems, if any, clearly and concisely. +Describe any changes that have been made in this pull request. + +## Type of Change + +- [ ] Bugfix +- [ ] Enhancement +- [ ] New feature +- [ ] Breaking change (fix or feature that would cause the existing functionality to not work as expected) + +## How Has This Been Tested? + +- [ ] New unit tests added. +- [ ] Manual tested. + +## Checklist: + +- [ ] Using Branch Name Convention + - `feature/JIRA-ID-SHORT-DESCRIPTION` if has a JIRA ticket + - `enhancement/SHORT-DESCRIPTION` if has/has no JIRA ticket and contain enhancement + - `hotfix/SHORT-DESCRIPTION` if the change doesn't need to be tested (urgent) +- [ ] I have commented on my code, particularly in hard-to-understand areas. +- [ ] I have made the documentation for the corresponding changes. \ No newline at end of file diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml new file mode 100644 index 0000000..c0bef04 --- /dev/null +++ b/.github/workflows/ci.yml @@ -0,0 +1,21 @@ +name: CI +on: + workflow_dispatch: + pull_request: + push: + branches: [master] +jobs: + build-and-test: + name: Build and Test + runs-on: ubuntu-latest + steps: + - name: Checkout + uses: actions/checkout@v4.1.1 + with: + path: hurocup + + - name: Setup workspace + uses: ichiro-its/ros2-ws-action/setup@v1.0.1 + + - name: Build workspace + uses: ichiro-its/ros2-ws-action/build@v1.0.1 diff --git a/.github/workflows/dispatch-pr-bot.yml b/.github/workflows/dispatch-pr-bot.yml new file mode 100644 index 0000000..016b0cb --- /dev/null +++ b/.github/workflows/dispatch-pr-bot.yml @@ -0,0 +1,21 @@ +name: Dispatch Discord PR Bot + +on: + pull_request: + types: [opened, closed, reopened] + workflow_dispatch: + +jobs: + dispatch-discord-pr-bot: + runs-on: ubuntu-latest + steps: + - uses: peter-evans/repository-dispatch@v3 + with: + token: ${{ secrets.GH_TOKEN }} + repository: ichiro-its/${{ vars.DISCORD_PR_BOT_REPO }} + event-type: pull_request + client-payload: | + { + "repository": "${{ github.event.repository.name }}", + "pr_type": "${{ github.event.action || 'opened' }}" + } diff --git a/CMakeLists.txt b/CMakeLists.txt index 95e5b28..9ca04ac 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,35 +1,89 @@ -cmake_minimum_required(VERSION 3.8) +cmake_minimum_required(VERSION 3.5) project(basho) +if(NOT CMAKE_C_STANDARD) + set(CMAKE_C_STANDARD 99) +endif() + +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 17) +endif() + if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") - add_compile_options(-Wall -Wextra -Wpedantic) + add_compile_options(-Wall -Wextra -Wpedantic -fPIC) endif() -# find dependencies find_package(ament_cmake REQUIRED) -# uncomment the following section in order to fill in -# further dependencies manually. -# find_package( REQUIRED) +find_package(ament_index_cpp REQUIRED) +find_package(aruku REQUIRED) +find_package(aruku_interfaces REQUIRED) +find_package(gyakuenki_interfaces REQUIRED) +find_package(jitsuyo REQUIRED) +find_package(kansei REQUIRED) +find_package(kansei_interfaces REQUIRED) +find_package(keisan REQUIRED) +find_package(rclcpp REQUIRED) +find_package(std_msgs REQUIRED) +find_package(basho_interfaces REQUIRED) +include_directories(${EIGEN3_INCLUDE_DIRS}) + +add_library(${PROJECT_NAME} SHARED + "src/${PROJECT_NAME}/node/basho_node.cpp" + "src/${PROJECT_NAME}/localization/mcl.cpp" + "src/${PROJECT_NAME}/process/localization.cpp") + +target_include_directories(${PROJECT_NAME} PUBLIC + $ + $) + +ament_target_dependencies(${PROJECT_NAME} + aruku + aruku_interfaces + gyakuenki_interfaces + jitsuyo + kansei + kansei_interfaces + keisan + rclcpp + std_msgs + basho_interfaces) + +install(DIRECTORY "include" DESTINATION ".") + +install(TARGETS ${PROJECT_NAME} + EXPORT export_${PROJECT_NAME} + ARCHIVE DESTINATION "lib" + LIBRARY DESTINATION "lib" + RUNTIME DESTINATION "bin") -add_executable(basho_node src/basho_node.cpp) -target_include_directories(basho_node PUBLIC +add_executable(main "src/basho_main.cpp") +target_include_directories(main PUBLIC $ - $) -target_compile_features(basho_node PUBLIC c_std_99 cxx_std_17) # Require C99 and C++17 + $) +target_link_libraries(main ${PROJECT_NAME}) -install(TARGETS basho_node +install(TARGETS + main DESTINATION lib/${PROJECT_NAME}) if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) - # the following line skips the linter which checks for copyrights - # comment the line when a copyright and license is added to all source files - set(ament_cmake_copyright_FOUND TRUE) - # the following line skips cpplint (only works in a git repo) - # comment the line when this package is in a git repo and when - # a copyright and license is added to all source files - set(ament_cmake_cpplint_FOUND TRUE) ament_lint_auto_find_test_dependencies() endif() +ament_export_dependencies( + ament_index_cpp + aruku + aruku_interfaces + atama_interfaces + gyakuenki_interfaces + jitsuyo + kansei + kansei_interfaces + keisan + rclcpp + std_msgs + basho_interfaces) +ament_export_include_directories("include") +ament_export_libraries(${PROJECT_NAME}) ament_package() diff --git a/include/basho/basho.hpp b/include/basho/basho.hpp new file mode 100644 index 0000000..9a45a8d --- /dev/null +++ b/include/basho/basho.hpp @@ -0,0 +1,27 @@ +// Copyright (c) 2026 Ichiro ITS +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in +// all copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL +// THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// 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. + +#ifndef BASHO__BASHO_HPP_ +#define BASHO__BASHO_HPP_ + +#include "basho/node/basho_node.hpp" +#include "basho/localization/mcl.hpp" + +#endif // BASHO__BASHO_HPP_ diff --git a/include/basho/localization/mcl.hpp b/include/basho/localization/mcl.hpp new file mode 100644 index 0000000..55d9be8 --- /dev/null +++ b/include/basho/localization/mcl.hpp @@ -0,0 +1,136 @@ +// Copyright (c) 2026 Ichiro ITS +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in +// all copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL +// THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// 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. + +#ifndef BASHO__LOCALIZATION__MCL_HPP_ +#define BASHO__LOCALIZATION__MCL_HPP_ + +#include +#include + +#include "keisan/keisan.hpp" +#include "keisan/hungarian.hpp" +#include "basho/model/field.hpp" + +namespace basho +{ + +struct ProjectedObject +{ + std::string label; + keisan::Point3 position; +}; + +struct Particle +{ + keisan::Point2 position; + keisan::Angle orientation; + double weight; +}; + +struct LandmarkGroup { + std::vector* projected; + std::vector* landmarks; +}; + +class MCL +{ +public: + MCL(); + + void localize(); + void reset_localization(); + void clear_projected_objects(); + void init_particles(const keisan::Point2 init_position); + void resample_particles(); + void update_motion(); + void calculate_weight(); + void estimate_position(); + void print_particles(); + void print_estimate_position(); + void set_initial_localization(bool initial) { initial_localization = initial; } + + double calculate_distance(const keisan::Point2 & point_1, const keisan::Point2 & point_2); + double calculate_total_likelihood(const Particle & particle); + double calculate_object_likelihood(const ProjectedObject & measurement, + const keisan::Point2 & landmark, + const Particle & particle); + + double get_sum_weight(); + keisan::Matrix<6, 6> calculate_cost_matrix(const Particle & particle, + const std::vector & projected_objects, + const std::vector & landmarks); + + std::vector particles; + keisan::Point2 estimated_position; + + int num_particles; + double min_centered_particles_ratio; + double reset_particles_threshold; + double short_term_avg_ratio; + double long_term_avg_ratio; + double sigma_x; + double sigma_y; + + bool apply_localization; + bool print_debug; + + // IPM + std::vector projected_X; + std::vector projected_L; + std::vector projected_T; + std::vector projected_goalpost; + keisan::Point2 max_object_distance; + int num_projected_objects; + + // member for getting + bool is_calibrated; + keisan::Angle orientation; + keisan::Angle orientation_roll; + keisan::Angle orientation_pitch; + keisan::Point2 position; + keisan::Point2 delta_position; + +private: + Field field; + Particle* best_particle; + std::vector center_particles; + + double xvar; + double yvar; + + std::mt19937 rand_gen; + keisan::Hungarian<6> hungarian; + + double weight_avg; + double short_term_avg; + double long_term_avg; + double last_weight_avg; + double prob; + + bool initial_localization; + bool reset_particles; + + int pending_accept_count; + keisan::Point2 pending_estimated_position; +}; + +} // namespace basho + +#endif // BASHO__LOCALIZATION__MCL_HPP_ diff --git a/include/basho/model/field.hpp b/include/basho/model/field.hpp new file mode 100644 index 0000000..ec8eae1 --- /dev/null +++ b/include/basho/model/field.hpp @@ -0,0 +1,81 @@ +// Copyright (c) 2026 Ichiro ITS +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in +// all copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL +// THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// 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. + +#ifndef BASHO__MODEL__FIELD_HPP_ +#define BASHO__MODEL__FIELD_HPP_ + +#include + +#include "keisan/keisan.hpp" + +namespace basho +{ + +struct Field +{ +public: + int width; + int length; + std::vector landmarks_L; + std::vector landmarks_T; + std::vector landmarks_X; + std::vector landmarks_goalpost; + + Field() + : width(600), + length(900), + landmarks_L({ + {0.0, 0.0}, + {0.0, 600.0}, + {100.0, 50.0}, + {100.0, 550.0}, + {900.0, 0.0}, + {900.0, 600.0}, + {800.0, 50.0}, + {800.0, 550.0} + }), + landmarks_T({ + {0.0, 50.0}, + {0.0, 550.0}, + {450.0, 0.0}, + {450.0, 600.0}, + {900.0, 50.0}, + {900.0, 550.0} + }), + landmarks_X({ + {210, 300}, + {690, 300}, + {450, 300}, + {450, 375}, + {450, 225} + }), + landmarks_goalpost({ + {0.0, 170.0}, + {0.0, 430.0}, + {900.0, 170.0}, + {900.0, 430.0} + }) + { + } +}; + +} // namespace basho + +#endif // BASHO__MODEL__FIELD_HPP_ diff --git a/include/basho/node/basho_node.hpp b/include/basho/node/basho_node.hpp new file mode 100644 index 0000000..f5daf27 --- /dev/null +++ b/include/basho/node/basho_node.hpp @@ -0,0 +1,78 @@ +// Copyright (c) 2026 Ichiro ITS +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in +// all copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL +// THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// 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. + +#ifndef BASHO__NODE__BASHO_NODE_HPP_ +#define BASHO__NODE__BASHO_NODE_HPP_ + +#include +#include +#include + +#include "aruku_interfaces/msg/point2.hpp" +#include "aruku_interfaces/msg/status.hpp" +#include "basho/localization/mcl.hpp" +#include "basho/process/localization.hpp" +#include "basho_interfaces/msg/particle.hpp" +#include "basho_interfaces/msg/particles.hpp" +#include "gyakuenki_interfaces/msg/projected_objects.hpp" +#include "kansei_interfaces/msg/status.hpp" + +namespace basho +{ + +class BashoNode +{ +public: + using MeasurementStatus = kansei_interfaces::msg::Status; + using ProjectedObjects = gyakuenki_interfaces::msg::ProjectedObjects; + using Particle = basho_interfaces::msg::Particle; + using Particles = basho_interfaces::msg::Particles; + using Point2 = aruku_interfaces::msg::Point2; + using WalkingStatus = aruku_interfaces::msg::Status; + + static std::string get_node_prefix(); + + void update(); + + BashoNode( + const std::shared_ptr & node, + const std::shared_ptr & localization, + const std::string & path + ); + +private: + void publish_odometry(); + void publish_particles(); + + rclcpp::Node::SharedPtr node; + + rclcpp::Publisher::SharedPtr set_odometry_publisher; + rclcpp::Publisher::SharedPtr particles_publisher; + + rclcpp::Subscription::SharedPtr measurement_status_subscriber; + rclcpp::Subscription::SharedPtr projected_objects_subscriber; + rclcpp::Subscription::SharedPtr delta_position_subscriber; + + std::shared_ptr localization; +}; + +} // namespace basho + +#endif // BASHO__NODE__BASHO_NODE_HPP_ diff --git a/include/basho/process/localization.hpp b/include/basho/process/localization.hpp new file mode 100644 index 0000000..1f8bdc9 --- /dev/null +++ b/include/basho/process/localization.hpp @@ -0,0 +1,69 @@ +// Copyright (c) 2026 Ichiro ITS +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in +// all copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL +// THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// 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. + +#ifndef BASHO__PROCESS__LOCALIZATION_HPP_ +#define BASHO__PROCESS__LOCALIZATION_HPP_ + +#include +#include + +#include "keisan/keisan.hpp" +#include "nlohmann/json.hpp" +#include "basho/localization/mcl.hpp" + +namespace basho +{ + +class Localization +{ +public: + explicit Localization(std::shared_ptr mcl); + + void load_config(const std::string & path); + void set_config(const nlohmann::json & json); + void update(); + + std::shared_ptr get_mcl() const; + + std::string config_name; + +private: + bool debug; + + int num_particles; + + double min_centered_particles_ratio; + double reset_particles_threshold; + + double short_term_avg_ratio; + double long_term_avg_ratio; + + double sigma_x; + double sigma_y; + + double max_distance_x; + double max_distance_y; + + std::shared_ptr mcl; +}; + +} // namespace basho + +#endif // BASHO__PROCESS__LOCALIZATION_HPP_ diff --git a/package.xml b/package.xml index 6d5aac4..a8ddddb 100644 --- a/package.xml +++ b/package.xml @@ -6,12 +6,20 @@ Localization Package for ROS 2 Project mbsaloka MIT License - ament_cmake - + ament_index_cpp + aruku + aruku_interfaces + jitsuyo + kansei + kansei_interfaces + keisan + nlohmann-json-dev + rclcpp + std_msgs + basho_interfaces ament_lint_auto ament_lint_common - ament_cmake diff --git a/src/basho/localization/mcl.cpp b/src/basho/localization/mcl.cpp new file mode 100644 index 0000000..843c460 --- /dev/null +++ b/src/basho/localization/mcl.cpp @@ -0,0 +1,500 @@ +// Copyright (c) 2026 Ichiro ITS +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in +// all copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL +// THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// 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. + +#include +#include + +#include "basho/localization/mcl.hpp" + +#include "keisan/keisan.hpp" + +using keisan::literals::operator""_deg; + +namespace basho +{ + +MCL::MCL() +: position(0.0, 0.0), orientation(0_deg), orientation_roll(0_deg), orientation_pitch(0_deg), + is_calibrated(false), apply_localization(false), print_debug(false), + num_particles(500), min_centered_particles_ratio(0.3), reset_particles_threshold(0.3), + short_term_avg_ratio(0.1), long_term_avg_ratio(0.001), sigma_x(5.0), sigma_y(5.0), + xvar(10.0), yvar(10.0), best_particle(nullptr), rand_gen(std::random_device{}()), + weight_avg(0.0), short_term_avg(0.0), long_term_avg(0.0), last_weight_avg(0.0), + estimated_position(-1.0, -1.0), initial_localization(true), reset_particles(false), + num_projected_objects(0), max_object_distance(300.0, 300.0), pending_accept_count(0) +{ +} + + +void MCL::localize() +{ + if (initial_localization) { + init_particles(keisan::Point2(-1, -1)); + } else { + update_motion(); + } + + bool evaluate_particles = num_projected_objects > 0; + evaluate_particles &= fabs(orientation_roll.degree()) <= 30; + evaluate_particles &= fabs(orientation_pitch.degree()) <= 30; + // evaluate_particles &= fabs(get_pan().degree()) <= 90; + // evaluate_particles &= get_tilt().degree() > -50; + + if (evaluate_particles) { + weight_avg = 0.0; + calculate_weight(); + if (weight_avg <= 0 || std::isnan(weight_avg)) { + weight_avg = last_weight_avg; + } else { + if (initial_localization) { + short_term_avg = weight_avg; + long_term_avg = weight_avg; + } + last_weight_avg = weight_avg; + } + + if (!std::isnan(weight_avg)) { + short_term_avg += short_term_avg_ratio * (weight_avg - short_term_avg); + long_term_avg += long_term_avg_ratio * (weight_avg - long_term_avg); + } + + resample_particles(); + } + estimate_position(); + + clear_projected_objects(); +} + +void MCL::reset_localization() { + apply_localization = true; + initial_localization = true; + particles.clear(); + center_particles.clear(); + clear_projected_objects(); + estimated_position = keisan::Point2(-1.0, -1.0); + best_particle = nullptr; + weight_avg = 0.0; + short_term_avg = 0.0; + long_term_avg = 0.0; + last_weight_avg = 0.0; + reset_particles = false; + prob = 0.0; + pending_accept_count = 0; + pending_estimated_position = keisan::Point2(-1.0, -1.0); + + localize(); +} + +void MCL::clear_projected_objects() +{ + projected_X.clear(); + projected_L.clear(); + projected_T.clear(); + projected_goalpost.clear(); + num_projected_objects = 0; +} + +void MCL::init_particles(const keisan::Point2 init_position) +{ + double uniform_weight = 1.0 / num_particles; + particles.clear(); + particles.resize(num_particles); + initial_localization = true; + estimated_position = init_position; + + if (init_position.x == -1 && init_position.y == -1) { + std::uniform_int_distribution xrg(0, 900); + std::uniform_int_distribution yrg(0, 600); + + for (int i = 0; i < num_particles; ++i) { + particles[i].position = keisan::Point2(xrg(rand_gen), yrg(rand_gen)); + particles[i].orientation = orientation; + particles[i].weight = uniform_weight; + } + } else { + std::normal_distribution xrg(init_position.x, 50); + std::normal_distribution yrg(init_position.y, 50); + + for (int i = 0; i < num_particles; ++i) { + particles[i].position = keisan::Point2(xrg(rand_gen), yrg(rand_gen)); + particles[i].orientation = orientation; + particles[i].weight = uniform_weight; + } + } +} + +void MCL::resample_particles() +{ + std::vector new_particles(num_particles); + std::uniform_int_distribution rand_index(0, num_particles - 1); + int index = rand_index(rand_gen); + + double beta = 0.0; + double max_weight = 0.0; + double sum_weight = get_sum_weight(); + prob = std::max(0.0, 1.0 - short_term_avg / long_term_avg); + // reset_particles = prob > reset_particles_threshold; + + // find the best particle + if (sum_weight > 0) { + for (auto & p : particles) { + if (p.weight > max_weight) { + max_weight = p.weight; + best_particle = &p; + } + } + } + + // determine resample interval area + int interval_x[2] = {10, 890}; + int interval_y[2] = {10, 590}; + + bool resample_nearby = num_projected_objects == 1; + // resample_nearby |= fabs(get_pan().degree()) > 80; + // resample_nearby |= get_tilt().degree() < -40; + resample_nearby |= reset_particles; + + if (resample_nearby) { + interval_x[0] = position.x - 50; + interval_x[1] = position.x + 50; + interval_y[0] = position.y - 50; + interval_y[1] = position.y + 50; + reset_particles = false; + prob = std::min(0.05, prob); + } + + // resample particles + std::uniform_real_distribution rand_prob(0.0, 1.0); + std::uniform_real_distribution rand_beta(0.0, 2.0 * max_weight); + std::uniform_int_distribution xrg(interval_x[0], interval_x[1]); + std::uniform_int_distribution yrg(interval_y[0], interval_y[1]); + + // adaptive noise + double max_sigma = 3.0; + double min_sigma = 0.1; + + double sigma_factor = 1.0 - prob; + double resample_sigma_x = min_sigma + sigma_factor * (max_sigma - min_sigma); + double resample_sigma_y = min_sigma + sigma_factor * (max_sigma - min_sigma); + + std::normal_distribution noise_x(0.0, resample_sigma_x); + std::normal_distribution noise_y(0.0, resample_sigma_y); + + for (int i = 0; i < num_particles; ++i) { + if (rand_prob(rand_gen) < prob) { + new_particles[i].position = keisan::Point2(xrg(rand_gen), yrg(rand_gen)); + new_particles[i].orientation = orientation; + new_particles[i].weight = 0.0; + } else { + beta += rand_beta(rand_gen); + + while (beta > particles[index].weight) { + beta -= particles[index].weight; + index = (index + 1) % num_particles; + } + new_particles[i] = particles[index]; + + new_particles[i].position.x += noise_x(rand_gen); + new_particles[i].position.y += noise_y(rand_gen); + } + + // check is particle out of bound + if (new_particles[i].position.x < 0.0 || new_particles[i].position.x > 900.0 || + new_particles[i].position.y < 0.0 || new_particles[i].position.y > 600.0) { + new_particles[i].position = keisan::Point2(xrg(rand_gen), yrg(rand_gen)); + new_particles[i].orientation = orientation; + new_particles[i].weight = 0.0; + } + } + + particles = new_particles; +} + +void MCL::update_motion() +{ + static std::normal_distribution xgen(0.0, xvar), ygen(0.0, yvar); + bool update_with_noise = num_projected_objects > 0; + update_with_noise &= fabs(orientation_roll.degree()) <= 30; + update_with_noise &= fabs(orientation_pitch.degree()) <= 30; + // update_with_noise &= fabs(get_pan().degree()) <= 90; + // update_with_noise &= get_tilt().degree() > -50; + + if (update_with_noise || true) { + for (auto & p : particles) { + double static_noise_x = xgen(rand_gen) / 10.0; + double static_noise_y = ygen(rand_gen) / 10.0; + double dynamic_noise_x = fabs(delta_position.x) * xgen(rand_gen) / 5.0; + double dynamic_noise_y = fabs(delta_position.y) * ygen(rand_gen) / 5.0; + double x_yterm = fabs(delta_position.y) * xgen(rand_gen) / 30.0; + double y_xterm = fabs(delta_position.x) * ygen(rand_gen) / 30.0; + + p.position.x += delta_position.x + static_noise_x + dynamic_noise_x + x_yterm; + p.position.y += delta_position.y + static_noise_y + dynamic_noise_y + y_xterm; + p.orientation = orientation; + + p.position.x = keisan::clamp(p.position.x, 0.0, 900.0); + p.position.y = keisan::clamp(p.position.y, 0.0, 600.0); + } + } else { + for (auto & p : particles) { + p.position.x += delta_position.x; + p.position.y += delta_position.y; + p.orientation = orientation; + + p.position.x = keisan::clamp(p.position.x, 0.0, 900.0); + p.position.y = keisan::clamp(p.position.y, 0.0, 600.0); + } + } + + estimated_position += delta_position; +} + +double MCL::get_sum_weight() +{ + double sum_weight = 0.0; + for (const auto & p : particles) { + sum_weight += p.weight; + } + return sum_weight; +} + +void MCL::calculate_weight() +{ + double sum_weight = 0.0; + + for (auto & p : particles) { + p.weight = calculate_total_likelihood(p); + sum_weight += p.weight; + } + + if (!std::isnan(sum_weight) && sum_weight > 0) { + weight_avg = sum_weight / num_particles; + for (auto & p : particles) { + p.weight /= sum_weight; + } + } else { + weight_avg = 0.0; + } +} + +double MCL::calculate_distance(const keisan::Point2 & point_1, const keisan::Point2 & point_2) { + double dx = point_1.x - point_2.x; + double dy = point_1.y - point_2.y; + + return sqrt(dx * dx + dy * dy); +} + +keisan::Matrix<6, 6> MCL::calculate_cost_matrix( + const Particle & particle, const std::vector & projected_objects, + const std::vector & landmarks) { + auto cost_matrix = keisan::Matrix<6, 6>::infinite(); + int num_projected_objects = projected_objects.size(); + int num_landmarks = landmarks.size(); + + for (int i = 0; i < num_projected_objects; ++i) { + for (int j = 0; j < num_landmarks; ++j) { + double dx = projected_objects[i].position.x * 100; + double dy = projected_objects[i].position.y * 100; + + double x_rot = dx * particle.orientation.cos() + dy * particle.orientation.sin(); + double y_rot = dx * particle.orientation.sin() - dy * particle.orientation.cos(); + + double relative_position_x = particle.position.x + x_rot; + double relative_position_y = particle.position.y + y_rot; + + keisan::Point2 projected_object(relative_position_x, relative_position_y); + + cost_matrix[i][j] = calculate_distance(landmarks[j], projected_object); + } + } + + return cost_matrix; +} + +double MCL::calculate_total_likelihood(const Particle & particle) { + double total_likelihood = 1.0; + + std::vector landmark_groups = { + {&projected_X, &field.landmarks_X}, + {&projected_L, &field.landmarks_L}, + {&projected_T, &field.landmarks_T}, + {&projected_goalpost, &field.landmarks_goalpost} + }; + + for (const auto & group : landmark_groups) { + if (group.projected->empty() || group.landmarks->empty()) { + continue; + } + + auto cost_matrix = calculate_cost_matrix(particle, *group.projected, *group.landmarks); + auto result = hungarian.solve(cost_matrix, group.landmarks->size()); + + for (int i = 0; i < group.projected->size(); ++i) { + for (int j = 0; j < group.landmarks->size(); ++j) { + if (result[i][j] == 1) { + total_likelihood *= calculate_object_likelihood((*group.projected)[i], (*group.landmarks)[j], particle); + } + } + } + } + + return total_likelihood; +} + +double MCL::calculate_object_likelihood( + const ProjectedObject & measurement, const keisan::Point2 & landmark, const Particle & particle) { + + double dx = measurement.position.x * 100; + double dy = measurement.position.y * 100; + + double x_rot = dx * particle.orientation.cos() + dy * particle.orientation.sin(); + double y_rot = dx * particle.orientation.sin() - dy * particle.orientation.cos(); + + double relative_position_x = particle.position.x + x_rot; + double relative_position_y = particle.position.y + y_rot; + + double dist = sqrt(dx * dx + dy * dy); + double dist_factor = keisan::map(dist, 150.0, 500.0, 1.0, 0.9); + + double exponent = + -0.5 * + (pow((landmark.x - relative_position_x), 2) / pow(sigma_x, 2) + + pow((landmark.y - relative_position_y), 2) / pow(sigma_y, 2)); + + return exp(exponent) / (2 * M_PI * sigma_x * sigma_y) * dist_factor; +} + +void MCL::estimate_position() { + if (best_particle == nullptr || num_projected_objects == 0) { + return; + } + + keisan::Point2 sum_position = {0.0, 0.0}; + int centered_particles = 0; + center_particles.clear(); + + // Find numbers of particles closed to the best particle + for (int i = 0; i < num_particles; ++i) { + double distance = calculate_distance(particles[i].position, best_particle->position); + + if (distance < 25.0) { + centered_particles++; + sum_position.x += particles[i].position.x; + sum_position.y += particles[i].position.y; + + center_particles.push_back(&particles[i]); + } + } + + // Use mean of centered particles position if more than threshold are centered + if (centered_particles > min_centered_particles_ratio * num_particles) { + auto last_estimated_position = estimated_position; + estimated_position.x = sum_position.x / centered_particles; + estimated_position.y = sum_position.y / centered_particles; + + estimated_position.x = keisan::clamp(estimated_position.x, 0.0, 900.0); + estimated_position.y = keisan::clamp(estimated_position.y, 0.0, 600.0); + + double jump_distance = calculate_distance(estimated_position, position); + + if (jump_distance <= 100.0 || initial_localization) { + position = estimated_position; + apply_localization = true; + if (initial_localization) { + initial_localization = false; + } + } else { + double delta_estimate = calculate_distance(pending_estimated_position, estimated_position); + if (delta_estimate < 20.0) { + pending_accept_count++; + } else { + pending_accept_count = 0; + } + pending_estimated_position = estimated_position; + + if (pending_accept_count >= 10) { + position = estimated_position; + apply_localization = true; + pending_accept_count = 0; + } else { + estimated_position = last_estimated_position; + apply_localization = false; + reset_particles = true; + } + } + } +} + +void MCL::print_particles() { + if (!print_debug) { + return; + } + + printf("Particles num: %d\n", particles.size()); + printf("Prob: %.2f | Threshold: %.2f | Above threshold: %s\n", + prob, + reset_particles_threshold, + (prob > reset_particles_threshold) ? "true" : "false"); + printf("Reset particles: %s\n", reset_particles ? "true" : "false"); + printf("Short term avg: %.8f\n", short_term_avg); + printf("Long term avg: %.8f\n", long_term_avg); + printf("Last weight avg: %.8f\n", last_weight_avg); + printf("Weight avg: %.8f\n", weight_avg); + printf("Sum weights: %.8f\n", get_sum_weight()); + + printf("========================================\n"); + printf("Minimal Centered Particles: %.0f\n", min_centered_particles_ratio * num_particles); + + printf("Centered particles: %d\n", center_particles.size()); + for (int i = 0; i < center_particles.size(); ++i) { + printf("Centered Particle %d weight: %.5f [%.2f, %.2f, %.2f]\n", + i, + center_particles[i]->weight, + center_particles[i]->position.x, + center_particles[i]->position.y, + center_particles[i]->orientation.degree()); + if (i >= 10) { + printf("... and %d more\n", center_particles.size() - 10); + break; + } + } + + if (best_particle == nullptr) { + printf("Best particle: NULL\n"); + } else { + printf("Best particle: %.5f [%.2f, %.2f, %.2f]\n", + best_particle->weight, + best_particle->position.x, + best_particle->position.y, + best_particle->orientation.degree()); + } + + printf("Num objects: %d\n", num_projected_objects); + printf("Num particles: %d\n", num_particles); + printf("Projected objects: %d\n", num_projected_objects); + printf("Initial Localization %d\n", initial_localization); + print_estimate_position(); +} + +void MCL::print_estimate_position() { + printf("Pose estimation: [%.2f, %.2f])\n", estimated_position.x, estimated_position.y); +} + +} // namespace basho diff --git a/src/basho/node/basho_node.cpp b/src/basho/node/basho_node.cpp new file mode 100644 index 0000000..0091bc7 --- /dev/null +++ b/src/basho/node/basho_node.cpp @@ -0,0 +1,166 @@ +// Copyright (c) 2026 Ichiro ITS +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in +// all copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL +// THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// 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. + +#include "basho/node/basho_node.hpp" + +#include + +#include "aruku/walking/walking.hpp" +#include "kansei/measurement/measurement.hpp" + +namespace basho +{ + +std::string BashoNode::get_node_prefix() +{ + return "localization"; +} + +BashoNode::BashoNode( + const rclcpp::Node::SharedPtr & node, + const std::shared_ptr & localization, + const std::string & path +) : node(node), + localization(localization) +{ + measurement_status_subscriber = + node->create_subscription( + kansei::measurement::MeasurementNode::status_topic(), + 10, + [this](const MeasurementStatus::SharedPtr msg) { + auto mcl = this->localization->get_mcl(); + + mcl->is_calibrated = msg->is_calibrated; + mcl->orientation = + keisan::make_degree(msg->orientation.yaw); + mcl->orientation_roll = + keisan::make_degree(msg->orientation.roll); + mcl->orientation_pitch = + keisan::make_degree(msg->orientation.pitch); + }); + + delta_position_subscriber = + node->create_subscription( + aruku::WalkingNode::delta_position_topic(), + 10, + [this](const Point2::SharedPtr msg) { + auto mcl = this->localization->get_mcl(); + mcl->delta_position = {msg->x, msg->y}; + }); + + projected_objects_subscriber = + node->create_subscription( + "/gyakuenki_cpp/projected_objects", + 10, + [this](const ProjectedObjects::SharedPtr msg) { + auto mcl = this->localization->get_mcl(); + + mcl->clear_projected_objects(); + + for (const auto & obj : msg->projected_objects) { + const bool ignore = + obj.label == "ball" || + obj.label == "mcl" || + obj.label == "self" || + obj.position.x * 100 > mcl->max_object_distance.x || + std::abs(obj.position.y * 100) > mcl->max_object_distance.y; + + if (ignore) { + continue; + } + + ProjectedObject projected{ + obj.label, + {obj.position.x, obj.position.y, obj.position.z} + }; + + if (obj.label == "X-Intersection") { + mcl->projected_X.push_back(projected); + } else if (obj.label == "L-Intersection") { + mcl->projected_L.push_back(projected); + } else if (obj.label == "T-Intersection") { + mcl->projected_T.push_back(projected); + } else if (obj.label == "goalpost") { + mcl->projected_goalpost.push_back(projected); + } + + mcl->num_projected_objects++; + } + }); + + set_odometry_publisher = + node->create_publisher(aruku::WalkingNode::set_odometry_topic(), 10); + + particles_publisher = + node->create_publisher( + get_node_prefix() + "/particles", 10); +} + +void BashoNode::update() +{ + localization->update(); + publish_particles(); + + auto mcl = localization->get_mcl(); + if (mcl->apply_localization) { + publish_odometry(); + mcl->apply_localization = false; + } +} + +void BashoNode::publish_odometry() +{ + auto mcl = localization->get_mcl(); + + Point2 msg; + msg.x = mcl->position.x; + msg.y = mcl->position.y; + + set_odometry_publisher->publish(msg); +} + +void BashoNode::publish_particles() +{ + auto mcl = localization->get_mcl(); + + Particles msg; + double max_weight = 0.0; + + for (const auto & p : mcl->particles) { + Particle particle; + particle.x = p.position.x; + particle.y = p.position.y; + particle.orientation = p.orientation.degree(); + particle.weight = p.weight; + + msg.particles.push_back(particle); + max_weight = std::max(max_weight, p.weight); + } + + msg.estimated_position.x = mcl->estimated_position.x; + msg.estimated_position.y = mcl->estimated_position.y; + msg.estimated_position.orientation = + mcl->orientation.degree(); + msg.estimated_position.weight = max_weight; + + particles_publisher->publish(msg); +} + +} // namespace basho \ No newline at end of file diff --git a/src/basho/process/localization.cpp b/src/basho/process/localization.cpp new file mode 100644 index 0000000..b6aa7fd --- /dev/null +++ b/src/basho/process/localization.cpp @@ -0,0 +1,121 @@ +// Copyright (c) 2026 Ichiro ITS +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in +// all copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL +// THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// 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. + +#include +#include +#include +#include +#include +#include + +#include "basho/process/localization.hpp" + +#include "keisan/keisan.hpp" +#include "jitsuyo/config.hpp" +#include "nlohmann/json.hpp" + +#include "unistd.h" // NOLINT + +using namespace keisan::literals; // NOLINT + +namespace basho +{ + +Localization::Localization(std::shared_ptr mcl) +: config_name("localization.json"), + mcl(mcl), + debug(false), + num_particles(0), + min_centered_particles_ratio(0.0), + reset_particles_threshold(0.0), + short_term_avg_ratio(0.0), + long_term_avg_ratio(0.0), + sigma_x(0.0), + sigma_y(0.0), + max_distance_x(0.0), + max_distance_y(0.0) +{ +} + +void Localization::load_config(const std::string & path) +{ + nlohmann::json data; + if (!jitsuyo::load_config(path, config_name, data)) { + throw std::runtime_error("Failed to find config file `localization.json`"); + } + + set_config(data); +} + +void Localization::set_config(const nlohmann::json & json) +{ + + bool valid_config = true; + + nlohmann::json localization_section; + if (jitsuyo::assign_val(json, "localization", localization_section)) { + bool valid_section = true; + + valid_section &= jitsuyo::assign_val(localization_section, "debug", debug); + valid_section &= jitsuyo::assign_val(localization_section, "num_particles", num_particles); + valid_section &= jitsuyo::assign_val(localization_section, "min_centered_particles_ratio", min_centered_particles_ratio); + valid_section &= jitsuyo::assign_val(localization_section, "short_term_avg_ratio", short_term_avg_ratio); + valid_section &= jitsuyo::assign_val(localization_section, "long_term_avg_ratio", long_term_avg_ratio); + valid_section &= jitsuyo::assign_val(localization_section, "reset_particles_threshold", reset_particles_threshold); + valid_section &= jitsuyo::assign_val(localization_section, "sigma_x", sigma_x); + valid_section &= jitsuyo::assign_val(localization_section, "sigma_y", sigma_y); + valid_section &= jitsuyo::assign_val(localization_section, "max_distance_x", max_distance_x); + valid_section &= jitsuyo::assign_val(localization_section, "max_distance_y", max_distance_y); + + if (!valid_section) { + std::cout << "Error found at section `localization`" << std::endl; + valid_config = false; + } + } else { + valid_config = false; + } + + if (!valid_config) { + throw std::runtime_error("Failed to load config file `localization.json`"); + } + + mcl->print_debug = debug; + mcl->num_particles = num_particles; + mcl->min_centered_particles_ratio = min_centered_particles_ratio; + mcl->reset_particles_threshold = reset_particles_threshold; + mcl->short_term_avg_ratio = short_term_avg_ratio; + mcl->long_term_avg_ratio = long_term_avg_ratio; + mcl->sigma_x = sigma_x; + mcl->sigma_y = sigma_y; + mcl->max_object_distance = {max_distance_x, max_distance_y}; +} + +void Localization::update() +{ + mcl->localize(); + mcl->print_particles(); +} + +std::shared_ptr Localization::get_mcl() const +{ + return mcl; +} + +} // namespace basho diff --git a/src/basho_main.cpp b/src/basho_main.cpp new file mode 100644 index 0000000..90cb16f --- /dev/null +++ b/src/basho_main.cpp @@ -0,0 +1,57 @@ +// Copyright (c) 2026 Ichiro ITS +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in +// all copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL +// THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// 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. + +#include + +#include "basho/basho.hpp" + +using namespace std::chrono_literals; + +int main(int argc, char ** argv) +{ + auto args = rclcpp::init_and_remove_ros_arguments(argc, argv); + + if (argc < 2) { + std::cerr << "Please specify the path!" << std::endl; + return 0; + } + + const std::string & path = args[1]; + + auto node = std::make_shared("basho_node"); + + auto localization = std::make_shared(std::make_shared()); + localization->load_config(path); + + auto basho_node = std::make_shared(node, localization, path); + + rclcpp::Rate rcl_rate(8ms); + while (rclcpp::ok()) { + rcl_rate.sleep(); + + rclcpp::spin_some(node); + + basho_node->update(); + } + + rclcpp::shutdown(); + + return 0; +} \ No newline at end of file diff --git a/src/basho_node.cpp b/src/basho_node.cpp deleted file mode 100644 index 98ef1d9..0000000 --- a/src/basho_node.cpp +++ /dev/null @@ -1,10 +0,0 @@ -#include - -int main(int argc, char ** argv) -{ - (void) argc; - (void) argv; - - printf("hello world basho package\n"); - return 0; -} From 762ea81973691828aa3b59e0b3bb1bf6d5eeb794 Mon Sep 17 00:00:00 2001 From: mbsaloka Date: Sun, 25 Jan 2026 21:19:23 +0700 Subject: [PATCH 03/33] feat: enhance prob calculation for resampling factor --- src/basho/localization/mcl.cpp | 56 +++++++++++------------------- src/basho/node/basho_node.cpp | 3 +- src/basho/process/localization.cpp | 1 - 3 files changed, 22 insertions(+), 38 deletions(-) diff --git a/src/basho/localization/mcl.cpp b/src/basho/localization/mcl.cpp index 843c460..ae35723 100644 --- a/src/basho/localization/mcl.cpp +++ b/src/basho/localization/mcl.cpp @@ -54,31 +54,34 @@ void MCL::localize() bool evaluate_particles = num_projected_objects > 0; evaluate_particles &= fabs(orientation_roll.degree()) <= 30; evaluate_particles &= fabs(orientation_pitch.degree()) <= 30; - // evaluate_particles &= fabs(get_pan().degree()) <= 90; - // evaluate_particles &= get_tilt().degree() > -50; if (evaluate_particles) { - weight_avg = 0.0; calculate_weight(); - if (weight_avg <= 0 || std::isnan(weight_avg)) { - weight_avg = last_weight_avg; - } else { - if (initial_localization) { - short_term_avg = weight_avg; - long_term_avg = weight_avg; - } - last_weight_avg = weight_avg; + + if (initial_localization) { + short_term_avg = weight_avg; + long_term_avg = weight_avg; } - if (!std::isnan(weight_avg)) { - short_term_avg += short_term_avg_ratio * (weight_avg - short_term_avg); - long_term_avg += long_term_avg_ratio * (weight_avg - long_term_avg); + short_term_avg += short_term_avg_ratio * (weight_avg - short_term_avg); + long_term_avg += long_term_avg_ratio * (weight_avg - long_term_avg); + + prob = keisan::clamp(1.0 - short_term_avg / (long_term_avg + 1e-6), 0.0, 1.0); + + // calculate effective number of particles for resampling + double sum_sq = 0.0; + for (auto &p : particles) { + sum_sq += p.weight * p.weight; } + double neff = 1.0 / (sum_sq + 1e-12); - resample_particles(); + if (neff < 0.5 * num_particles) { + resample_particles(); + } } - estimate_position(); + estimate_position(); + print_particles(); clear_projected_objects(); } @@ -149,8 +152,6 @@ void MCL::resample_particles() double beta = 0.0; double max_weight = 0.0; double sum_weight = get_sum_weight(); - prob = std::max(0.0, 1.0 - short_term_avg / long_term_avg); - // reset_particles = prob > reset_particles_threshold; // find the best particle if (sum_weight > 0) { @@ -163,22 +164,8 @@ void MCL::resample_particles() } // determine resample interval area - int interval_x[2] = {10, 890}; - int interval_y[2] = {10, 590}; - - bool resample_nearby = num_projected_objects == 1; - // resample_nearby |= fabs(get_pan().degree()) > 80; - // resample_nearby |= get_tilt().degree() < -40; - resample_nearby |= reset_particles; - - if (resample_nearby) { - interval_x[0] = position.x - 50; - interval_x[1] = position.x + 50; - interval_y[0] = position.y - 50; - interval_y[1] = position.y + 50; - reset_particles = false; - prob = std::min(0.05, prob); - } + int interval_x[2] = {0, 900}; + int interval_y[2] = {0, 600}; // resample particles std::uniform_real_distribution rand_prob(0.0, 1.0); @@ -488,7 +475,6 @@ void MCL::print_particles() { printf("Num objects: %d\n", num_projected_objects); printf("Num particles: %d\n", num_particles); - printf("Projected objects: %d\n", num_projected_objects); printf("Initial Localization %d\n", initial_localization); print_estimate_position(); } diff --git a/src/basho/node/basho_node.cpp b/src/basho/node/basho_node.cpp index 0091bc7..4f0abf2 100644 --- a/src/basho/node/basho_node.cpp +++ b/src/basho/node/basho_node.cpp @@ -109,8 +109,7 @@ BashoNode::BashoNode( node->create_publisher(aruku::WalkingNode::set_odometry_topic(), 10); particles_publisher = - node->create_publisher( - get_node_prefix() + "/particles", 10); + node->create_publisher(get_node_prefix() + "/particles", 10); } void BashoNode::update() diff --git a/src/basho/process/localization.cpp b/src/basho/process/localization.cpp index b6aa7fd..f1531df 100644 --- a/src/basho/process/localization.cpp +++ b/src/basho/process/localization.cpp @@ -110,7 +110,6 @@ void Localization::set_config(const nlohmann::json & json) void Localization::update() { mcl->localize(); - mcl->print_particles(); } std::shared_ptr Localization::get_mcl() const From 326f6bb4ac2a5d7dd40ddbd1aee375c416700875 Mon Sep 17 00:00:00 2001 From: mbsaloka Date: Sun, 25 Jan 2026 22:06:57 +0700 Subject: [PATCH 04/33] fix: likelihood calculation and use mahalanobis for measurement cost --- include/basho/localization/mcl.hpp | 5 +- include/basho/process/localization.hpp | 5 +- src/basho/localization/mcl.cpp | 94 +++++++++++++++++--------- src/basho/process/localization.cpp | 15 ++-- 4 files changed, 76 insertions(+), 43 deletions(-) diff --git a/include/basho/localization/mcl.hpp b/include/basho/localization/mcl.hpp index 55d9be8..b520f33 100644 --- a/include/basho/localization/mcl.hpp +++ b/include/basho/localization/mcl.hpp @@ -85,8 +85,9 @@ class MCL double reset_particles_threshold; double short_term_avg_ratio; double long_term_avg_ratio; - double sigma_x; - double sigma_y; + double base_sigma; + double range_sigma; + double chi_square_gate; bool apply_localization; bool print_debug; diff --git a/include/basho/process/localization.hpp b/include/basho/process/localization.hpp index 1f8bdc9..870ac46 100644 --- a/include/basho/process/localization.hpp +++ b/include/basho/process/localization.hpp @@ -55,8 +55,9 @@ class Localization double short_term_avg_ratio; double long_term_avg_ratio; - double sigma_x; - double sigma_y; + double base_sigma; + double range_sigma; + double chi_square_gate; double max_distance_x; double max_distance_y; diff --git a/src/basho/localization/mcl.cpp b/src/basho/localization/mcl.cpp index ae35723..6ebadc6 100644 --- a/src/basho/localization/mcl.cpp +++ b/src/basho/localization/mcl.cpp @@ -34,11 +34,12 @@ MCL::MCL() : position(0.0, 0.0), orientation(0_deg), orientation_roll(0_deg), orientation_pitch(0_deg), is_calibrated(false), apply_localization(false), print_debug(false), num_particles(500), min_centered_particles_ratio(0.3), reset_particles_threshold(0.3), - short_term_avg_ratio(0.1), long_term_avg_ratio(0.001), sigma_x(5.0), sigma_y(5.0), - xvar(10.0), yvar(10.0), best_particle(nullptr), rand_gen(std::random_device{}()), - weight_avg(0.0), short_term_avg(0.0), long_term_avg(0.0), last_weight_avg(0.0), - estimated_position(-1.0, -1.0), initial_localization(true), reset_particles(false), - num_projected_objects(0), max_object_distance(300.0, 300.0), pending_accept_count(0) + short_term_avg_ratio(0.1), long_term_avg_ratio(0.001), base_sigma(10.0), range_sigma(0.05), + chi_square_gate(9.21034), xvar(10.0), yvar(10.0), best_particle(nullptr), + rand_gen(std::random_device{}()), weight_avg(0.0), short_term_avg(0.0), + long_term_avg(0.0), last_weight_avg(0.0), estimated_position(-1.0, -1.0), + initial_localization(true), reset_particles(false), num_projected_objects(0), + max_object_distance(300.0, 300.0), pending_accept_count(0) { } @@ -75,7 +76,7 @@ void MCL::localize() } double neff = 1.0 / (sum_sq + 1e-12); - if (neff < 0.5 * num_particles) { + if (neff < 0.75 * num_particles) { resample_particles(); } } @@ -281,7 +282,8 @@ void MCL::calculate_weight() } } -double MCL::calculate_distance(const keisan::Point2 & point_1, const keisan::Point2 & point_2) { +double MCL::calculate_distance(const keisan::Point2 & point_1, const keisan::Point2 & point_2) +{ double dx = point_1.x - point_2.x; double dy = point_1.y - point_2.y; @@ -289,34 +291,51 @@ double MCL::calculate_distance(const keisan::Point2 & point_1, const keisan::Poi } keisan::Matrix<6, 6> MCL::calculate_cost_matrix( - const Particle & particle, const std::vector & projected_objects, - const std::vector & landmarks) { + const Particle & particle, + const std::vector & projected_objects, + const std::vector & landmarks) +{ auto cost_matrix = keisan::Matrix<6, 6>::infinite(); + int num_projected_objects = projected_objects.size(); int num_landmarks = landmarks.size(); for (int i = 0; i < num_projected_objects; ++i) { - for (int j = 0; j < num_landmarks; ++j) { - double dx = projected_objects[i].position.x * 100; - double dy = projected_objects[i].position.y * 100; + double dx = projected_objects[i].position.x * 100; + double dy = projected_objects[i].position.y * 100; - double x_rot = dx * particle.orientation.cos() + dy * particle.orientation.sin(); - double y_rot = dx * particle.orientation.sin() - dy * particle.orientation.cos(); + double x_rot = dx * particle.orientation.cos() + dy * particle.orientation.sin(); + double y_rot = dx * particle.orientation.sin() - dy * particle.orientation.cos(); - double relative_position_x = particle.position.x + x_rot; - double relative_position_y = particle.position.y + y_rot; + double px = particle.position.x + x_rot; + double py = particle.position.y + y_rot; - keisan::Point2 projected_object(relative_position_x, relative_position_y); + double dist = std::sqrt(dx*dx + dy*dy); + double sigma = base_sigma + range_sigma * dist; + double var = sigma * sigma; - cost_matrix[i][j] = calculate_distance(landmarks[j], projected_object); + for (int j = 0; j < num_landmarks; ++j) { + double ex = landmarks[j].x - px; + double ey = landmarks[j].y - py; + + // Squared Mahalanobis distance + double d2 = (ex*ex + ey*ey) / var; + + // Chi-square gating to reject unlikely associations + if (d2 < chi_square_gate) { + cost_matrix[i][j] = d2; + } else { + cost_matrix[i][j] = 1e9; + } } } return cost_matrix; } -double MCL::calculate_total_likelihood(const Particle & particle) { - double total_likelihood = 1.0; +double MCL::calculate_total_likelihood(const Particle & particle) +{ + double log_total = 0.0; std::vector landmark_groups = { {&projected_X, &field.landmarks_X}, @@ -336,36 +355,45 @@ double MCL::calculate_total_likelihood(const Particle & particle) { for (int i = 0; i < group.projected->size(); ++i) { for (int j = 0; j < group.landmarks->size(); ++j) { if (result[i][j] == 1) { - total_likelihood *= calculate_object_likelihood((*group.projected)[i], (*group.landmarks)[j], particle); + double likelihood = + calculate_object_likelihood((*group.projected)[i], (*group.landmarks)[j], particle); + log_total += std::log(std::max(likelihood, 1e-9)); } } } } - return total_likelihood; + return std::exp(log_total); } double MCL::calculate_object_likelihood( - const ProjectedObject & measurement, const keisan::Point2 & landmark, const Particle & particle) { - + const ProjectedObject & measurement, + const keisan::Point2 & landmark, + const Particle & particle) +{ double dx = measurement.position.x * 100; double dy = measurement.position.y * 100; double x_rot = dx * particle.orientation.cos() + dy * particle.orientation.sin(); double y_rot = dx * particle.orientation.sin() - dy * particle.orientation.cos(); - double relative_position_x = particle.position.x + x_rot; - double relative_position_y = particle.position.y + y_rot; + double px = particle.position.x + x_rot; + double py = particle.position.y + y_rot; - double dist = sqrt(dx * dx + dy * dy); - double dist_factor = keisan::map(dist, 150.0, 500.0, 1.0, 0.9); + double ex = landmark.x - px; + double ey = landmark.y - py; - double exponent = - -0.5 * - (pow((landmark.x - relative_position_x), 2) / pow(sigma_x, 2) + - pow((landmark.y - relative_position_y), 2) / pow(sigma_y, 2)); + double dist = std::sqrt(dx*dx + dy*dy); + double sigma = base_sigma + range_sigma * dist; + double var = sigma * sigma; + + double d2 = (ex*ex + ey*ey) / var; + + if (d2 > chi_square_gate) { + return 1e-9; + } - return exp(exponent) / (2 * M_PI * sigma_x * sigma_y) * dist_factor; + return std::exp(-0.5 * d2); } void MCL::estimate_position() { diff --git a/src/basho/process/localization.cpp b/src/basho/process/localization.cpp index f1531df..aebe06d 100644 --- a/src/basho/process/localization.cpp +++ b/src/basho/process/localization.cpp @@ -47,8 +47,9 @@ Localization::Localization(std::shared_ptr mcl) reset_particles_threshold(0.0), short_term_avg_ratio(0.0), long_term_avg_ratio(0.0), - sigma_x(0.0), - sigma_y(0.0), + base_sigma(0.0), + range_sigma(0.0), + chi_square_gate(0.0), max_distance_x(0.0), max_distance_y(0.0) { @@ -79,8 +80,9 @@ void Localization::set_config(const nlohmann::json & json) valid_section &= jitsuyo::assign_val(localization_section, "short_term_avg_ratio", short_term_avg_ratio); valid_section &= jitsuyo::assign_val(localization_section, "long_term_avg_ratio", long_term_avg_ratio); valid_section &= jitsuyo::assign_val(localization_section, "reset_particles_threshold", reset_particles_threshold); - valid_section &= jitsuyo::assign_val(localization_section, "sigma_x", sigma_x); - valid_section &= jitsuyo::assign_val(localization_section, "sigma_y", sigma_y); + valid_section &= jitsuyo::assign_val(localization_section, "base_sigma", base_sigma); + valid_section &= jitsuyo::assign_val(localization_section, "range_sigma", range_sigma); + valid_section &= jitsuyo::assign_val(localization_section, "chi_square_gate", chi_square_gate); valid_section &= jitsuyo::assign_val(localization_section, "max_distance_x", max_distance_x); valid_section &= jitsuyo::assign_val(localization_section, "max_distance_y", max_distance_y); @@ -102,8 +104,9 @@ void Localization::set_config(const nlohmann::json & json) mcl->reset_particles_threshold = reset_particles_threshold; mcl->short_term_avg_ratio = short_term_avg_ratio; mcl->long_term_avg_ratio = long_term_avg_ratio; - mcl->sigma_x = sigma_x; - mcl->sigma_y = sigma_y; + mcl->base_sigma = base_sigma; + mcl->range_sigma = range_sigma; + mcl->chi_square_gate = chi_square_gate; mcl->max_object_distance = {max_distance_x, max_distance_y}; } From 0bee80f801d3d142615d6d7518777c49a1a8ed67 Mon Sep 17 00:00:00 2001 From: mbsaloka Date: Sun, 25 Jan 2026 22:12:35 +0700 Subject: [PATCH 05/33] fix: false cost matrix size --- include/basho/localization/mcl.hpp | 4 ++-- src/basho/localization/mcl.cpp | 4 ++-- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/include/basho/localization/mcl.hpp b/include/basho/localization/mcl.hpp index b520f33..9736a40 100644 --- a/include/basho/localization/mcl.hpp +++ b/include/basho/localization/mcl.hpp @@ -73,7 +73,7 @@ class MCL const Particle & particle); double get_sum_weight(); - keisan::Matrix<6, 6> calculate_cost_matrix(const Particle & particle, + keisan::Matrix<8, 8> calculate_cost_matrix(const Particle & particle, const std::vector & projected_objects, const std::vector & landmarks); @@ -117,7 +117,7 @@ class MCL double yvar; std::mt19937 rand_gen; - keisan::Hungarian<6> hungarian; + keisan::Hungarian<8> hungarian; double weight_avg; double short_term_avg; diff --git a/src/basho/localization/mcl.cpp b/src/basho/localization/mcl.cpp index 6ebadc6..2e5857f 100644 --- a/src/basho/localization/mcl.cpp +++ b/src/basho/localization/mcl.cpp @@ -290,12 +290,12 @@ double MCL::calculate_distance(const keisan::Point2 & point_1, const keisan::Poi return sqrt(dx * dx + dy * dy); } -keisan::Matrix<6, 6> MCL::calculate_cost_matrix( +keisan::Matrix<8, 8> MCL::calculate_cost_matrix( const Particle & particle, const std::vector & projected_objects, const std::vector & landmarks) { - auto cost_matrix = keisan::Matrix<6, 6>::infinite(); + auto cost_matrix = keisan::Matrix<8, 8>::infinite(); int num_projected_objects = projected_objects.size(); int num_landmarks = landmarks.size(); From 0f4cb6785031ceab9a186de9a4e5f00bc72f2232 Mon Sep 17 00:00:00 2001 From: mbsaloka Date: Sun, 25 Jan 2026 22:34:40 +0700 Subject: [PATCH 06/33] refactor: remove unused attribute and method --- include/basho/localization/mcl.hpp | 7 +--- include/basho/process/localization.hpp | 17 -------- src/basho/localization/mcl.cpp | 54 ++++++-------------------- src/basho/node/basho_node.cpp | 1 - src/basho/process/localization.cpp | 47 +++++++--------------- 5 files changed, 26 insertions(+), 100 deletions(-) diff --git a/include/basho/localization/mcl.hpp b/include/basho/localization/mcl.hpp index 9736a40..f3cbd43 100644 --- a/include/basho/localization/mcl.hpp +++ b/include/basho/localization/mcl.hpp @@ -55,7 +55,6 @@ class MCL MCL(); void localize(); - void reset_localization(); void clear_projected_objects(); void init_particles(const keisan::Point2 init_position); void resample_particles(); @@ -63,7 +62,6 @@ class MCL void calculate_weight(); void estimate_position(); void print_particles(); - void print_estimate_position(); void set_initial_localization(bool initial) { initial_localization = initial; } double calculate_distance(const keisan::Point2 & point_1, const keisan::Point2 & point_2); @@ -81,8 +79,7 @@ class MCL keisan::Point2 estimated_position; int num_particles; - double min_centered_particles_ratio; - double reset_particles_threshold; + double min_centered_ratio; double short_term_avg_ratio; double long_term_avg_ratio; double base_sigma; @@ -101,7 +98,6 @@ class MCL int num_projected_objects; // member for getting - bool is_calibrated; keisan::Angle orientation; keisan::Angle orientation_roll; keisan::Angle orientation_pitch; @@ -126,7 +122,6 @@ class MCL double prob; bool initial_localization; - bool reset_particles; int pending_accept_count; keisan::Point2 pending_estimated_position; diff --git a/include/basho/process/localization.hpp b/include/basho/process/localization.hpp index 870ac46..3daa3bb 100644 --- a/include/basho/process/localization.hpp +++ b/include/basho/process/localization.hpp @@ -45,23 +45,6 @@ class Localization std::string config_name; private: - bool debug; - - int num_particles; - - double min_centered_particles_ratio; - double reset_particles_threshold; - - double short_term_avg_ratio; - double long_term_avg_ratio; - - double base_sigma; - double range_sigma; - double chi_square_gate; - - double max_distance_x; - double max_distance_y; - std::shared_ptr mcl; }; diff --git a/src/basho/localization/mcl.cpp b/src/basho/localization/mcl.cpp index 2e5857f..830f5e1 100644 --- a/src/basho/localization/mcl.cpp +++ b/src/basho/localization/mcl.cpp @@ -32,18 +32,15 @@ namespace basho MCL::MCL() : position(0.0, 0.0), orientation(0_deg), orientation_roll(0_deg), orientation_pitch(0_deg), - is_calibrated(false), apply_localization(false), print_debug(false), - num_particles(500), min_centered_particles_ratio(0.3), reset_particles_threshold(0.3), - short_term_avg_ratio(0.1), long_term_avg_ratio(0.001), base_sigma(10.0), range_sigma(0.05), - chi_square_gate(9.21034), xvar(10.0), yvar(10.0), best_particle(nullptr), - rand_gen(std::random_device{}()), weight_avg(0.0), short_term_avg(0.0), - long_term_avg(0.0), last_weight_avg(0.0), estimated_position(-1.0, -1.0), - initial_localization(true), reset_particles(false), num_projected_objects(0), + initial_localization(true), apply_localization(false), print_debug(false), + num_particles(500), min_centered_ratio(0.3), short_term_avg_ratio(0.1), long_term_avg_ratio(0.001), + base_sigma(10.0), range_sigma(0.05), chi_square_gate(9.21034), xvar(10.0), yvar(10.0), + best_particle(nullptr), rand_gen(std::random_device{}()), weight_avg(0.0), last_weight_avg(0.0), + short_term_avg(0.0), long_term_avg(0.0), estimated_position(-1.0, -1.0), num_projected_objects(0), max_object_distance(300.0, 300.0), pending_accept_count(0) { } - void MCL::localize() { if (initial_localization) { @@ -86,26 +83,6 @@ void MCL::localize() clear_projected_objects(); } -void MCL::reset_localization() { - apply_localization = true; - initial_localization = true; - particles.clear(); - center_particles.clear(); - clear_projected_objects(); - estimated_position = keisan::Point2(-1.0, -1.0); - best_particle = nullptr; - weight_avg = 0.0; - short_term_avg = 0.0; - long_term_avg = 0.0; - last_weight_avg = 0.0; - reset_particles = false; - prob = 0.0; - pending_accept_count = 0; - pending_estimated_position = keisan::Point2(-1.0, -1.0); - - localize(); -} - void MCL::clear_projected_objects() { projected_X.clear(); @@ -419,7 +396,7 @@ void MCL::estimate_position() { } // Use mean of centered particles position if more than threshold are centered - if (centered_particles > min_centered_particles_ratio * num_particles) { + if (centered_particles > min_centered_ratio * num_particles) { auto last_estimated_position = estimated_position; estimated_position.x = sum_position.x / centered_particles; estimated_position.y = sum_position.y / centered_particles; @@ -451,7 +428,6 @@ void MCL::estimate_position() { } else { estimated_position = last_estimated_position; apply_localization = false; - reset_particles = true; } } } @@ -462,20 +438,16 @@ void MCL::print_particles() { return; } + printf("========================================\n"); printf("Particles num: %d\n", particles.size()); - printf("Prob: %.2f | Threshold: %.2f | Above threshold: %s\n", - prob, - reset_particles_threshold, - (prob > reset_particles_threshold) ? "true" : "false"); - printf("Reset particles: %s\n", reset_particles ? "true" : "false"); + printf("Prob: %.2f", prob); printf("Short term avg: %.8f\n", short_term_avg); printf("Long term avg: %.8f\n", long_term_avg); printf("Last weight avg: %.8f\n", last_weight_avg); printf("Weight avg: %.8f\n", weight_avg); printf("Sum weights: %.8f\n", get_sum_weight()); - printf("========================================\n"); - printf("Minimal Centered Particles: %.0f\n", min_centered_particles_ratio * num_particles); + printf("Minimal Centered Particles: %.0f\n", min_centered_ratio * num_particles); printf("Centered particles: %d\n", center_particles.size()); for (int i = 0; i < center_particles.size(); ++i) { @@ -485,8 +457,8 @@ void MCL::print_particles() { center_particles[i]->position.x, center_particles[i]->position.y, center_particles[i]->orientation.degree()); - if (i >= 10) { - printf("... and %d more\n", center_particles.size() - 10); + if (i >= 5) { + printf("... and %d more\n", center_particles.size() - 5); break; } } @@ -504,10 +476,6 @@ void MCL::print_particles() { printf("Num objects: %d\n", num_projected_objects); printf("Num particles: %d\n", num_particles); printf("Initial Localization %d\n", initial_localization); - print_estimate_position(); -} - -void MCL::print_estimate_position() { printf("Pose estimation: [%.2f, %.2f])\n", estimated_position.x, estimated_position.y); } diff --git a/src/basho/node/basho_node.cpp b/src/basho/node/basho_node.cpp index 4f0abf2..7438d54 100644 --- a/src/basho/node/basho_node.cpp +++ b/src/basho/node/basho_node.cpp @@ -47,7 +47,6 @@ BashoNode::BashoNode( [this](const MeasurementStatus::SharedPtr msg) { auto mcl = this->localization->get_mcl(); - mcl->is_calibrated = msg->is_calibrated; mcl->orientation = keisan::make_degree(msg->orientation.yaw); mcl->orientation_roll = diff --git a/src/basho/process/localization.cpp b/src/basho/process/localization.cpp index aebe06d..50aec90 100644 --- a/src/basho/process/localization.cpp +++ b/src/basho/process/localization.cpp @@ -39,19 +39,7 @@ namespace basho { Localization::Localization(std::shared_ptr mcl) -: config_name("localization.json"), - mcl(mcl), - debug(false), - num_particles(0), - min_centered_particles_ratio(0.0), - reset_particles_threshold(0.0), - short_term_avg_ratio(0.0), - long_term_avg_ratio(0.0), - base_sigma(0.0), - range_sigma(0.0), - chi_square_gate(0.0), - max_distance_x(0.0), - max_distance_y(0.0) +: config_name("localization.json"), mcl(mcl) { } @@ -74,18 +62,22 @@ void Localization::set_config(const nlohmann::json & json) if (jitsuyo::assign_val(json, "localization", localization_section)) { bool valid_section = true; - valid_section &= jitsuyo::assign_val(localization_section, "debug", debug); - valid_section &= jitsuyo::assign_val(localization_section, "num_particles", num_particles); - valid_section &= jitsuyo::assign_val(localization_section, "min_centered_particles_ratio", min_centered_particles_ratio); - valid_section &= jitsuyo::assign_val(localization_section, "short_term_avg_ratio", short_term_avg_ratio); - valid_section &= jitsuyo::assign_val(localization_section, "long_term_avg_ratio", long_term_avg_ratio); - valid_section &= jitsuyo::assign_val(localization_section, "reset_particles_threshold", reset_particles_threshold); - valid_section &= jitsuyo::assign_val(localization_section, "base_sigma", base_sigma); - valid_section &= jitsuyo::assign_val(localization_section, "range_sigma", range_sigma); - valid_section &= jitsuyo::assign_val(localization_section, "chi_square_gate", chi_square_gate); + double max_distance_x = 0.0; + double max_distance_y = 0.0; + + valid_section &= jitsuyo::assign_val(localization_section, "debug", mcl->print_debug); + valid_section &= jitsuyo::assign_val(localization_section, "num_particles", mcl->num_particles); + valid_section &= jitsuyo::assign_val(localization_section, "min_centered_ratio", mcl->min_centered_ratio); + valid_section &= jitsuyo::assign_val(localization_section, "short_term_avg_ratio", mcl->short_term_avg_ratio); + valid_section &= jitsuyo::assign_val(localization_section, "long_term_avg_ratio", mcl->long_term_avg_ratio); + valid_section &= jitsuyo::assign_val(localization_section, "base_sigma", mcl->base_sigma); + valid_section &= jitsuyo::assign_val(localization_section, "range_sigma", mcl->range_sigma); + valid_section &= jitsuyo::assign_val(localization_section, "chi_square_gate", mcl->chi_square_gate); valid_section &= jitsuyo::assign_val(localization_section, "max_distance_x", max_distance_x); valid_section &= jitsuyo::assign_val(localization_section, "max_distance_y", max_distance_y); + mcl->max_object_distance = {max_distance_x, max_distance_y}; + if (!valid_section) { std::cout << "Error found at section `localization`" << std::endl; valid_config = false; @@ -97,17 +89,6 @@ void Localization::set_config(const nlohmann::json & json) if (!valid_config) { throw std::runtime_error("Failed to load config file `localization.json`"); } - - mcl->print_debug = debug; - mcl->num_particles = num_particles; - mcl->min_centered_particles_ratio = min_centered_particles_ratio; - mcl->reset_particles_threshold = reset_particles_threshold; - mcl->short_term_avg_ratio = short_term_avg_ratio; - mcl->long_term_avg_ratio = long_term_avg_ratio; - mcl->base_sigma = base_sigma; - mcl->range_sigma = range_sigma; - mcl->chi_square_gate = chi_square_gate; - mcl->max_object_distance = {max_distance_x, max_distance_y}; } void Localization::update() From 457fd829eaa536ed708e7439b4a73e0ad4d1a0c9 Mon Sep 17 00:00:00 2001 From: mbsaloka Date: Sun, 25 Jan 2026 22:37:05 +0700 Subject: [PATCH 07/33] fix: always apply noise for update motion --- src/basho/localization/mcl.cpp | 44 +++++++++++----------------------- 1 file changed, 14 insertions(+), 30 deletions(-) diff --git a/src/basho/localization/mcl.cpp b/src/basho/localization/mcl.cpp index 830f5e1..d5eee05 100644 --- a/src/basho/localization/mcl.cpp +++ b/src/basho/localization/mcl.cpp @@ -195,37 +195,21 @@ void MCL::resample_particles() void MCL::update_motion() { static std::normal_distribution xgen(0.0, xvar), ygen(0.0, yvar); - bool update_with_noise = num_projected_objects > 0; - update_with_noise &= fabs(orientation_roll.degree()) <= 30; - update_with_noise &= fabs(orientation_pitch.degree()) <= 30; - // update_with_noise &= fabs(get_pan().degree()) <= 90; - // update_with_noise &= get_tilt().degree() > -50; - if (update_with_noise || true) { - for (auto & p : particles) { - double static_noise_x = xgen(rand_gen) / 10.0; - double static_noise_y = ygen(rand_gen) / 10.0; - double dynamic_noise_x = fabs(delta_position.x) * xgen(rand_gen) / 5.0; - double dynamic_noise_y = fabs(delta_position.y) * ygen(rand_gen) / 5.0; - double x_yterm = fabs(delta_position.y) * xgen(rand_gen) / 30.0; - double y_xterm = fabs(delta_position.x) * ygen(rand_gen) / 30.0; - - p.position.x += delta_position.x + static_noise_x + dynamic_noise_x + x_yterm; - p.position.y += delta_position.y + static_noise_y + dynamic_noise_y + y_xterm; - p.orientation = orientation; - - p.position.x = keisan::clamp(p.position.x, 0.0, 900.0); - p.position.y = keisan::clamp(p.position.y, 0.0, 600.0); - } - } else { - for (auto & p : particles) { - p.position.x += delta_position.x; - p.position.y += delta_position.y; - p.orientation = orientation; - - p.position.x = keisan::clamp(p.position.x, 0.0, 900.0); - p.position.y = keisan::clamp(p.position.y, 0.0, 600.0); - } + for (auto & p : particles) { + double static_noise_x = xgen(rand_gen) / 10.0; + double static_noise_y = ygen(rand_gen) / 10.0; + double dynamic_noise_x = fabs(delta_position.x) * xgen(rand_gen) / 5.0; + double dynamic_noise_y = fabs(delta_position.y) * ygen(rand_gen) / 5.0; + double x_yterm = fabs(delta_position.y) * xgen(rand_gen) / 30.0; + double y_xterm = fabs(delta_position.x) * ygen(rand_gen) / 30.0; + + p.position.x += delta_position.x + static_noise_x + dynamic_noise_x + x_yterm; + p.position.y += delta_position.y + static_noise_y + dynamic_noise_y + y_xterm; + p.orientation = orientation; + + p.position.x = keisan::clamp(p.position.x, 0.0, 900.0); + p.position.y = keisan::clamp(p.position.y, 0.0, 600.0); } estimated_position += delta_position; From 06de9b9348f93503694e048680d3577dae5097a4 Mon Sep 17 00:00:00 2001 From: mbsaloka Date: Sun, 25 Jan 2026 23:14:44 +0700 Subject: [PATCH 08/33] feat: use weighted mean for estimate position --- src/basho/localization/mcl.cpp | 16 +++++++++------- 1 file changed, 9 insertions(+), 7 deletions(-) diff --git a/src/basho/localization/mcl.cpp b/src/basho/localization/mcl.cpp index d5eee05..ecd6ab8 100644 --- a/src/basho/localization/mcl.cpp +++ b/src/basho/localization/mcl.cpp @@ -362,7 +362,8 @@ void MCL::estimate_position() { return; } - keisan::Point2 sum_position = {0.0, 0.0}; + keisan::Point2 weighted_sum = {0.0, 0.0}; + double weight_sum = 0.0; int centered_particles = 0; center_particles.clear(); @@ -370,10 +371,11 @@ void MCL::estimate_position() { for (int i = 0; i < num_particles; ++i) { double distance = calculate_distance(particles[i].position, best_particle->position); - if (distance < 25.0) { + if (distance < 30.0) { centered_particles++; - sum_position.x += particles[i].position.x; - sum_position.y += particles[i].position.y; + weighted_sum.x += particles[i].weight * particles[i].position.x; + weighted_sum.y += particles[i].weight * particles[i].position.y; + weight_sum += particles[i].weight; center_particles.push_back(&particles[i]); } @@ -382,13 +384,13 @@ void MCL::estimate_position() { // Use mean of centered particles position if more than threshold are centered if (centered_particles > min_centered_ratio * num_particles) { auto last_estimated_position = estimated_position; - estimated_position.x = sum_position.x / centered_particles; - estimated_position.y = sum_position.y / centered_particles; + estimated_position.x = weighted_sum.x / weight_sum; + estimated_position.y = weighted_sum.y / weight_sum; estimated_position.x = keisan::clamp(estimated_position.x, 0.0, 900.0); estimated_position.y = keisan::clamp(estimated_position.y, 0.0, 600.0); - double jump_distance = calculate_distance(estimated_position, position); + double jump_distance = calculate_distance(estimated_position, last_estimated_position); if (jump_distance <= 100.0 || initial_localization) { position = estimated_position; From 164bdf54c4576737f1a5ce4b9a171a24ad91a190 Mon Sep 17 00:00:00 2001 From: mbsaloka Date: Sun, 25 Jan 2026 23:25:36 +0700 Subject: [PATCH 09/33] fix: minor typo --- src/basho/node/basho_node.cpp | 7 +++---- 1 file changed, 3 insertions(+), 4 deletions(-) diff --git a/src/basho/node/basho_node.cpp b/src/basho/node/basho_node.cpp index 7438d54..760f6ef 100644 --- a/src/basho/node/basho_node.cpp +++ b/src/basho/node/basho_node.cpp @@ -76,9 +76,9 @@ BashoNode::BashoNode( for (const auto & obj : msg->projected_objects) { const bool ignore = obj.label == "ball" || - obj.label == "mcl" || + obj.label == "robot" || obj.label == "self" || - obj.position.x * 100 > mcl->max_object_distance.x || + std::abs(obj.position.x * 100) > mcl->max_object_distance.x || std::abs(obj.position.y * 100) > mcl->max_object_distance.y; if (ignore) { @@ -154,8 +154,7 @@ void BashoNode::publish_particles() msg.estimated_position.x = mcl->estimated_position.x; msg.estimated_position.y = mcl->estimated_position.y; - msg.estimated_position.orientation = - mcl->orientation.degree(); + msg.estimated_position.orientation = mcl->orientation.degree(); msg.estimated_position.weight = max_weight; particles_publisher->publish(msg); From f0ac34eea78bbbdafff0ac4abbcdda354b05b295 Mon Sep 17 00:00:00 2001 From: mbsaloka Date: Sun, 25 Jan 2026 23:25:52 +0700 Subject: [PATCH 10/33] docs: add readme --- README.md | 2 +- package.xml | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/README.md b/README.md index b541d6f..1cbad6e 100644 --- a/README.md +++ b/README.md @@ -1,2 +1,2 @@ # basho -Localization Package for ROS 2 Project. +Localization (MCL) Package for ROS 2 Project. diff --git a/package.xml b/package.xml index a8ddddb..27e2e3d 100644 --- a/package.xml +++ b/package.xml @@ -3,7 +3,7 @@ basho 0.0.0 - Localization Package for ROS 2 Project + Localization (MCL) Package for ROS 2 Project mbsaloka MIT License ament_cmake From ea2297b0999999f7481ab3cb7212de23bf1ea3c8 Mon Sep 17 00:00:00 2001 From: mbsaloka Date: Mon, 26 Jan 2026 23:34:00 +0700 Subject: [PATCH 11/33] feat: use pose covariance for apply estimate pose --- include/basho/localization/mcl.hpp | 6 +++++ src/basho/localization/mcl.cpp | 38 +++++++++++++++++++++++++++--- src/basho/process/localization.cpp | 5 ++-- 3 files changed, 44 insertions(+), 5 deletions(-) diff --git a/include/basho/localization/mcl.hpp b/include/basho/localization/mcl.hpp index f3cbd43..43e2ec9 100644 --- a/include/basho/localization/mcl.hpp +++ b/include/basho/localization/mcl.hpp @@ -75,6 +75,9 @@ class MCL const std::vector & projected_objects, const std::vector & landmarks); + void compute_pose_covariance(); + double pose_uncertainty() const; + std::vector particles; keisan::Point2 estimated_position; @@ -85,6 +88,7 @@ class MCL double base_sigma; double range_sigma; double chi_square_gate; + double uncertainty_threshold; bool apply_localization; bool print_debug; @@ -125,6 +129,8 @@ class MCL int pending_accept_count; keisan::Point2 pending_estimated_position; + + keisan::Matrix<2,2> pose_cov; }; } // namespace basho diff --git a/src/basho/localization/mcl.cpp b/src/basho/localization/mcl.cpp index ecd6ab8..e7b0a36 100644 --- a/src/basho/localization/mcl.cpp +++ b/src/basho/localization/mcl.cpp @@ -37,7 +37,7 @@ MCL::MCL() base_sigma(10.0), range_sigma(0.05), chi_square_gate(9.21034), xvar(10.0), yvar(10.0), best_particle(nullptr), rand_gen(std::random_device{}()), weight_avg(0.0), last_weight_avg(0.0), short_term_avg(0.0), long_term_avg(0.0), estimated_position(-1.0, -1.0), num_projected_objects(0), - max_object_distance(300.0, 300.0), pending_accept_count(0) + max_object_distance(300.0, 300.0), pending_accept_count(0), uncertainty_threshold(50.0) { } @@ -327,6 +327,35 @@ double MCL::calculate_total_likelihood(const Particle & particle) return std::exp(log_total); } +void MCL::compute_pose_covariance() +{ + double c_xx = 0.0; + double c_yy = 0.0; + double c_xy = 0.0; + double w_sum = 0.0; + + for (auto p : center_particles) { + double dx = p->position.x - estimated_position.x; + double dy = p->position.y - estimated_position.y; + + c_xx += p->weight * dx * dx; + c_yy += p->weight * dy * dy; + c_xy += p->weight * dx * dy; + w_sum += p->weight; + } + + if (w_sum > 1e-9) { + pose_cov[0][0] = c_xx / w_sum; + pose_cov[1][1] = c_yy / w_sum; + pose_cov[0][1] = c_xy / w_sum; + pose_cov[1][0] = c_xy / w_sum; + } +} + +double MCL::pose_uncertainty() const { + return pose_cov[0][0] + pose_cov[1][1]; +} + double MCL::calculate_object_likelihood( const ProjectedObject & measurement, const keisan::Point2 & landmark, @@ -392,7 +421,10 @@ void MCL::estimate_position() { double jump_distance = calculate_distance(estimated_position, last_estimated_position); - if (jump_distance <= 100.0 || initial_localization) { + compute_pose_covariance(); + double unc = pose_uncertainty(); + + if (unc < uncertainty_threshold || initial_localization) { position = estimated_position; apply_localization = true; if (initial_localization) { @@ -426,7 +458,7 @@ void MCL::print_particles() { printf("========================================\n"); printf("Particles num: %d\n", particles.size()); - printf("Prob: %.2f", prob); + printf("Prob: %.2f\n", prob); printf("Short term avg: %.8f\n", short_term_avg); printf("Long term avg: %.8f\n", long_term_avg); printf("Last weight avg: %.8f\n", last_weight_avg); diff --git a/src/basho/process/localization.cpp b/src/basho/process/localization.cpp index 50aec90..89d0514 100644 --- a/src/basho/process/localization.cpp +++ b/src/basho/process/localization.cpp @@ -31,9 +31,9 @@ #include "jitsuyo/config.hpp" #include "nlohmann/json.hpp" -#include "unistd.h" // NOLINT +#include "unistd.h" -using namespace keisan::literals; // NOLINT +using namespace keisan::literals; namespace basho { @@ -75,6 +75,7 @@ void Localization::set_config(const nlohmann::json & json) valid_section &= jitsuyo::assign_val(localization_section, "chi_square_gate", mcl->chi_square_gate); valid_section &= jitsuyo::assign_val(localization_section, "max_distance_x", max_distance_x); valid_section &= jitsuyo::assign_val(localization_section, "max_distance_y", max_distance_y); + valid_section &= jitsuyo::assign_val(localization_section, "uncertainty_threshold", mcl->uncertainty_threshold); mcl->max_object_distance = {max_distance_x, max_distance_y}; From 3be18de755638d16463fe5ed9df6b09ac8a8795a Mon Sep 17 00:00:00 2001 From: mbsaloka Date: Tue, 27 Jan 2026 03:20:00 +0700 Subject: [PATCH 12/33] refactor: cleanup mcl class structure --- include/basho/localization/mcl.hpp | 70 +++--- src/basho/localization/mcl.cpp | 346 ++++++++++++++--------------- src/basho/node/basho_node.cpp | 25 +-- 3 files changed, 213 insertions(+), 228 deletions(-) diff --git a/include/basho/localization/mcl.hpp b/include/basho/localization/mcl.hpp index 43e2ec9..9ead65e 100644 --- a/include/basho/localization/mcl.hpp +++ b/include/basho/localization/mcl.hpp @@ -55,31 +55,19 @@ class MCL MCL(); void localize(); - void clear_projected_objects(); void init_particles(const keisan::Point2 init_position); - void resample_particles(); - void update_motion(); - void calculate_weight(); - void estimate_position(); - void print_particles(); + void clear_projected_objects(); void set_initial_localization(bool initial) { initial_localization = initial; } - double calculate_distance(const keisan::Point2 & point_1, const keisan::Point2 & point_2); - double calculate_total_likelihood(const Particle & particle); - double calculate_object_likelihood(const ProjectedObject & measurement, - const keisan::Point2 & landmark, - const Particle & particle); - - double get_sum_weight(); - keisan::Matrix<8, 8> calculate_cost_matrix(const Particle & particle, - const std::vector & projected_objects, - const std::vector & landmarks); - - void compute_pose_covariance(); - double pose_uncertainty() const; + keisan::Point2 get_position() const { return position; } + keisan::Point2 get_estimated_position() const { return estimated_position; } + keisan::Angle get_orientation() const { return orientation; } + const std::vector & get_particles() const { return particles; } - std::vector particles; - keisan::Point2 estimated_position; + void set_delta_position(const keisan::Point2 & delta) { delta_position = delta; } + void set_orientation(const keisan::Angle & yaw) { orientation = yaw; } + void set_orientation_roll(const keisan::Angle & roll) { orientation_roll = roll; } + void set_orientation_pitch(const keisan::Angle & pitch) { orientation_pitch = pitch; } int num_particles; double min_centered_ratio; @@ -89,33 +77,52 @@ class MCL double range_sigma; double chi_square_gate; double uncertainty_threshold; + keisan::Point2 max_object_distance; + bool print_debug; bool apply_localization; - bool print_debug; // IPM std::vector projected_X; std::vector projected_L; std::vector projected_T; std::vector projected_goalpost; - keisan::Point2 max_object_distance; int num_projected_objects; - // member for getting +private: + // MCL Steps + void update_motion(); + void calculate_weight(); + void resample_particles(); + void estimate_position(); + void compute_pose_covariance(); + + // Likelihood Calculations + double calculate_distance(const keisan::Point2 & point_1, const keisan::Point2 & point_2); + double calculate_total_likelihood(const Particle & particle); + double calculate_object_likelihood(const ProjectedObject & measurement, + const keisan::Point2 & landmark, + const Particle & particle); + + keisan::Matrix<8, 8> calculate_cost_matrix(const Particle & particle, + const std::vector & detected_landmarks, + const std::vector & landmarks); + + void print_particles(); + + keisan::Point2 position; + keisan::Point2 delta_position; keisan::Angle orientation; keisan::Angle orientation_roll; keisan::Angle orientation_pitch; - keisan::Point2 position; - keisan::Point2 delta_position; -private: + keisan::Point2 estimated_position; + Field field; + std::vector particles; Particle* best_particle; std::vector center_particles; - double xvar; - double yvar; - std::mt19937 rand_gen; keisan::Hungarian<8> hungarian; @@ -126,10 +133,11 @@ class MCL double prob; bool initial_localization; - int pending_accept_count; keisan::Point2 pending_estimated_position; + double xvar; + double yvar; keisan::Matrix<2,2> pose_cov; }; diff --git a/src/basho/localization/mcl.cpp b/src/basho/localization/mcl.cpp index e7b0a36..f53fa5a 100644 --- a/src/basho/localization/mcl.cpp +++ b/src/basho/localization/mcl.cpp @@ -32,12 +32,12 @@ namespace basho MCL::MCL() : position(0.0, 0.0), orientation(0_deg), orientation_roll(0_deg), orientation_pitch(0_deg), - initial_localization(true), apply_localization(false), print_debug(false), - num_particles(500), min_centered_ratio(0.3), short_term_avg_ratio(0.1), long_term_avg_ratio(0.001), - base_sigma(10.0), range_sigma(0.05), chi_square_gate(9.21034), xvar(10.0), yvar(10.0), - best_particle(nullptr), rand_gen(std::random_device{}()), weight_avg(0.0), last_weight_avg(0.0), - short_term_avg(0.0), long_term_avg(0.0), estimated_position(-1.0, -1.0), num_projected_objects(0), - max_object_distance(300.0, 300.0), pending_accept_count(0), uncertainty_threshold(50.0) + delta_position(0.0, 0.0), estimated_position(0.0, 0.0), best_particle(nullptr), print_debug(false), + weight_avg(0.0), initial_localization(true), apply_localization(false), prob(0.0), last_weight_avg(0.0), + short_term_avg(0.0), long_term_avg(0.0), rand_gen(std::random_device{}()), pending_accept_count(0), + num_projected_objects(0), num_particles(100), min_centered_ratio(0.3), short_term_avg_ratio(0.1), + long_term_avg_ratio(0.001), base_sigma(10.0), range_sigma(0.05), chi_square_gate(9.21034), + uncertainty_threshold(50.0), max_object_distance(300.0, 300.0), xvar(10.0), yvar(10.0) { } @@ -66,7 +66,7 @@ void MCL::localize() prob = keisan::clamp(1.0 - short_term_avg / (long_term_avg + 1e-6), 0.0, 1.0); - // calculate effective number of particles for resampling + // calculate number of effective particles for resampling double sum_sq = 0.0; for (auto &p : particles) { sum_sq += p.weight * p.weight; @@ -83,15 +83,6 @@ void MCL::localize() clear_projected_objects(); } -void MCL::clear_projected_objects() -{ - projected_X.clear(); - projected_L.clear(); - projected_T.clear(); - projected_goalpost.clear(); - num_projected_objects = 0; -} - void MCL::init_particles(const keisan::Point2 init_position) { double uniform_weight = 1.0 / num_particles; @@ -121,6 +112,57 @@ void MCL::init_particles(const keisan::Point2 init_position) } } +void MCL::clear_projected_objects() +{ + projected_X.clear(); + projected_L.clear(); + projected_T.clear(); + projected_goalpost.clear(); + num_projected_objects = 0; +} + +void MCL::update_motion() +{ + static std::normal_distribution xgen(0.0, xvar), ygen(0.0, yvar); + + for (auto & p : particles) { + double static_noise_x = xgen(rand_gen) / 10.0; + double static_noise_y = ygen(rand_gen) / 10.0; + double dynamic_noise_x = fabs(delta_position.x) * xgen(rand_gen) / 5.0; + double dynamic_noise_y = fabs(delta_position.y) * ygen(rand_gen) / 5.0; + double x_yterm = fabs(delta_position.y) * xgen(rand_gen) / 30.0; + double y_xterm = fabs(delta_position.x) * ygen(rand_gen) / 30.0; + + p.position.x += delta_position.x + static_noise_x + dynamic_noise_x + x_yterm; + p.position.y += delta_position.y + static_noise_y + dynamic_noise_y + y_xterm; + p.orientation = orientation; + + p.position.x = keisan::clamp(p.position.x, 0.0, 900.0); + p.position.y = keisan::clamp(p.position.y, 0.0, 600.0); + } + + estimated_position += delta_position; +} + +void MCL::calculate_weight() +{ + double sum_weight = 0.0; + + for (auto & p : particles) { + p.weight = calculate_total_likelihood(p); + sum_weight += p.weight; + } + + if (!std::isnan(sum_weight) && sum_weight > 0) { + weight_avg = sum_weight / num_particles; + for (auto & p : particles) { + p.weight /= sum_weight; + } + } else { + weight_avg = 0.0; + } +} + void MCL::resample_particles() { std::vector new_particles(num_particles); @@ -129,15 +171,12 @@ void MCL::resample_particles() double beta = 0.0; double max_weight = 0.0; - double sum_weight = get_sum_weight(); // find the best particle - if (sum_weight > 0) { - for (auto & p : particles) { - if (p.weight > max_weight) { - max_weight = p.weight; - best_particle = &p; - } + for (auto & p : particles) { + if (p.weight > max_weight) { + max_weight = p.weight; + best_particle = &p; } } @@ -192,54 +231,93 @@ void MCL::resample_particles() particles = new_particles; } -void MCL::update_motion() -{ - static std::normal_distribution xgen(0.0, xvar), ygen(0.0, yvar); +void MCL::estimate_position() { + if (best_particle == nullptr || num_projected_objects == 0) { + return; + } - for (auto & p : particles) { - double static_noise_x = xgen(rand_gen) / 10.0; - double static_noise_y = ygen(rand_gen) / 10.0; - double dynamic_noise_x = fabs(delta_position.x) * xgen(rand_gen) / 5.0; - double dynamic_noise_y = fabs(delta_position.y) * ygen(rand_gen) / 5.0; - double x_yterm = fabs(delta_position.y) * xgen(rand_gen) / 30.0; - double y_xterm = fabs(delta_position.x) * ygen(rand_gen) / 30.0; + keisan::Point2 weighted_sum = {0.0, 0.0}; + double weight_sum = 0.0; + int centered_particles = 0; + center_particles.clear(); - p.position.x += delta_position.x + static_noise_x + dynamic_noise_x + x_yterm; - p.position.y += delta_position.y + static_noise_y + dynamic_noise_y + y_xterm; - p.orientation = orientation; + // Find numbers of particles closed to the best particle + for (int i = 0; i < num_particles; ++i) { + double distance = calculate_distance(particles[i].position, best_particle->position); - p.position.x = keisan::clamp(p.position.x, 0.0, 900.0); - p.position.y = keisan::clamp(p.position.y, 0.0, 600.0); + if (distance < 30.0) { + centered_particles++; + weighted_sum.x += particles[i].weight * particles[i].position.x; + weighted_sum.y += particles[i].weight * particles[i].position.y; + weight_sum += particles[i].weight; + + center_particles.push_back(&particles[i]); + } } - estimated_position += delta_position; -} + // Use mean of centered particles position if more than threshold are centered + if (centered_particles > min_centered_ratio * num_particles) { + auto last_estimated_position = estimated_position; + estimated_position.x = weighted_sum.x / weight_sum; + estimated_position.y = weighted_sum.y / weight_sum; -double MCL::get_sum_weight() -{ - double sum_weight = 0.0; - for (const auto & p : particles) { - sum_weight += p.weight; + estimated_position.x = keisan::clamp(estimated_position.x, 0.0, 900.0); + estimated_position.y = keisan::clamp(estimated_position.y, 0.0, 600.0); + + double jump_distance = calculate_distance(estimated_position, last_estimated_position); + + compute_pose_covariance(); + double pose_uncertainty = pose_cov[0][0] + pose_cov[1][1]; + + if (pose_uncertainty < uncertainty_threshold || initial_localization) { + position = estimated_position; + apply_localization = true; + if (initial_localization) { + initial_localization = false; + } + } else { + double delta_estimate = calculate_distance(pending_estimated_position, estimated_position); + if (delta_estimate < 20.0) { + pending_accept_count++; + } else { + pending_accept_count = 0; + } + pending_estimated_position = estimated_position; + + if (pending_accept_count >= 10) { + position = estimated_position; + apply_localization = true; + pending_accept_count = 0; + } else { + estimated_position = last_estimated_position; + apply_localization = false; + } + } } - return sum_weight; } -void MCL::calculate_weight() +void MCL::compute_pose_covariance() { - double sum_weight = 0.0; + double c_xx = 0.0; + double c_yy = 0.0; + double c_xy = 0.0; + double w_sum = 0.0; - for (auto & p : particles) { - p.weight = calculate_total_likelihood(p); - sum_weight += p.weight; + for (auto p : center_particles) { + double dx = p->position.x - estimated_position.x; + double dy = p->position.y - estimated_position.y; + + c_xx += p->weight * dx * dx; + c_yy += p->weight * dy * dy; + c_xy += p->weight * dx * dy; + w_sum += p->weight; } - if (!std::isnan(sum_weight) && sum_weight > 0) { - weight_avg = sum_weight / num_particles; - for (auto & p : particles) { - p.weight /= sum_weight; - } - } else { - weight_avg = 0.0; + if (w_sum > 1e-9) { + pose_cov[0][0] = c_xx / w_sum; + pose_cov[1][1] = c_yy / w_sum; + pose_cov[0][1] = c_xy / w_sum; + pose_cov[1][0] = c_xy / w_sum; } } @@ -251,49 +329,6 @@ double MCL::calculate_distance(const keisan::Point2 & point_1, const keisan::Poi return sqrt(dx * dx + dy * dy); } -keisan::Matrix<8, 8> MCL::calculate_cost_matrix( - const Particle & particle, - const std::vector & projected_objects, - const std::vector & landmarks) -{ - auto cost_matrix = keisan::Matrix<8, 8>::infinite(); - - int num_projected_objects = projected_objects.size(); - int num_landmarks = landmarks.size(); - - for (int i = 0; i < num_projected_objects; ++i) { - double dx = projected_objects[i].position.x * 100; - double dy = projected_objects[i].position.y * 100; - - double x_rot = dx * particle.orientation.cos() + dy * particle.orientation.sin(); - double y_rot = dx * particle.orientation.sin() - dy * particle.orientation.cos(); - - double px = particle.position.x + x_rot; - double py = particle.position.y + y_rot; - - double dist = std::sqrt(dx*dx + dy*dy); - double sigma = base_sigma + range_sigma * dist; - double var = sigma * sigma; - - for (int j = 0; j < num_landmarks; ++j) { - double ex = landmarks[j].x - px; - double ey = landmarks[j].y - py; - - // Squared Mahalanobis distance - double d2 = (ex*ex + ey*ey) / var; - - // Chi-square gating to reject unlikely associations - if (d2 < chi_square_gate) { - cost_matrix[i][j] = d2; - } else { - cost_matrix[i][j] = 1e9; - } - } - } - - return cost_matrix; -} - double MCL::calculate_total_likelihood(const Particle & particle) { double log_total = 0.0; @@ -327,35 +362,6 @@ double MCL::calculate_total_likelihood(const Particle & particle) return std::exp(log_total); } -void MCL::compute_pose_covariance() -{ - double c_xx = 0.0; - double c_yy = 0.0; - double c_xy = 0.0; - double w_sum = 0.0; - - for (auto p : center_particles) { - double dx = p->position.x - estimated_position.x; - double dy = p->position.y - estimated_position.y; - - c_xx += p->weight * dx * dx; - c_yy += p->weight * dy * dy; - c_xy += p->weight * dx * dy; - w_sum += p->weight; - } - - if (w_sum > 1e-9) { - pose_cov[0][0] = c_xx / w_sum; - pose_cov[1][1] = c_yy / w_sum; - pose_cov[0][1] = c_xy / w_sum; - pose_cov[1][0] = c_xy / w_sum; - } -} - -double MCL::pose_uncertainty() const { - return pose_cov[0][0] + pose_cov[1][1]; -} - double MCL::calculate_object_likelihood( const ProjectedObject & measurement, const keisan::Point2 & landmark, @@ -386,69 +392,44 @@ double MCL::calculate_object_likelihood( return std::exp(-0.5 * d2); } -void MCL::estimate_position() { - if (best_particle == nullptr || num_projected_objects == 0) { - return; - } - - keisan::Point2 weighted_sum = {0.0, 0.0}; - double weight_sum = 0.0; - int centered_particles = 0; - center_particles.clear(); - - // Find numbers of particles closed to the best particle - for (int i = 0; i < num_particles; ++i) { - double distance = calculate_distance(particles[i].position, best_particle->position); - - if (distance < 30.0) { - centered_particles++; - weighted_sum.x += particles[i].weight * particles[i].position.x; - weighted_sum.y += particles[i].weight * particles[i].position.y; - weight_sum += particles[i].weight; +keisan::Matrix<8, 8> MCL::calculate_cost_matrix( + const Particle & particle, + const std::vector & detected_landmarks, + const std::vector & landmarks) +{ + auto cost_matrix = keisan::Matrix<8, 8>::infinite(); - center_particles.push_back(&particles[i]); - } - } + for (int i = 0; i < detected_landmarks.size(); ++i) { + double dx = detected_landmarks[i].position.x * 100; + double dy = detected_landmarks[i].position.y * 100; - // Use mean of centered particles position if more than threshold are centered - if (centered_particles > min_centered_ratio * num_particles) { - auto last_estimated_position = estimated_position; - estimated_position.x = weighted_sum.x / weight_sum; - estimated_position.y = weighted_sum.y / weight_sum; + double x_rot = dx * particle.orientation.cos() + dy * particle.orientation.sin(); + double y_rot = dx * particle.orientation.sin() - dy * particle.orientation.cos(); - estimated_position.x = keisan::clamp(estimated_position.x, 0.0, 900.0); - estimated_position.y = keisan::clamp(estimated_position.y, 0.0, 600.0); + double px = particle.position.x + x_rot; + double py = particle.position.y + y_rot; - double jump_distance = calculate_distance(estimated_position, last_estimated_position); + double dist = std::sqrt(dx*dx + dy*dy); + double sigma = base_sigma + range_sigma * dist; + double var = sigma * sigma; - compute_pose_covariance(); - double unc = pose_uncertainty(); + for (int j = 0; j < landmarks.size(); ++j) { + double ex = landmarks[j].x - px; + double ey = landmarks[j].y - py; - if (unc < uncertainty_threshold || initial_localization) { - position = estimated_position; - apply_localization = true; - if (initial_localization) { - initial_localization = false; - } - } else { - double delta_estimate = calculate_distance(pending_estimated_position, estimated_position); - if (delta_estimate < 20.0) { - pending_accept_count++; - } else { - pending_accept_count = 0; - } - pending_estimated_position = estimated_position; + // Squared Mahalanobis distance + double d2 = (ex*ex + ey*ey) / var; - if (pending_accept_count >= 10) { - position = estimated_position; - apply_localization = true; - pending_accept_count = 0; + // Chi-square gating to reject unlikely associations + if (d2 < chi_square_gate) { + cost_matrix[i][j] = d2; } else { - estimated_position = last_estimated_position; - apply_localization = false; + cost_matrix[i][j] = 1e9; } } } + + return cost_matrix; } void MCL::print_particles() { @@ -463,7 +444,6 @@ void MCL::print_particles() { printf("Long term avg: %.8f\n", long_term_avg); printf("Last weight avg: %.8f\n", last_weight_avg); printf("Weight avg: %.8f\n", weight_avg); - printf("Sum weights: %.8f\n", get_sum_weight()); printf("Minimal Centered Particles: %.0f\n", min_centered_ratio * num_particles); diff --git a/src/basho/node/basho_node.cpp b/src/basho/node/basho_node.cpp index 760f6ef..9a7c4ae 100644 --- a/src/basho/node/basho_node.cpp +++ b/src/basho/node/basho_node.cpp @@ -47,12 +47,9 @@ BashoNode::BashoNode( [this](const MeasurementStatus::SharedPtr msg) { auto mcl = this->localization->get_mcl(); - mcl->orientation = - keisan::make_degree(msg->orientation.yaw); - mcl->orientation_roll = - keisan::make_degree(msg->orientation.roll); - mcl->orientation_pitch = - keisan::make_degree(msg->orientation.pitch); + mcl->set_orientation(keisan::make_degree(msg->orientation.yaw)); + mcl->set_orientation_roll(keisan::make_degree(msg->orientation.roll)); + mcl->set_orientation_pitch(keisan::make_degree(msg->orientation.pitch)); }); delta_position_subscriber = @@ -61,7 +58,7 @@ BashoNode::BashoNode( 10, [this](const Point2::SharedPtr msg) { auto mcl = this->localization->get_mcl(); - mcl->delta_position = {msg->x, msg->y}; + mcl->set_delta_position({msg->x, msg->y}); }); projected_objects_subscriber = @@ -128,8 +125,8 @@ void BashoNode::publish_odometry() auto mcl = localization->get_mcl(); Point2 msg; - msg.x = mcl->position.x; - msg.y = mcl->position.y; + msg.x = mcl->get_position().x; + msg.y = mcl->get_position().y; set_odometry_publisher->publish(msg); } @@ -141,7 +138,7 @@ void BashoNode::publish_particles() Particles msg; double max_weight = 0.0; - for (const auto & p : mcl->particles) { + for (const auto & p : mcl->get_particles()) { Particle particle; particle.x = p.position.x; particle.y = p.position.y; @@ -152,12 +149,12 @@ void BashoNode::publish_particles() max_weight = std::max(max_weight, p.weight); } - msg.estimated_position.x = mcl->estimated_position.x; - msg.estimated_position.y = mcl->estimated_position.y; - msg.estimated_position.orientation = mcl->orientation.degree(); + msg.estimated_position.x = mcl->get_estimated_position().x; + msg.estimated_position.y = mcl->get_estimated_position().y; + msg.estimated_position.orientation = mcl->get_orientation().degree(); msg.estimated_position.weight = max_weight; particles_publisher->publish(msg); } -} // namespace basho \ No newline at end of file +} // namespace basho From f48370b0bb61e735a60d9cf033ac39ca581f01c1 Mon Sep 17 00:00:00 2001 From: mbsaloka Date: Tue, 27 Jan 2026 13:36:21 +0700 Subject: [PATCH 13/33] feat: use cluster and covariance to determine localization state --- include/basho/localization/mcl.hpp | 10 ++- src/basho/localization/mcl.cpp | 121 +++++++++++++++-------------- src/basho/process/localization.cpp | 3 +- 3 files changed, 72 insertions(+), 62 deletions(-) diff --git a/include/basho/localization/mcl.hpp b/include/basho/localization/mcl.hpp index 9ead65e..3d04eee 100644 --- a/include/basho/localization/mcl.hpp +++ b/include/basho/localization/mcl.hpp @@ -52,6 +52,8 @@ struct LandmarkGroup { class MCL { public: + enum { INIT, TRACKING, UNCERTAIN, LOST }; + MCL(); void localize(); @@ -70,6 +72,7 @@ class MCL void set_orientation_pitch(const keisan::Angle & pitch) { orientation_pitch = pitch; } int num_particles; + double neff_threshold_ratio; double min_centered_ratio; double short_term_avg_ratio; double long_term_avg_ratio; @@ -81,6 +84,7 @@ class MCL bool print_debug; bool apply_localization; + int state; // IPM std::vector projected_X; @@ -95,7 +99,6 @@ class MCL void calculate_weight(); void resample_particles(); void estimate_position(); - void compute_pose_covariance(); // Likelihood Calculations double calculate_distance(const keisan::Point2 & point_1, const keisan::Point2 & point_2); @@ -108,6 +111,9 @@ class MCL const std::vector & detected_landmarks, const std::vector & landmarks); + double calculate_neff(); + double calculate_pose_covariance(); + void print_particles(); keisan::Point2 position; @@ -122,6 +128,7 @@ class MCL std::vector particles; Particle* best_particle; std::vector center_particles; + double centered_ratio; std::mt19937 rand_gen; keisan::Hungarian<8> hungarian; @@ -139,6 +146,7 @@ class MCL double xvar; double yvar; keisan::Matrix<2,2> pose_cov; + double pose_uncertainty; }; } // namespace basho diff --git a/src/basho/localization/mcl.cpp b/src/basho/localization/mcl.cpp index f53fa5a..77d63bc 100644 --- a/src/basho/localization/mcl.cpp +++ b/src/basho/localization/mcl.cpp @@ -31,13 +31,14 @@ namespace basho { MCL::MCL() -: position(0.0, 0.0), orientation(0_deg), orientation_roll(0_deg), orientation_pitch(0_deg), +: position(0.0, 0.0), orientation(0_deg), orientation_roll(0_deg), orientation_pitch(0_deg), state(INIT), delta_position(0.0, 0.0), estimated_position(0.0, 0.0), best_particle(nullptr), print_debug(false), weight_avg(0.0), initial_localization(true), apply_localization(false), prob(0.0), last_weight_avg(0.0), short_term_avg(0.0), long_term_avg(0.0), rand_gen(std::random_device{}()), pending_accept_count(0), - num_projected_objects(0), num_particles(100), min_centered_ratio(0.3), short_term_avg_ratio(0.1), - long_term_avg_ratio(0.001), base_sigma(10.0), range_sigma(0.05), chi_square_gate(9.21034), - uncertainty_threshold(50.0), max_object_distance(300.0, 300.0), xvar(10.0), yvar(10.0) + num_projected_objects(0), num_particles(100), neff_threshold_ratio(0.75), min_centered_ratio(0.3), + short_term_avg_ratio(0.1), long_term_avg_ratio(0.001), base_sigma(10.0), range_sigma(0.05), xvar(10.0), + yvar(10.0), chi_square_gate(9.21034), uncertainty_threshold(50.0), max_object_distance(300.0, 300.0), + centered_ratio(0.0), pose_uncertainty(0.0) { } @@ -59,6 +60,8 @@ void MCL::localize() if (initial_localization) { short_term_avg = weight_avg; long_term_avg = weight_avg; + + initial_localization = false; } short_term_avg += short_term_avg_ratio * (weight_avg - short_term_avg); @@ -66,19 +69,28 @@ void MCL::localize() prob = keisan::clamp(1.0 - short_term_avg / (long_term_avg + 1e-6), 0.0, 1.0); - // calculate number of effective particles for resampling - double sum_sq = 0.0; - for (auto &p : particles) { - sum_sq += p.weight * p.weight; - } - double neff = 1.0 / (sum_sq + 1e-12); - - if (neff < 0.75 * num_particles) { + double neff = calculate_neff(); + if (neff < neff_threshold_ratio * num_particles) { resample_particles(); } } estimate_position(); + pose_uncertainty = calculate_pose_covariance(); + + apply_localization = false; + if (initial_localization) { + state = INIT; + } else if (centered_ratio < min_centered_ratio) { + state = LOST; + } else if (pose_uncertainty < uncertainty_threshold * 1.5) { + position = estimated_position; + apply_localization = true; + state = pose_uncertainty < uncertainty_threshold ? TRACKING : UNCERTAIN; + } else { + state = LOST; + } + print_particles(); clear_projected_objects(); } @@ -88,7 +100,6 @@ void MCL::init_particles(const keisan::Point2 init_position) double uniform_weight = 1.0 / num_particles; particles.clear(); particles.resize(num_particles); - initial_localization = true; estimated_position = init_position; if (init_position.x == -1 && init_position.y == -1) { @@ -159,7 +170,7 @@ void MCL::calculate_weight() p.weight /= sum_weight; } } else { - weight_avg = 0.0; + weight_avg = 1e-9; } } @@ -232,71 +243,51 @@ void MCL::resample_particles() } void MCL::estimate_position() { - if (best_particle == nullptr || num_projected_objects == 0) { + if (best_particle == nullptr) { return; } keisan::Point2 weighted_sum = {0.0, 0.0}; - double weight_sum = 0.0; + double weight_sum = 1e-9; int centered_particles = 0; center_particles.clear(); - // Find numbers of particles closed to the best particle - for (int i = 0; i < num_particles; ++i) { - double distance = calculate_distance(particles[i].position, best_particle->position); + // Find particles clustered around the best particle + for (auto & p : particles) { + double distance = calculate_distance(p.position, best_particle->position); - if (distance < 30.0) { + if (distance < 50.0) { centered_particles++; - weighted_sum.x += particles[i].weight * particles[i].position.x; - weighted_sum.y += particles[i].weight * particles[i].position.y; - weight_sum += particles[i].weight; + weighted_sum.x += p.weight * p.position.x; + weighted_sum.y += p.weight * p.position.y; + weight_sum += p.weight; - center_particles.push_back(&particles[i]); + center_particles.push_back(&p); } } - // Use mean of centered particles position if more than threshold are centered - if (centered_particles > min_centered_ratio * num_particles) { - auto last_estimated_position = estimated_position; - estimated_position.x = weighted_sum.x / weight_sum; - estimated_position.y = weighted_sum.y / weight_sum; - - estimated_position.x = keisan::clamp(estimated_position.x, 0.0, 900.0); - estimated_position.y = keisan::clamp(estimated_position.y, 0.0, 600.0); + centered_ratio = (double)centered_particles / num_particles; - double jump_distance = calculate_distance(estimated_position, last_estimated_position); + // Use mean of centered particles position as estimated position + estimated_position.x = weighted_sum.x / weight_sum; + estimated_position.y = weighted_sum.y / weight_sum; - compute_pose_covariance(); - double pose_uncertainty = pose_cov[0][0] + pose_cov[1][1]; + estimated_position.x = keisan::clamp(estimated_position.x, 0.0, 900.0); + estimated_position.y = keisan::clamp(estimated_position.y, 0.0, 600.0); +} - if (pose_uncertainty < uncertainty_threshold || initial_localization) { - position = estimated_position; - apply_localization = true; - if (initial_localization) { - initial_localization = false; - } - } else { - double delta_estimate = calculate_distance(pending_estimated_position, estimated_position); - if (delta_estimate < 20.0) { - pending_accept_count++; - } else { - pending_accept_count = 0; - } - pending_estimated_position = estimated_position; +double MCL::calculate_neff() +{ + double sum_sq = 1e-9; - if (pending_accept_count >= 10) { - position = estimated_position; - apply_localization = true; - pending_accept_count = 0; - } else { - estimated_position = last_estimated_position; - apply_localization = false; - } - } + for (auto & p : particles) { + sum_sq += p.weight * p.weight; } + + return 1.0 / sum_sq; } -void MCL::compute_pose_covariance() +double MCL::calculate_pose_covariance() { double c_xx = 0.0; double c_yy = 0.0; @@ -319,6 +310,8 @@ void MCL::compute_pose_covariance() pose_cov[0][1] = c_xy / w_sum; pose_cov[1][0] = c_xy / w_sum; } + + return pose_cov[0][0] + pose_cov[1][1]; } double MCL::calculate_distance(const keisan::Point2 & point_1, const keisan::Point2 & point_2) @@ -437,7 +430,7 @@ void MCL::print_particles() { return; } - printf("========================================\n"); + printf("\033c"); printf("Particles num: %d\n", particles.size()); printf("Prob: %.2f\n", prob); printf("Short term avg: %.8f\n", short_term_avg); @@ -445,6 +438,8 @@ void MCL::print_particles() { printf("Last weight avg: %.8f\n", last_weight_avg); printf("Weight avg: %.8f\n", weight_avg); + printf("Minimal Centered Ratio: %.2f\n", min_centered_ratio); + printf("Centered Ratio: %.2f\n", centered_ratio); printf("Minimal Centered Particles: %.0f\n", min_centered_ratio * num_particles); printf("Centered particles: %d\n", center_particles.size()); @@ -475,6 +470,12 @@ void MCL::print_particles() { printf("Num particles: %d\n", num_particles); printf("Initial Localization %d\n", initial_localization); printf("Pose estimation: [%.2f, %.2f])\n", estimated_position.x, estimated_position.y); + printf("Pose covariance uncertainty: %.2f\n", pose_uncertainty); + printf("STATE: [%s]\n", + (state == INIT) ? "INIT" : + (state == TRACKING) ? "TRACKING" : + (state == UNCERTAIN) ? "UNCERTAIN" : + (state == LOST) ? "LOST" : "UNKNOWN"); } } // namespace basho diff --git a/src/basho/process/localization.cpp b/src/basho/process/localization.cpp index 89d0514..ad63de0 100644 --- a/src/basho/process/localization.cpp +++ b/src/basho/process/localization.cpp @@ -65,8 +65,8 @@ void Localization::set_config(const nlohmann::json & json) double max_distance_x = 0.0; double max_distance_y = 0.0; - valid_section &= jitsuyo::assign_val(localization_section, "debug", mcl->print_debug); valid_section &= jitsuyo::assign_val(localization_section, "num_particles", mcl->num_particles); + valid_section &= jitsuyo::assign_val(localization_section, "neff_threshold_ratio", mcl->neff_threshold_ratio); valid_section &= jitsuyo::assign_val(localization_section, "min_centered_ratio", mcl->min_centered_ratio); valid_section &= jitsuyo::assign_val(localization_section, "short_term_avg_ratio", mcl->short_term_avg_ratio); valid_section &= jitsuyo::assign_val(localization_section, "long_term_avg_ratio", mcl->long_term_avg_ratio); @@ -76,6 +76,7 @@ void Localization::set_config(const nlohmann::json & json) valid_section &= jitsuyo::assign_val(localization_section, "max_distance_x", max_distance_x); valid_section &= jitsuyo::assign_val(localization_section, "max_distance_y", max_distance_y); valid_section &= jitsuyo::assign_val(localization_section, "uncertainty_threshold", mcl->uncertainty_threshold); + valid_section &= jitsuyo::assign_val(localization_section, "debug", mcl->print_debug); mcl->max_object_distance = {max_distance_x, max_distance_y}; From 6a46bdfc63c1bae63e651e22f2c1d08302db53cb Mon Sep 17 00:00:00 2001 From: mbsaloka Date: Tue, 27 Jan 2026 22:57:18 +0700 Subject: [PATCH 14/33] feat: add config node --- CMakeLists.txt | 1 + include/basho/config/config.hpp | 26 ++++++ include/basho/config/node/config_node.hpp | 60 ++++++++++++++ include/basho/localization/mcl.hpp | 3 +- include/basho/node/basho_node.hpp | 5 ++ include/basho/process/localization.hpp | 8 +- src/basho/config/node/config_node.cpp | 97 +++++++++++++++++++++++ src/basho/node/basho_node.cpp | 5 ++ src/basho/process/localization.cpp | 36 ++++++++- src/basho_main.cpp | 5 +- 10 files changed, 237 insertions(+), 9 deletions(-) create mode 100644 include/basho/config/config.hpp create mode 100644 include/basho/config/node/config_node.hpp create mode 100644 src/basho/config/node/config_node.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index 9ca04ac..b70e47a 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -28,6 +28,7 @@ find_package(basho_interfaces REQUIRED) include_directories(${EIGEN3_INCLUDE_DIRS}) add_library(${PROJECT_NAME} SHARED + "src/${PROJECT_NAME}/config/node/config_node.cpp" "src/${PROJECT_NAME}/node/basho_node.cpp" "src/${PROJECT_NAME}/localization/mcl.cpp" "src/${PROJECT_NAME}/process/localization.cpp") diff --git a/include/basho/config/config.hpp b/include/basho/config/config.hpp new file mode 100644 index 0000000..84cf12c --- /dev/null +++ b/include/basho/config/config.hpp @@ -0,0 +1,26 @@ +// Copyright (c) 2026 ICHIRO ITS +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in +// all copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL +// THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// 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. + +#ifndef BASHO__CONFIG__CONFIG_HPP_ +#define BASHO__CONFIG__CONFIG_HPP_ + +#include "basho/config/node/config_node.hpp" + +#endif // BASHO__CONFIG__CONFIG_HPP_ diff --git a/include/basho/config/node/config_node.hpp b/include/basho/config/node/config_node.hpp new file mode 100644 index 0000000..155f41d --- /dev/null +++ b/include/basho/config/node/config_node.hpp @@ -0,0 +1,60 @@ +// Copyright (c) 2026 Ichiro ITS +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in +// all copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL +// THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// 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. + +#ifndef BASHO__CONFIG__NODE__CONFIG_NODE_HPP_ +#define BASHO__CONFIG__NODE__CONFIG_NODE_HPP_ + +#include +#include + +#include "basho/process/localization.hpp" +#include "basho_interfaces/srv/set_config.hpp" +#include "basho_interfaces/srv/get_config.hpp" +#include "basho_interfaces/srv/save_config.hpp" +#include "rclcpp/rclcpp.hpp" + +namespace basho +{ + +class ConfigNode +{ +public: + using GetConfig = basho_interfaces::srv::GetConfig; + using SaveConfig = basho_interfaces::srv::SaveConfig; + using SetConfig = basho_interfaces::srv::SetConfig; + + explicit ConfigNode(rclcpp::Node::SharedPtr node, const std::string & path, + const std::shared_ptr & localization); + +private: + std::string get_node_prefix() const; + + std::shared_ptr localization; + + rclcpp::Node::SharedPtr node; + + rclcpp::Service::SharedPtr get_config_server; + rclcpp::Service::SharedPtr save_config_server; + rclcpp::Service::SharedPtr set_config_server; +}; + +} // namespace basho + +#endif // BASHO__CONFIG__NODE__CONFIG_NODE_HPP_ diff --git a/include/basho/localization/mcl.hpp b/include/basho/localization/mcl.hpp index 3d04eee..5ccd9f9 100644 --- a/include/basho/localization/mcl.hpp +++ b/include/basho/localization/mcl.hpp @@ -57,7 +57,7 @@ class MCL MCL(); void localize(); - void init_particles(const keisan::Point2 init_position); + void reset() { initial_localization = true; } void clear_projected_objects(); void set_initial_localization(bool initial) { initial_localization = initial; } @@ -95,6 +95,7 @@ class MCL private: // MCL Steps + void init_particles(const keisan::Point2 init_position); void update_motion(); void calculate_weight(); void resample_particles(); diff --git a/include/basho/node/basho_node.hpp b/include/basho/node/basho_node.hpp index f5daf27..bee05a7 100644 --- a/include/basho/node/basho_node.hpp +++ b/include/basho/node/basho_node.hpp @@ -28,6 +28,7 @@ #include "aruku_interfaces/msg/point2.hpp" #include "aruku_interfaces/msg/status.hpp" #include "basho/localization/mcl.hpp" +#include "basho/config/node/config_node.hpp" #include "basho/process/localization.hpp" #include "basho_interfaces/msg/particle.hpp" #include "basho_interfaces/msg/particles.hpp" @@ -57,6 +58,8 @@ class BashoNode const std::string & path ); + void run_config_service(const std::string & path); + private: void publish_odometry(); void publish_particles(); @@ -71,6 +74,8 @@ class BashoNode rclcpp::Subscription::SharedPtr delta_position_subscriber; std::shared_ptr localization; + + std::shared_ptr config_node; }; } // namespace basho diff --git a/include/basho/process/localization.hpp b/include/basho/process/localization.hpp index 3daa3bb..6b43d39 100644 --- a/include/basho/process/localization.hpp +++ b/include/basho/process/localization.hpp @@ -34,14 +34,18 @@ namespace basho class Localization { public: - explicit Localization(std::shared_ptr mcl); + explicit Localization(std::shared_ptr mcl, const std::string & config_path); - void load_config(const std::string & path); + void load_config(); void set_config(const nlohmann::json & json); + void save_config(); + void sync_config(); + void update(); std::shared_ptr get_mcl() const; + std::string config_path; std::string config_name; private: diff --git a/src/basho/config/node/config_node.cpp b/src/basho/config/node/config_node.cpp new file mode 100644 index 0000000..e84bf3b --- /dev/null +++ b/src/basho/config/node/config_node.cpp @@ -0,0 +1,97 @@ +// Copyright (c) 2026 Ichiro ITS +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in +// all copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL +// THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// 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. + +#include +#include +#include + +#include "basho/config/node/config_node.hpp" +#include "jitsuyo/config.hpp" +#include "nlohmann/json.hpp" +#include "rclcpp/rclcpp.hpp" + +namespace basho +{ + +ConfigNode::ConfigNode( + rclcpp::Node::SharedPtr node, + const std::string & path, + const std::shared_ptr & localization) +: node(node), localization(localization) +{ + get_config_server = node->create_service( + get_node_prefix() + "/get_config", + [this, path](GetConfig::Request::SharedPtr, GetConfig::Response::SharedPtr response) { + try { + nlohmann::ordered_json data; + if (!jitsuyo::load_config(path, "localization.json", data)) { + RCLCPP_ERROR(this->node->get_logger(), "Failed to load localization config"); + return; + } + response->json = data.dump(); + } catch (const std::exception & e) { + RCLCPP_ERROR(this->node->get_logger(), "Exception: %s", e.what()); + } + } + ); + + save_config_server = node->create_service( + get_node_prefix() + "/save_config", + [this, path](SaveConfig::Request::SharedPtr request, SaveConfig::Response::SharedPtr response) { + try { + response->status = false; + nlohmann::ordered_json data = nlohmann::ordered_json::parse(request->json); + + if (!jitsuyo::save_config(path, "localization.json", data)) { + RCLCPP_ERROR(this->node->get_logger(), "Failed to save localization config"); + return; + } + + response->status = true; + } catch (const std::exception & e) { + RCLCPP_ERROR(this->node->get_logger(), "Exception: %s", e.what()); + } + } + ); + + set_config_server = node->create_service( + get_node_prefix() + "/set_config", + [this](SetConfig::Request::SharedPtr request, SetConfig::Response::SharedPtr response) { + try { + response->status = false; + nlohmann::ordered_json data = nlohmann::ordered_json::parse(request->json); + + this->localization->set_config(data); + this->localization->get_mcl()->reset(); + + response->status = true; + } catch (const std::exception & e) { + RCLCPP_ERROR(this->node->get_logger(), "Exception: %s", e.what()); + } + } + ); +} + +std::string ConfigNode::get_node_prefix() const +{ + return "basho/config"; +} + +} // namespace basho diff --git a/src/basho/node/basho_node.cpp b/src/basho/node/basho_node.cpp index 9a7c4ae..826ed57 100644 --- a/src/basho/node/basho_node.cpp +++ b/src/basho/node/basho_node.cpp @@ -157,4 +157,9 @@ void BashoNode::publish_particles() particles_publisher->publish(msg); } +void BashoNode::run_config_service(const std::string & path) +{ + config_node = std::make_shared(node, path, localization); +} + } // namespace basho diff --git a/src/basho/process/localization.cpp b/src/basho/process/localization.cpp index ad63de0..9209224 100644 --- a/src/basho/process/localization.cpp +++ b/src/basho/process/localization.cpp @@ -38,15 +38,15 @@ using namespace keisan::literals; namespace basho { -Localization::Localization(std::shared_ptr mcl) -: config_name("localization.json"), mcl(mcl) +Localization::Localization(std::shared_ptr mcl, const std::string & config_path) +: config_name("localization.json"), config_path(config_path), mcl(mcl) { } -void Localization::load_config(const std::string & path) +void Localization::load_config() { nlohmann::json data; - if (!jitsuyo::load_config(path, config_name, data)) { + if (!jitsuyo::load_config(config_path, config_name, data)) { throw std::runtime_error("Failed to find config file `localization.json`"); } @@ -93,6 +93,34 @@ void Localization::set_config(const nlohmann::json & json) } } +void Localization::save_config() +{ + nlohmann::json config; + + config["localization"] = { + {"num_particles", mcl->num_particles}, + {"neff_threshold_ratio", mcl->neff_threshold_ratio}, + {"min_centered_ratio", mcl->min_centered_ratio}, + {"short_term_avg_ratio", mcl->short_term_avg_ratio}, + {"long_term_avg_ratio", mcl->long_term_avg_ratio}, + {"base_sigma", mcl->base_sigma}, + {"range_sigma", mcl->range_sigma}, + {"chi_square_gate", mcl->chi_square_gate}, + {"max_distance_x", mcl->max_object_distance.x}, + {"max_distance_y", mcl->max_object_distance.y}, + {"uncertainty_threshold", mcl->uncertainty_threshold}, + {"debug", mcl->print_debug} + }; + + jitsuyo::save_config(config_path, config_name, config); +} + +void Localization::sync_config() +{ + load_config(); + save_config(); +} + void Localization::update() { mcl->localize(); diff --git a/src/basho_main.cpp b/src/basho_main.cpp index 90cb16f..b584f92 100644 --- a/src/basho_main.cpp +++ b/src/basho_main.cpp @@ -37,10 +37,11 @@ int main(int argc, char ** argv) auto node = std::make_shared("basho_node"); - auto localization = std::make_shared(std::make_shared()); - localization->load_config(path); + auto localization = std::make_shared(std::make_shared(), path); + localization->sync_config(); auto basho_node = std::make_shared(node, localization, path); + basho_node->run_config_service(path); rclcpp::Rate rcl_rate(8ms); while (rclcpp::ok()) { From e639f4c4f32e61b592bc14d86259be6652c8465b Mon Sep 17 00:00:00 2001 From: mbsaloka Date: Wed, 25 Feb 2026 12:22:21 +0700 Subject: [PATCH 15/33] fix: improve localize main flow --- include/basho/localization/mcl.hpp | 1 + src/basho/localization/mcl.cpp | 41 ++++++++++++++++-------------- 2 files changed, 23 insertions(+), 19 deletions(-) diff --git a/include/basho/localization/mcl.hpp b/include/basho/localization/mcl.hpp index 5ccd9f9..b794f6c 100644 --- a/include/basho/localization/mcl.hpp +++ b/include/basho/localization/mcl.hpp @@ -141,6 +141,7 @@ class MCL double prob; bool initial_localization; + bool initial_avg_weight; int pending_accept_count; keisan::Point2 pending_estimated_position; diff --git a/src/basho/localization/mcl.cpp b/src/basho/localization/mcl.cpp index 77d63bc..13ee237 100644 --- a/src/basho/localization/mcl.cpp +++ b/src/basho/localization/mcl.cpp @@ -46,22 +46,28 @@ void MCL::localize() { if (initial_localization) { init_particles(keisan::Point2(-1, -1)); - } else { - update_motion(); + initial_localization = false; + initial_avg_weight = true; + state = INIT; + clear_projected_objects(); + return; } + update_motion(); + bool evaluate_particles = num_projected_objects > 0; evaluate_particles &= fabs(orientation_roll.degree()) <= 30; evaluate_particles &= fabs(orientation_pitch.degree()) <= 30; + apply_localization = false; + if (evaluate_particles) { calculate_weight(); - - if (initial_localization) { + if (initial_avg_weight) { short_term_avg = weight_avg; long_term_avg = weight_avg; - initial_localization = false; + initial_avg_weight = false; } short_term_avg += short_term_avg_ratio * (weight_avg - short_term_avg); @@ -73,22 +79,19 @@ void MCL::localize() if (neff < neff_threshold_ratio * num_particles) { resample_particles(); } - } - estimate_position(); - pose_uncertainty = calculate_pose_covariance(); + estimate_position(); + pose_uncertainty = calculate_pose_covariance(); - apply_localization = false; - if (initial_localization) { - state = INIT; - } else if (centered_ratio < min_centered_ratio) { - state = LOST; - } else if (pose_uncertainty < uncertainty_threshold * 1.5) { - position = estimated_position; - apply_localization = true; - state = pose_uncertainty < uncertainty_threshold ? TRACKING : UNCERTAIN; - } else { - state = LOST; + if (centered_ratio < min_centered_ratio) { + state = LOST; + } else if (pose_uncertainty < uncertainty_threshold * 1.5) { + position = estimated_position; + apply_localization = true; + state = pose_uncertainty < uncertainty_threshold ? TRACKING : UNCERTAIN; + } else { + state = LOST; + } } print_particles(); From da39435e7debdb536a999c73a9da82d443b3c2c6 Mon Sep 17 00:00:00 2001 From: mbsaloka Date: Wed, 25 Mar 2026 18:08:06 +0700 Subject: [PATCH 16/33] feat: add init particle topic --- include/basho/localization/mcl.hpp | 5 +++-- include/basho/node/basho_node.hpp | 1 + src/basho/localization/mcl.cpp | 14 +++++++------- src/basho/node/basho_node.cpp | 10 ++++++++++ 4 files changed, 21 insertions(+), 9 deletions(-) diff --git a/include/basho/localization/mcl.hpp b/include/basho/localization/mcl.hpp index b794f6c..2bf025c 100644 --- a/include/basho/localization/mcl.hpp +++ b/include/basho/localization/mcl.hpp @@ -59,7 +59,7 @@ class MCL void localize(); void reset() { initial_localization = true; } void clear_projected_objects(); - void set_initial_localization(bool initial) { initial_localization = initial; } + void set_initial_position(const keisan::Point2 & position) { initial_position = position; } keisan::Point2 get_position() const { return position; } keisan::Point2 get_estimated_position() const { return estimated_position; } @@ -95,7 +95,7 @@ class MCL private: // MCL Steps - void init_particles(const keisan::Point2 init_position); + void init_particles(); void update_motion(); void calculate_weight(); void resample_particles(); @@ -123,6 +123,7 @@ class MCL keisan::Angle orientation_roll; keisan::Angle orientation_pitch; + keisan::Point2 initial_position; keisan::Point2 estimated_position; Field field; diff --git a/include/basho/node/basho_node.hpp b/include/basho/node/basho_node.hpp index bee05a7..4a269a1 100644 --- a/include/basho/node/basho_node.hpp +++ b/include/basho/node/basho_node.hpp @@ -72,6 +72,7 @@ class BashoNode rclcpp::Subscription::SharedPtr measurement_status_subscriber; rclcpp::Subscription::SharedPtr projected_objects_subscriber; rclcpp::Subscription::SharedPtr delta_position_subscriber; + rclcpp::Subscription::SharedPtr init_localization_subscriber; std::shared_ptr localization; diff --git a/src/basho/localization/mcl.cpp b/src/basho/localization/mcl.cpp index 13ee237..85f2ef5 100644 --- a/src/basho/localization/mcl.cpp +++ b/src/basho/localization/mcl.cpp @@ -38,14 +38,14 @@ MCL::MCL() num_projected_objects(0), num_particles(100), neff_threshold_ratio(0.75), min_centered_ratio(0.3), short_term_avg_ratio(0.1), long_term_avg_ratio(0.001), base_sigma(10.0), range_sigma(0.05), xvar(10.0), yvar(10.0), chi_square_gate(9.21034), uncertainty_threshold(50.0), max_object_distance(300.0, 300.0), - centered_ratio(0.0), pose_uncertainty(0.0) + centered_ratio(0.0), pose_uncertainty(0.0), initial_avg_weight(true), initial_position(-1.0, -1.0) { } void MCL::localize() { if (initial_localization) { - init_particles(keisan::Point2(-1, -1)); + init_particles(); initial_localization = false; initial_avg_weight = true; state = INIT; @@ -98,14 +98,14 @@ void MCL::localize() clear_projected_objects(); } -void MCL::init_particles(const keisan::Point2 init_position) +void MCL::init_particles() { double uniform_weight = 1.0 / num_particles; particles.clear(); particles.resize(num_particles); - estimated_position = init_position; + estimated_position = initial_position; - if (init_position.x == -1 && init_position.y == -1) { + if (initial_position.x == -1 && initial_position.y == -1) { std::uniform_int_distribution xrg(0, 900); std::uniform_int_distribution yrg(0, 600); @@ -115,8 +115,8 @@ void MCL::init_particles(const keisan::Point2 init_position) particles[i].weight = uniform_weight; } } else { - std::normal_distribution xrg(init_position.x, 50); - std::normal_distribution yrg(init_position.y, 50); + std::normal_distribution xrg(initial_position.x, 50); + std::normal_distribution yrg(initial_position.y, 50); for (int i = 0; i < num_particles; ++i) { particles[i].position = keisan::Point2(xrg(rand_gen), yrg(rand_gen)); diff --git a/src/basho/node/basho_node.cpp b/src/basho/node/basho_node.cpp index 826ed57..aea4f3f 100644 --- a/src/basho/node/basho_node.cpp +++ b/src/basho/node/basho_node.cpp @@ -101,6 +101,16 @@ BashoNode::BashoNode( } }); + init_localization_subscriber = + node->create_subscription( + get_node_prefix() + "/init_localization", + 10, + [this](const Point2::SharedPtr msg) { + auto mcl = this->localization->get_mcl(); + mcl->set_initial_position({msg->x, msg->y}); + mcl->reset(); + }); + set_odometry_publisher = node->create_publisher(aruku::WalkingNode::set_odometry_topic(), 10); From 3b367f8435b89033962deff22523fda0de522323 Mon Sep 17 00:00:00 2001 From: mbsaloka Date: Wed, 25 Mar 2026 18:08:40 +0700 Subject: [PATCH 17/33] feat: fusion mcl with odometry using kf --- CMakeLists.txt | 1 + include/basho/basho.hpp | 1 + include/basho/localization/fusion.hpp | 52 +++++++++++++++++ include/basho/localization/mcl.hpp | 1 + include/basho/node/basho_node.hpp | 1 + include/basho/process/localization.hpp | 4 ++ src/basho/localization/fusion.cpp | 79 ++++++++++++++++++++++++++ src/basho/node/basho_node.cpp | 13 +++-- src/basho/process/localization.cpp | 20 +++++++ 9 files changed, 167 insertions(+), 5 deletions(-) create mode 100644 include/basho/localization/fusion.hpp create mode 100644 src/basho/localization/fusion.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index b70e47a..a35f59c 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -31,6 +31,7 @@ add_library(${PROJECT_NAME} SHARED "src/${PROJECT_NAME}/config/node/config_node.cpp" "src/${PROJECT_NAME}/node/basho_node.cpp" "src/${PROJECT_NAME}/localization/mcl.cpp" + "src/${PROJECT_NAME}/localization/fusion.cpp" "src/${PROJECT_NAME}/process/localization.cpp") target_include_directories(${PROJECT_NAME} PUBLIC diff --git a/include/basho/basho.hpp b/include/basho/basho.hpp index 9a45a8d..d3910ce 100644 --- a/include/basho/basho.hpp +++ b/include/basho/basho.hpp @@ -23,5 +23,6 @@ #include "basho/node/basho_node.hpp" #include "basho/localization/mcl.hpp" +#include "basho/localization/fusion.hpp" #endif // BASHO__BASHO_HPP_ diff --git a/include/basho/localization/fusion.hpp b/include/basho/localization/fusion.hpp new file mode 100644 index 0000000..ac03e83 --- /dev/null +++ b/include/basho/localization/fusion.hpp @@ -0,0 +1,52 @@ +// Copyright (c) 2026 Ichiro ITS +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in +// all copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL +// THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// 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. + +#ifndef BASHO__LOCALIZATION__FUSION_HPP_ +#define BASHO__LOCALIZATION__FUSION_HPP_ + +#include "keisan/keisan.hpp" + +namespace basho { + +class Fusion2D { +public: + Fusion2D(); + ~Fusion2D() = default; + + void init(double x, double y); + void predict(double dx, double dy); + void update(double mx, double my); + + bool is_initialized() const { return initialized; } + + keisan::Point2 getPosition() const; + +private: + keisan::Matrix<2,1> Xk; // state [x, y] + keisan::Matrix<2,2> P; // covariance + keisan::Matrix<2,2> Q; // process noise + keisan::Matrix<2,2> R; // measurement noise + + bool initialized = false; +}; + +} // namespace basho + +#endif // BASHO__LOCALIZATION__FUSION_HPP_ diff --git a/include/basho/localization/mcl.hpp b/include/basho/localization/mcl.hpp index 2bf025c..6a7eecf 100644 --- a/include/basho/localization/mcl.hpp +++ b/include/basho/localization/mcl.hpp @@ -62,6 +62,7 @@ class MCL void set_initial_position(const keisan::Point2 & position) { initial_position = position; } keisan::Point2 get_position() const { return position; } + keisan::Point2 get_delta_position() const { return delta_position; } keisan::Point2 get_estimated_position() const { return estimated_position; } keisan::Angle get_orientation() const { return orientation; } const std::vector & get_particles() const { return particles; } diff --git a/include/basho/node/basho_node.hpp b/include/basho/node/basho_node.hpp index 4a269a1..bafa7bd 100644 --- a/include/basho/node/basho_node.hpp +++ b/include/basho/node/basho_node.hpp @@ -67,6 +67,7 @@ class BashoNode rclcpp::Node::SharedPtr node; rclcpp::Publisher::SharedPtr set_odometry_publisher; + rclcpp::Publisher::SharedPtr fused_position_publisher; rclcpp::Publisher::SharedPtr particles_publisher; rclcpp::Subscription::SharedPtr measurement_status_subscriber; diff --git a/include/basho/process/localization.hpp b/include/basho/process/localization.hpp index 6b43d39..823555c 100644 --- a/include/basho/process/localization.hpp +++ b/include/basho/process/localization.hpp @@ -27,6 +27,7 @@ #include "keisan/keisan.hpp" #include "nlohmann/json.hpp" #include "basho/localization/mcl.hpp" +#include "basho/localization/fusion.hpp" namespace basho { @@ -43,6 +44,8 @@ class Localization void update(); + keisan::Point2 getFusedPose() const; + std::shared_ptr get_mcl() const; std::string config_path; @@ -50,6 +53,7 @@ class Localization private: std::shared_ptr mcl; + Fusion2D fusion; }; } // namespace basho diff --git a/src/basho/localization/fusion.cpp b/src/basho/localization/fusion.cpp new file mode 100644 index 0000000..62b39ee --- /dev/null +++ b/src/basho/localization/fusion.cpp @@ -0,0 +1,79 @@ +// Copyright (c) 2026 Ichiro ITS +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in +// all copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL +// THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// 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. + +#include "basho/localization/fusion.hpp" + +namespace basho { + +Fusion2D::Fusion2D() +{ + Xk = keisan::Matrix<2,1>::zero(); + P = keisan::Matrix<2,2>::identity() * 0.01; + Q = keisan::Matrix<2,2>::identity() * 0.002*0.002; + R = keisan::Matrix<2,2>::identity() * 0.05*0.05; +} + +void Fusion2D::init(double x, double y) +{ + Xk[0][0] = x; + Xk[1][0] = y; + + P = keisan::Matrix<2,2>::identity() * 0.01; + + initialized = true; +} + +void Fusion2D::predict(double dx, double dy) +{ + // Predict state menggunakan delta odometry + Xk[0][0] += dx; + Xk[1][0] += dy; + + // Predict covariance + P = P + Q; +} + +void Fusion2D::update(double mx, double my) +{ + keisan::Matrix<2,1> z; + z[0][0] = mx; + z[1][0] = my; + + // Kalman Gain + keisan::Matrix<2,2> S = P + R; + keisan::Matrix<2,2> K = P * S.inverse2(); + + // Update state + Xk = Xk + K*(z - Xk); + + // Update covariance + keisan::Matrix<2,2> I = keisan::Matrix<2,2>::identity(); + P = (I - K)*P; +} + +keisan::Point2 Fusion2D::getPosition() const +{ + keisan::Point2 pos; + pos.x = Xk[0][0]; + pos.y = Xk[1][0]; + return pos; +} + +} // namespace basho diff --git a/src/basho/node/basho_node.cpp b/src/basho/node/basho_node.cpp index aea4f3f..f3a4df1 100644 --- a/src/basho/node/basho_node.cpp +++ b/src/basho/node/basho_node.cpp @@ -116,6 +116,9 @@ BashoNode::BashoNode( particles_publisher = node->create_publisher(get_node_prefix() + "/particles", 10); + + fused_position_publisher = + node->create_publisher(get_node_prefix() + "/fused_pose", 10); } void BashoNode::update() @@ -123,11 +126,11 @@ void BashoNode::update() localization->update(); publish_particles(); - auto mcl = localization->get_mcl(); - if (mcl->apply_localization) { - publish_odometry(); - mcl->apply_localization = false; - } + auto fused = localization->getFusedPose(); + Point2 msg; + msg.x = fused.x; + msg.y = fused.y; + fused_position_publisher->publish(msg); } void BashoNode::publish_odometry() diff --git a/src/basho/process/localization.cpp b/src/basho/process/localization.cpp index 9209224..2802c4a 100644 --- a/src/basho/process/localization.cpp +++ b/src/basho/process/localization.cpp @@ -124,6 +124,26 @@ void Localization::sync_config() void Localization::update() { mcl->localize(); + + auto delta = mcl->get_delta_position(); + + if (!fusion.is_initialized()) { + auto odom_pos = mcl->get_estimated_position(); + fusion.init(odom_pos.x, odom_pos.y); + } + + fusion.predict(delta.x, delta.y); + + if (mcl->apply_localization) { + auto pos = mcl->get_position(); + fusion.update(pos.x, pos.y); + mcl->apply_localization = false; + } +} + +keisan::Point2 Localization::getFusedPose() const +{ + return fusion.getPosition(); } std::shared_ptr Localization::get_mcl() const From fa75fa8454b922f184ccb9ee24d6737f00e8f50c Mon Sep 17 00:00:00 2001 From: mbsaloka Date: Wed, 25 Mar 2026 18:51:55 +0700 Subject: [PATCH 18/33] feat: adjust landmark with new robocup field --- include/basho/localization/mcl.hpp | 4 +- include/basho/model/field.hpp | 78 ++++++++++++++++-------------- src/basho/localization/mcl.cpp | 4 +- 3 files changed, 47 insertions(+), 39 deletions(-) diff --git a/include/basho/localization/mcl.hpp b/include/basho/localization/mcl.hpp index 2bf025c..e1245e1 100644 --- a/include/basho/localization/mcl.hpp +++ b/include/basho/localization/mcl.hpp @@ -108,7 +108,7 @@ class MCL const keisan::Point2 & landmark, const Particle & particle); - keisan::Matrix<8, 8> calculate_cost_matrix(const Particle & particle, + keisan::Matrix<12, 12> calculate_cost_matrix(const Particle & particle, const std::vector & detected_landmarks, const std::vector & landmarks); @@ -133,7 +133,7 @@ class MCL double centered_ratio; std::mt19937 rand_gen; - keisan::Hungarian<8> hungarian; + keisan::Hungarian<12> hungarian; double weight_avg; double short_term_avg; diff --git a/include/basho/model/field.hpp b/include/basho/model/field.hpp index ec8eae1..fe085ea 100644 --- a/include/basho/model/field.hpp +++ b/include/basho/model/field.hpp @@ -39,41 +39,49 @@ struct Field std::vector landmarks_goalpost; Field() - : width(600), - length(900), - landmarks_L({ - {0.0, 0.0}, - {0.0, 600.0}, - {100.0, 50.0}, - {100.0, 550.0}, - {900.0, 0.0}, - {900.0, 600.0}, - {800.0, 50.0}, - {800.0, 550.0} - }), - landmarks_T({ - {0.0, 50.0}, - {0.0, 550.0}, - {450.0, 0.0}, - {450.0, 600.0}, - {900.0, 50.0}, - {900.0, 550.0} - }), - landmarks_X({ - {210, 300}, - {690, 300}, - {450, 300}, - {450, 375}, - {450, 225} - }), - landmarks_goalpost({ - {0.0, 170.0}, - {0.0, 430.0}, - {900.0, 170.0}, - {900.0, 430.0} - }) - { - } + : width(600), + length(900), + landmarks_L({ + {0.0, 0.0}, // Top-left corner (touchline x goal line) + {0.0, 600.0}, // Bottom-left corner (touchline x goal line) + {900.0, 0.0}, // Top-right corner (touchline x goal line) + {900.0, 600.0}, // Bottom-right corner (touchline x goal line) + {200.0, 100.0}, // Left penalty area - top corner + {200.0, 500.0}, // Left penalty area - bottom corner + {700.0, 100.0}, // Right penalty area - top corner + {700.0, 500.0}, // Right penalty area - bottom corner + {100.0, 150.0}, // Left goal area - top corner + {100.0, 450.0}, // Left goal area - bottom corner + {800.0, 150.0}, // Right goal area - top corner + {800.0, 450.0} // Right goal area - bottom corner + }), + landmarks_T({ + {450.0, 0.0}, // Halfway line x top touchline + {450.0, 600.0}, // Halfway line x bottom touchline + {0.0, 100.0}, // Left penalty area - top intersection with touchline + {0.0, 500.0}, // Left penalty area - bottom intersection with touchline + {900.0, 100.0}, // Right penalty area - top intersection with touchline + {900.0, 500.0}, // Right penalty area - bottom intersection with touchline + {0.0, 150.0}, // Left goal area - top intersection with touchline + {0.0, 450.0}, // Left goal area - bottom intersection with touchline + {900.0, 150.0}, // Right goal area - top intersection with touchline + {900.0, 450.0} // Right goal area - bottom intersection with touchline + }), + landmarks_X({ + {450.0, 300.0}, // Center mark + {450.0, 375.0}, // Halfway line (lower intersection region) + {450.0, 225.0}, // Halfway line (upper intersection region) + {150.0, 300.0}, // Left penalty mark + {750.0, 300.0} // Right penalty mark + }), + landmarks_goalpost({ + {0.0, 210.0}, // Left goal - upper goalpost (on goal line) + {0.0, 390.0}, // Left goal - lower goalpost (on goal line) + {900.0, 210.0}, // Right goal - upper goalpost (on goal line) + {900.0, 390.0} // Right goal - lower goalpost (on goal line) + }) + { + } }; } // namespace basho diff --git a/src/basho/localization/mcl.cpp b/src/basho/localization/mcl.cpp index 85f2ef5..b7b69a4 100644 --- a/src/basho/localization/mcl.cpp +++ b/src/basho/localization/mcl.cpp @@ -388,12 +388,12 @@ double MCL::calculate_object_likelihood( return std::exp(-0.5 * d2); } -keisan::Matrix<8, 8> MCL::calculate_cost_matrix( +keisan::Matrix<12, 12> MCL::calculate_cost_matrix( const Particle & particle, const std::vector & detected_landmarks, const std::vector & landmarks) { - auto cost_matrix = keisan::Matrix<8, 8>::infinite(); + auto cost_matrix = keisan::Matrix<12, 12>::infinite(); for (int i = 0; i < detected_landmarks.size(); ++i) { double dx = detected_landmarks[i].position.x * 100; From f35a6e6b914c22cf8ec21cef8f68fe3b2eb02f9f Mon Sep 17 00:00:00 2001 From: mbsaloka Date: Sun, 29 Mar 2026 12:28:00 +0700 Subject: [PATCH 19/33] feat: add fov landmark gating --- CMakeLists.txt | 2 ++ include/basho/localization/mcl.hpp | 8 ++++++ include/basho/node/basho_node.hpp | 3 ++ src/basho/localization/mcl.cpp | 45 ++++++++++++++++++++++++++---- src/basho/node/basho_node.cpp | 10 +++++++ 5 files changed, 62 insertions(+), 6 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index b70e47a..22b05d9 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -17,6 +17,7 @@ find_package(ament_cmake REQUIRED) find_package(ament_index_cpp REQUIRED) find_package(aruku REQUIRED) find_package(aruku_interfaces REQUIRED) +find_package(atama_interfaces REQUIRED) find_package(gyakuenki_interfaces REQUIRED) find_package(jitsuyo REQUIRED) find_package(kansei REQUIRED) @@ -40,6 +41,7 @@ target_include_directories(${PROJECT_NAME} PUBLIC ament_target_dependencies(${PROJECT_NAME} aruku aruku_interfaces + atama_interfaces gyakuenki_interfaces jitsuyo kansei diff --git a/include/basho/localization/mcl.hpp b/include/basho/localization/mcl.hpp index e1245e1..194a387 100644 --- a/include/basho/localization/mcl.hpp +++ b/include/basho/localization/mcl.hpp @@ -71,6 +71,9 @@ class MCL void set_orientation_roll(const keisan::Angle & roll) { orientation_roll = roll; } void set_orientation_pitch(const keisan::Angle & pitch) { orientation_pitch = pitch; } + void set_head_pan(const keisan::Angle & pan) { head_pan = pan; } + void set_head_tilt(const keisan::Angle & tilt) { head_tilt = tilt; } + int num_particles; double neff_threshold_ratio; double min_centered_ratio; @@ -115,6 +118,8 @@ class MCL double calculate_neff(); double calculate_pose_covariance(); + bool is_landmark_in_fov(const keisan::Point2 & landmark, const Particle & particle); + void print_particles(); keisan::Point2 position; @@ -123,6 +128,9 @@ class MCL keisan::Angle orientation_roll; keisan::Angle orientation_pitch; + keisan::Angle head_pan; + keisan::Angle head_tilt; + keisan::Point2 initial_position; keisan::Point2 estimated_position; diff --git a/include/basho/node/basho_node.hpp b/include/basho/node/basho_node.hpp index 4a269a1..ca132f5 100644 --- a/include/basho/node/basho_node.hpp +++ b/include/basho/node/basho_node.hpp @@ -27,6 +27,7 @@ #include "aruku_interfaces/msg/point2.hpp" #include "aruku_interfaces/msg/status.hpp" +#include "atama_interfaces/msg/head.hpp" #include "basho/localization/mcl.hpp" #include "basho/config/node/config_node.hpp" #include "basho/process/localization.hpp" @@ -47,6 +48,7 @@ class BashoNode using Particles = basho_interfaces::msg::Particles; using Point2 = aruku_interfaces::msg::Point2; using WalkingStatus = aruku_interfaces::msg::Status; + using HeadData = atama_interfaces::msg::Head; static std::string get_node_prefix(); @@ -73,6 +75,7 @@ class BashoNode rclcpp::Subscription::SharedPtr projected_objects_subscriber; rclcpp::Subscription::SharedPtr delta_position_subscriber; rclcpp::Subscription::SharedPtr init_localization_subscriber; + rclcpp::Subscription::SharedPtr head_data_subscriber; std::shared_ptr localization; diff --git a/src/basho/localization/mcl.cpp b/src/basho/localization/mcl.cpp index b7b69a4..a213241 100644 --- a/src/basho/localization/mcl.cpp +++ b/src/basho/localization/mcl.cpp @@ -328,6 +328,7 @@ double MCL::calculate_distance(const keisan::Point2 & point_1, const keisan::Poi double MCL::calculate_total_likelihood(const Particle & particle) { double log_total = 0.0; + int valid_associations = 0; std::vector landmark_groups = { {&projected_X, &field.landmarks_X}, @@ -347,15 +348,26 @@ double MCL::calculate_total_likelihood(const Particle & particle) for (int i = 0; i < group.projected->size(); ++i) { for (int j = 0; j < group.landmarks->size(); ++j) { if (result[i][j] == 1) { + if (cost_matrix[i][j] >= 1e8) { + log_total += std::log(1e-3); + valid_associations++; + continue; + } + double likelihood = calculate_object_likelihood((*group.projected)[i], (*group.landmarks)[j], particle); - log_total += std::log(std::max(likelihood, 1e-9)); + log_total += std::log(std::max(likelihood, 1e-6)); + valid_associations++; } } } } - return std::exp(log_total); + if (valid_associations == 0) { + return 1.0; + } + + return std::exp(log_total / valid_associations); } double MCL::calculate_object_likelihood( @@ -381,10 +393,6 @@ double MCL::calculate_object_likelihood( double d2 = (ex*ex + ey*ey) / var; - if (d2 > chi_square_gate) { - return 1e-9; - } - return std::exp(-0.5 * d2); } @@ -410,6 +418,11 @@ keisan::Matrix<12, 12> MCL::calculate_cost_matrix( double var = sigma * sigma; for (int j = 0; j < landmarks.size(); ++j) { + if (!is_landmark_in_fov(landmarks[j], particle)) { + cost_matrix[i][j] = 1e9; + continue; + } + double ex = landmarks[j].x - px; double ey = landmarks[j].y - py; @@ -428,6 +441,26 @@ keisan::Matrix<12, 12> MCL::calculate_cost_matrix( return cost_matrix; } +bool MCL::is_landmark_in_fov(const keisan::Point2 & landmark, const Particle & particle) +{ + static constexpr double FOV_HALF_DEG = 45.0; + + double dx_world = landmark.x - particle.position.x; + double dy_world = landmark.y - particle.position.y; + + double cos_ori = particle.orientation.cos(); + double sin_ori = particle.orientation.sin(); + double dx_robot = dx_world * cos_ori + dy_world * sin_ori; + double dy_robot = -dx_world * sin_ori + dy_world * cos_ori; + + auto angle_to_landmark = keisan::signed_arctan(dy_robot, dx_robot); + auto gaze = -head_pan; + + auto diff = (angle_to_landmark - gaze).normalize(); + + return std::abs(diff.degree()) <= FOV_HALF_DEG; +} + void MCL::print_particles() { if (!print_debug) { return; diff --git a/src/basho/node/basho_node.cpp b/src/basho/node/basho_node.cpp index aea4f3f..bb6b526 100644 --- a/src/basho/node/basho_node.cpp +++ b/src/basho/node/basho_node.cpp @@ -111,6 +111,16 @@ BashoNode::BashoNode( mcl->reset(); }); + head_data_subscriber = + node->create_subscription( + "head/set_head_data", + 10, + [this](const HeadData::SharedPtr msg) { + auto mcl = this->localization->get_mcl(); + mcl->set_head_pan(keisan::make_degree(msg->pan_angle)); + mcl->set_head_tilt(keisan::make_degree(msg->tilt_angle)); + }); + set_odometry_publisher = node->create_publisher(aruku::WalkingNode::set_odometry_topic(), 10); From ddfc7d26205ade3964e78e494b0d8a0609ad7746 Mon Sep 17 00:00:00 2001 From: mbsaloka Date: Wed, 1 Apr 2026 22:17:46 +0700 Subject: [PATCH 20/33] fix: fusion parameter --- src/basho/localization/fusion.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/basho/localization/fusion.cpp b/src/basho/localization/fusion.cpp index 62b39ee..cdd1bfb 100644 --- a/src/basho/localization/fusion.cpp +++ b/src/basho/localization/fusion.cpp @@ -26,8 +26,8 @@ Fusion2D::Fusion2D() { Xk = keisan::Matrix<2,1>::zero(); P = keisan::Matrix<2,2>::identity() * 0.01; - Q = keisan::Matrix<2,2>::identity() * 0.002*0.002; - R = keisan::Matrix<2,2>::identity() * 0.05*0.05; + Q = keisan::Matrix<2,2>::identity() * 0.02*0.02; + R = keisan::Matrix<2,2>::identity() * 0.1*0.1; } void Fusion2D::init(double x, double y) From bc4514fe6d26c412fb832ce6132fd80599cebecf Mon Sep 17 00:00:00 2001 From: mbsaloka Date: Thu, 2 Apr 2026 00:18:48 +0700 Subject: [PATCH 21/33] fix: reset fusion when init localization --- include/basho/process/localization.hpp | 1 + src/basho/localization/fusion.cpp | 4 ++-- src/basho/node/basho_node.cpp | 6 ++---- src/basho/process/localization.cpp | 7 +++++++ 4 files changed, 12 insertions(+), 6 deletions(-) diff --git a/include/basho/process/localization.hpp b/include/basho/process/localization.hpp index 823555c..9759a52 100644 --- a/include/basho/process/localization.hpp +++ b/include/basho/process/localization.hpp @@ -42,6 +42,7 @@ class Localization void save_config(); void sync_config(); + void reset(keisan::Point2 init_position); void update(); keisan::Point2 getFusedPose() const; diff --git a/src/basho/localization/fusion.cpp b/src/basho/localization/fusion.cpp index cdd1bfb..0f2caf9 100644 --- a/src/basho/localization/fusion.cpp +++ b/src/basho/localization/fusion.cpp @@ -26,8 +26,8 @@ Fusion2D::Fusion2D() { Xk = keisan::Matrix<2,1>::zero(); P = keisan::Matrix<2,2>::identity() * 0.01; - Q = keisan::Matrix<2,2>::identity() * 0.02*0.02; - R = keisan::Matrix<2,2>::identity() * 0.1*0.1; + Q = keisan::Matrix<2,2>::identity() * 0.01*0.01; + R = keisan::Matrix<2,2>::identity() * 0.5*0.5; } void Fusion2D::init(double x, double y) diff --git a/src/basho/node/basho_node.cpp b/src/basho/node/basho_node.cpp index 362c9d9..9dd26b5 100644 --- a/src/basho/node/basho_node.cpp +++ b/src/basho/node/basho_node.cpp @@ -106,9 +106,7 @@ BashoNode::BashoNode( get_node_prefix() + "/init_localization", 10, [this](const Point2::SharedPtr msg) { - auto mcl = this->localization->get_mcl(); - mcl->set_initial_position({msg->x, msg->y}); - mcl->reset(); + this->localization->reset({msg->x, msg->y}); }); head_data_subscriber = @@ -140,7 +138,7 @@ void BashoNode::update() Point2 msg; msg.x = fused.x; msg.y = fused.y; - fused_position_publisher->publish(msg); + set_odometry_publisher->publish(msg); } void BashoNode::publish_odometry() diff --git a/src/basho/process/localization.cpp b/src/basho/process/localization.cpp index 2802c4a..34c839e 100644 --- a/src/basho/process/localization.cpp +++ b/src/basho/process/localization.cpp @@ -121,6 +121,13 @@ void Localization::sync_config() save_config(); } +void Localization::reset(keisan::Point2 init_position) +{ + mcl->set_initial_position(init_position); + mcl->reset(); + fusion.init(init_position.x, init_position.y); +} + void Localization::update() { mcl->localize(); From 02268f582f5db9941b74e3b7479ed41b662bb144 Mon Sep 17 00:00:00 2001 From: mbsaloka Date: Fri, 3 Apr 2026 03:21:12 +0700 Subject: [PATCH 22/33] feat: generalized landmark type --- include/basho/localization/mcl.hpp | 5 +++-- include/basho/model/field.hpp | 5 +++++ src/basho/localization/mcl.cpp | 21 ++++++++++++++++----- src/basho/process/localization.cpp | 2 ++ 4 files changed, 26 insertions(+), 7 deletions(-) diff --git a/include/basho/localization/mcl.hpp b/include/basho/localization/mcl.hpp index 56abbe9..7e85ea0 100644 --- a/include/basho/localization/mcl.hpp +++ b/include/basho/localization/mcl.hpp @@ -85,6 +85,7 @@ class MCL double chi_square_gate; double uncertainty_threshold; keisan::Point2 max_object_distance; + bool generalized_landmark; bool print_debug; bool apply_localization; @@ -112,7 +113,7 @@ class MCL const keisan::Point2 & landmark, const Particle & particle); - keisan::Matrix<12, 12> calculate_cost_matrix(const Particle & particle, + keisan::Matrix<27, 27> calculate_cost_matrix(const Particle & particle, const std::vector & detected_landmarks, const std::vector & landmarks); @@ -142,7 +143,7 @@ class MCL double centered_ratio; std::mt19937 rand_gen; - keisan::Hungarian<12> hungarian; + keisan::Hungarian<27> hungarian; double weight_avg; double short_term_avg; diff --git a/include/basho/model/field.hpp b/include/basho/model/field.hpp index fe085ea..ec8a834 100644 --- a/include/basho/model/field.hpp +++ b/include/basho/model/field.hpp @@ -37,6 +37,7 @@ struct Field std::vector landmarks_T; std::vector landmarks_X; std::vector landmarks_goalpost; + std::vector landmarks_intersection; Field() : width(600), @@ -81,6 +82,10 @@ struct Field {900.0, 390.0} // Right goal - lower goalpost (on goal line) }) { + landmarks_intersection.reserve(landmarks_L.size() + landmarks_T.size() + landmarks_X.size()); + landmarks_intersection.insert(landmarks_intersection.end(), landmarks_L.begin(), landmarks_L.end()); + landmarks_intersection.insert(landmarks_intersection.end(), landmarks_T.begin(), landmarks_T.end()); + landmarks_intersection.insert(landmarks_intersection.end(), landmarks_X.begin(), landmarks_X.end()); } }; diff --git a/src/basho/localization/mcl.cpp b/src/basho/localization/mcl.cpp index a213241..d488b49 100644 --- a/src/basho/localization/mcl.cpp +++ b/src/basho/localization/mcl.cpp @@ -331,12 +331,23 @@ double MCL::calculate_total_likelihood(const Particle & particle) int valid_associations = 0; std::vector landmark_groups = { - {&projected_X, &field.landmarks_X}, - {&projected_L, &field.landmarks_L}, - {&projected_T, &field.landmarks_T}, {&projected_goalpost, &field.landmarks_goalpost} }; + if (generalized_landmark) { + std::vector projected_intersection; + projected_intersection.reserve(projected_T.size() + projected_X.size() + projected_L.size()); + projected_intersection.insert(projected_intersection.end(), projected_T.begin(), projected_T.end()); + projected_intersection.insert(projected_intersection.end(), projected_X.begin(), projected_X.end()); + projected_intersection.insert(projected_intersection.end(), projected_L.begin(), projected_L.end()); + + landmark_groups.push_back({&projected_intersection, &field.landmarks_intersection}); + } else { + landmark_groups.push_back({&projected_X, &field.landmarks_X}); + landmark_groups.push_back({&projected_L, &field.landmarks_L}); + landmark_groups.push_back({&projected_T, &field.landmarks_T}); + } + for (const auto & group : landmark_groups) { if (group.projected->empty() || group.landmarks->empty()) { continue; @@ -396,12 +407,12 @@ double MCL::calculate_object_likelihood( return std::exp(-0.5 * d2); } -keisan::Matrix<12, 12> MCL::calculate_cost_matrix( +keisan::Matrix<27, 27> MCL::calculate_cost_matrix( const Particle & particle, const std::vector & detected_landmarks, const std::vector & landmarks) { - auto cost_matrix = keisan::Matrix<12, 12>::infinite(); + auto cost_matrix = keisan::Matrix<27, 27>::infinite(); for (int i = 0; i < detected_landmarks.size(); ++i) { double dx = detected_landmarks[i].position.x * 100; diff --git a/src/basho/process/localization.cpp b/src/basho/process/localization.cpp index 34c839e..bf3f432 100644 --- a/src/basho/process/localization.cpp +++ b/src/basho/process/localization.cpp @@ -76,6 +76,7 @@ void Localization::set_config(const nlohmann::json & json) valid_section &= jitsuyo::assign_val(localization_section, "max_distance_x", max_distance_x); valid_section &= jitsuyo::assign_val(localization_section, "max_distance_y", max_distance_y); valid_section &= jitsuyo::assign_val(localization_section, "uncertainty_threshold", mcl->uncertainty_threshold); + valid_section &= jitsuyo::assign_val(localization_section, "generalized_landmark", mcl->generalized_landmark); valid_section &= jitsuyo::assign_val(localization_section, "debug", mcl->print_debug); mcl->max_object_distance = {max_distance_x, max_distance_y}; @@ -109,6 +110,7 @@ void Localization::save_config() {"max_distance_x", mcl->max_object_distance.x}, {"max_distance_y", mcl->max_object_distance.y}, {"uncertainty_threshold", mcl->uncertainty_threshold}, + {"generalized_landmark", mcl->generalized_landmark}, {"debug", mcl->print_debug} }; From dca04fba355b4db4edb464e4ac2fbd13ea4989d3 Mon Sep 17 00:00:00 2001 From: mbsaloka Date: Fri, 3 Apr 2026 15:23:43 +0700 Subject: [PATCH 23/33] refactor: move projected_intersection to class attributes --- include/basho/localization/mcl.hpp | 1 + src/basho/localization/mcl.cpp | 7 +------ src/basho/node/basho_node.cpp | 8 +++++--- 3 files changed, 7 insertions(+), 9 deletions(-) diff --git a/include/basho/localization/mcl.hpp b/include/basho/localization/mcl.hpp index 7e85ea0..068a294 100644 --- a/include/basho/localization/mcl.hpp +++ b/include/basho/localization/mcl.hpp @@ -95,6 +95,7 @@ class MCL std::vector projected_X; std::vector projected_L; std::vector projected_T; + std::vector projected_intersection; std::vector projected_goalpost; int num_projected_objects; diff --git a/src/basho/localization/mcl.cpp b/src/basho/localization/mcl.cpp index d488b49..3cae125 100644 --- a/src/basho/localization/mcl.cpp +++ b/src/basho/localization/mcl.cpp @@ -131,6 +131,7 @@ void MCL::clear_projected_objects() projected_X.clear(); projected_L.clear(); projected_T.clear(); + projected_intersection.clear(); projected_goalpost.clear(); num_projected_objects = 0; } @@ -335,12 +336,6 @@ double MCL::calculate_total_likelihood(const Particle & particle) }; if (generalized_landmark) { - std::vector projected_intersection; - projected_intersection.reserve(projected_T.size() + projected_X.size() + projected_L.size()); - projected_intersection.insert(projected_intersection.end(), projected_T.begin(), projected_T.end()); - projected_intersection.insert(projected_intersection.end(), projected_X.begin(), projected_X.end()); - projected_intersection.insert(projected_intersection.end(), projected_L.begin(), projected_L.end()); - landmark_groups.push_back({&projected_intersection, &field.landmarks_intersection}); } else { landmark_groups.push_back({&projected_X, &field.landmarks_X}); diff --git a/src/basho/node/basho_node.cpp b/src/basho/node/basho_node.cpp index 9dd26b5..0af25f9 100644 --- a/src/basho/node/basho_node.cpp +++ b/src/basho/node/basho_node.cpp @@ -87,14 +87,16 @@ BashoNode::BashoNode( {obj.position.x, obj.position.y, obj.position.z} }; - if (obj.label == "X-Intersection") { + if (obj.label == "goalpost") { + mcl->projected_goalpost.push_back(projected); + } else if (mcl->generalized_landmark) { + mcl->projected_intersection.push_back(projected); + } else if (obj.label == "X-Intersection") { mcl->projected_X.push_back(projected); } else if (obj.label == "L-Intersection") { mcl->projected_L.push_back(projected); } else if (obj.label == "T-Intersection") { mcl->projected_T.push_back(projected); - } else if (obj.label == "goalpost") { - mcl->projected_goalpost.push_back(projected); } mcl->num_projected_objects++; From 68a2bb0da6873216f1a5da5440e96eb40304e13b Mon Sep 17 00:00:00 2001 From: mbsaloka Date: Sat, 4 Apr 2026 16:31:55 +0700 Subject: [PATCH 24/33] fix: publish fused position for final estimated pose --- src/basho/node/basho_node.cpp | 2 +- src/basho/process/localization.cpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/src/basho/node/basho_node.cpp b/src/basho/node/basho_node.cpp index 0af25f9..8ef8f30 100644 --- a/src/basho/node/basho_node.cpp +++ b/src/basho/node/basho_node.cpp @@ -140,7 +140,7 @@ void BashoNode::update() Point2 msg; msg.x = fused.x; msg.y = fused.y; - set_odometry_publisher->publish(msg); + fused_position_publisher->publish(msg); } void BashoNode::publish_odometry() diff --git a/src/basho/process/localization.cpp b/src/basho/process/localization.cpp index bf3f432..a044949 100644 --- a/src/basho/process/localization.cpp +++ b/src/basho/process/localization.cpp @@ -126,8 +126,8 @@ void Localization::sync_config() void Localization::reset(keisan::Point2 init_position) { mcl->set_initial_position(init_position); - mcl->reset(); fusion.init(init_position.x, init_position.y); + mcl->reset(); } void Localization::update() From df048c4b9584897b4a874c0bc6d44210194d8eac Mon Sep 17 00:00:00 2001 From: mbsaloka Date: Mon, 6 Apr 2026 20:37:47 +0700 Subject: [PATCH 25/33] feat: estimate position using cluster particles --- include/basho/localization/mcl.hpp | 9 ++- src/basho/localization/mcl.cpp | 113 +++++++++++++++++++++-------- 2 files changed, 88 insertions(+), 34 deletions(-) diff --git a/include/basho/localization/mcl.hpp b/include/basho/localization/mcl.hpp index 068a294..7f9fa1f 100644 --- a/include/basho/localization/mcl.hpp +++ b/include/basho/localization/mcl.hpp @@ -49,6 +49,12 @@ struct LandmarkGroup { std::vector* landmarks; }; +struct ParticleCluster { + keisan::Point2 mean; + double weight_sum; + int count; +}; + class MCL { public: @@ -139,8 +145,7 @@ class MCL Field field; std::vector particles; - Particle* best_particle; - std::vector center_particles; + std::vector best_cluster_particles; double centered_ratio; std::mt19937 rand_gen; diff --git a/src/basho/localization/mcl.cpp b/src/basho/localization/mcl.cpp index 3cae125..9db4faf 100644 --- a/src/basho/localization/mcl.cpp +++ b/src/basho/localization/mcl.cpp @@ -251,33 +251,74 @@ void MCL::estimate_position() { return; } - keisan::Point2 weighted_sum = {0.0, 0.0}; - double weight_sum = 1e-9; - int centered_particles = 0; - center_particles.clear(); +void MCL::estimate_position() +{ + double cluster_radius = 50.0; + double min_weight = 1e-4; + + std::vector clusters; - // Find particles clustered around the best particle for (auto & p : particles) { - double distance = calculate_distance(p.position, best_particle->position); - if (distance < 50.0) { - centered_particles++; - weighted_sum.x += p.weight * p.position.x; - weighted_sum.y += p.weight * p.position.y; - weight_sum += p.weight; + if (p.weight < min_weight) continue; + + bool assigned = false; + + for (auto & c : clusters) { + double dist = calculate_distance(p.position, c.mean); + + if (dist < cluster_radius) { + double new_weight = c.weight_sum + p.weight; + + c.mean.x = (c.mean.x * c.weight_sum + p.position.x * p.weight) / new_weight; + c.mean.y = (c.mean.y * c.weight_sum + p.position.y * p.weight) / new_weight; + + c.weight_sum = new_weight; + c.count++; + assigned = true; + break; + } + } + + // create new cluster + if (!assigned) { + ParticleCluster c; + c.mean = p.position; + c.weight_sum = p.weight; + c.count = 1; + clusters.push_back(c); + } + } + + double best_weight = -1.0; + ParticleCluster* best_cluster = nullptr; - center_particles.push_back(&p); + for (auto & c : clusters) { + if (c.weight_sum > best_weight) { + best_weight = c.weight_sum; + best_cluster = &c; } } - centered_ratio = (double)centered_particles / num_particles; + if (best_cluster == nullptr) return; - // Use mean of centered particles position as estimated position - estimated_position.x = weighted_sum.x / weight_sum; - estimated_position.y = weighted_sum.y / weight_sum; + best_cluster_particles.clear(); + + for (auto & p : particles) { + if (p.weight < min_weight) continue; + + double dist = calculate_distance(p.position, best_cluster->mean); + if (dist < cluster_radius) { + best_cluster_particles.push_back(&p); + } + } + + estimated_position = best_cluster->mean; estimated_position.x = keisan::clamp(estimated_position.x, 0.0, 900.0); estimated_position.y = keisan::clamp(estimated_position.y, 0.0, 600.0); + + centered_ratio = (double)best_cluster_particles.size() / num_particles; } double MCL::calculate_neff() @@ -293,12 +334,16 @@ double MCL::calculate_neff() double MCL::calculate_pose_covariance() { + if (best_cluster_particles.empty()) { + return 1e9; + } + double c_xx = 0.0; double c_yy = 0.0; double c_xy = 0.0; double w_sum = 0.0; - for (auto p : center_particles) { + for (auto p : best_cluster_particles) { double dx = p->position.x - estimated_position.x; double dy = p->position.y - estimated_position.y; @@ -308,14 +353,18 @@ double MCL::calculate_pose_covariance() w_sum += p->weight; } - if (w_sum > 1e-9) { - pose_cov[0][0] = c_xx / w_sum; - pose_cov[1][1] = c_yy / w_sum; - pose_cov[0][1] = c_xy / w_sum; - pose_cov[1][0] = c_xy / w_sum; - } + if (w_sum < 1e-9) return 1e9; + + pose_cov[0][0] = c_xx / w_sum; + pose_cov[1][1] = c_yy / w_sum; + pose_cov[0][1] = c_xy / w_sum; + pose_cov[1][0] = c_xy / w_sum; + + double det = pose_cov[0][0]*pose_cov[1][1] - pose_cov[0][1]*pose_cov[1][0]; + double trace = pose_cov[0][0] + pose_cov[1][1]; + double lambda_max = trace/2 + sqrt((trace*trace)/4 - det); - return pose_cov[0][0] + pose_cov[1][1]; + return lambda_max; } double MCL::calculate_distance(const keisan::Point2 & point_1, const keisan::Point2 & point_2) @@ -474,7 +523,7 @@ void MCL::print_particles() { printf("\033c"); printf("Particles num: %d\n", particles.size()); - printf("Prob: %.2f\n", prob); + printf("Prob: %.5f\n", prob); printf("Short term avg: %.8f\n", short_term_avg); printf("Long term avg: %.8f\n", long_term_avg); printf("Last weight avg: %.8f\n", last_weight_avg); @@ -484,16 +533,16 @@ void MCL::print_particles() { printf("Centered Ratio: %.2f\n", centered_ratio); printf("Minimal Centered Particles: %.0f\n", min_centered_ratio * num_particles); - printf("Centered particles: %d\n", center_particles.size()); - for (int i = 0; i < center_particles.size(); ++i) { + printf("Best cluster particles: %d\n", best_cluster_particles.size()); + for (int i = 0; i < best_cluster_particles.size(); ++i) { printf("Centered Particle %d weight: %.5f [%.2f, %.2f, %.2f]\n", i, - center_particles[i]->weight, - center_particles[i]->position.x, - center_particles[i]->position.y, - center_particles[i]->orientation.degree()); + best_cluster_particles[i]->weight, + best_cluster_particles[i]->position.x, + best_cluster_particles[i]->position.y, + best_cluster_particles[i]->orientation.degree()); if (i >= 5) { - printf("... and %d more\n", center_particles.size() - 5); + printf("... and %d more\n", best_cluster_particles.size() - 5); break; } } From 585245195c1abcd7e494e03ce9f644d6cccbc1ba Mon Sep 17 00:00:00 2001 From: mbsaloka Date: Mon, 6 Apr 2026 20:40:08 +0700 Subject: [PATCH 26/33] feat: add noise for particles orientation --- src/basho/localization/mcl.cpp | 37 ++++++++++++++++++++-------------- 1 file changed, 22 insertions(+), 15 deletions(-) diff --git a/src/basho/localization/mcl.cpp b/src/basho/localization/mcl.cpp index 9db4faf..0a0a82d 100644 --- a/src/basho/localization/mcl.cpp +++ b/src/basho/localization/mcl.cpp @@ -35,10 +35,10 @@ MCL::MCL() delta_position(0.0, 0.0), estimated_position(0.0, 0.0), best_particle(nullptr), print_debug(false), weight_avg(0.0), initial_localization(true), apply_localization(false), prob(0.0), last_weight_avg(0.0), short_term_avg(0.0), long_term_avg(0.0), rand_gen(std::random_device{}()), pending_accept_count(0), - num_projected_objects(0), num_particles(100), neff_threshold_ratio(0.75), min_centered_ratio(0.3), - short_term_avg_ratio(0.1), long_term_avg_ratio(0.001), base_sigma(10.0), range_sigma(0.05), xvar(10.0), - yvar(10.0), chi_square_gate(9.21034), uncertainty_threshold(50.0), max_object_distance(300.0, 300.0), - centered_ratio(0.0), pose_uncertainty(0.0), initial_avg_weight(true), initial_position(-1.0, -1.0) + num_projected_objects(0), num_particles(0), neff_threshold_ratio(0.0), min_centered_ratio(0.0), + short_term_avg_ratio(0.0), long_term_avg_ratio(0.0), base_sigma(0.0), range_sigma(0.0), +chi_square_gate(0.0), uncertainty_threshold(0.0), max_object_distance(0.0, 0.0), centered_ratio(0.0), + pose_uncertainty(0.0), initial_avg_weight(true), initial_position(-1.0, -1.0) { } @@ -138,19 +138,26 @@ void MCL::clear_projected_objects() void MCL::update_motion() { - static std::normal_distribution xgen(0.0, xvar), ygen(0.0, yvar); + double k_trans = 0.05; + double min_trans_noise = 0.5; + + double sigma_theta = 5.0; + + double dx = delta_position.x; + double dy = delta_position.y; + double trans = std::sqrt(dx*dx + dy*dy); + + double sigma_trans = k_trans * trans + min_trans_noise; + + std::normal_distribution noise_x(0.0, sigma_trans); + std::normal_distribution noise_y(0.0, sigma_trans); + std::normal_distribution noise_theta(0.0, sigma_theta); for (auto & p : particles) { - double static_noise_x = xgen(rand_gen) / 10.0; - double static_noise_y = ygen(rand_gen) / 10.0; - double dynamic_noise_x = fabs(delta_position.x) * xgen(rand_gen) / 5.0; - double dynamic_noise_y = fabs(delta_position.y) * ygen(rand_gen) / 5.0; - double x_yterm = fabs(delta_position.y) * xgen(rand_gen) / 30.0; - double y_xterm = fabs(delta_position.x) * ygen(rand_gen) / 30.0; - - p.position.x += delta_position.x + static_noise_x + dynamic_noise_x + x_yterm; - p.position.y += delta_position.y + static_noise_y + dynamic_noise_y + y_xterm; - p.orientation = orientation; + p.position.x += dx + noise_x(rand_gen); + p.position.y += dy + noise_y(rand_gen); + + p.orientation = (orientation + keisan::make_degree(noise_theta(rand_gen))).normalize(); p.position.x = keisan::clamp(p.position.x, 0.0, 900.0); p.position.y = keisan::clamp(p.position.y, 0.0, 600.0); From 07139cb6c38fdeaf898a657aaa0e628efcce1af3 Mon Sep 17 00:00:00 2001 From: mbsaloka Date: Mon, 6 Apr 2026 20:57:16 +0700 Subject: [PATCH 27/33] feat: new resampling method low varience --- include/basho/localization/mcl.hpp | 3 +- src/basho/localization/mcl.cpp | 126 +++++++++++++++++++++-------- 2 files changed, 96 insertions(+), 33 deletions(-) diff --git a/include/basho/localization/mcl.hpp b/include/basho/localization/mcl.hpp index 7f9fa1f..050401b 100644 --- a/include/basho/localization/mcl.hpp +++ b/include/basho/localization/mcl.hpp @@ -110,7 +110,8 @@ class MCL void init_particles(); void update_motion(); void calculate_weight(); - void resample_particles(); + void resample_particles_wheel(); + void resample_particles_low_variance(); void estimate_position(); // Likelihood Calculations diff --git a/src/basho/localization/mcl.cpp b/src/basho/localization/mcl.cpp index 0a0a82d..50b57d3 100644 --- a/src/basho/localization/mcl.cpp +++ b/src/basho/localization/mcl.cpp @@ -32,13 +32,13 @@ namespace basho MCL::MCL() : position(0.0, 0.0), orientation(0_deg), orientation_roll(0_deg), orientation_pitch(0_deg), state(INIT), - delta_position(0.0, 0.0), estimated_position(0.0, 0.0), best_particle(nullptr), print_debug(false), + delta_position(0.0, 0.0), estimated_position(0.0, 0.0), print_debug(false), initial_position(-1.0, -1.0), weight_avg(0.0), initial_localization(true), apply_localization(false), prob(0.0), last_weight_avg(0.0), short_term_avg(0.0), long_term_avg(0.0), rand_gen(std::random_device{}()), pending_accept_count(0), num_projected_objects(0), num_particles(0), neff_threshold_ratio(0.0), min_centered_ratio(0.0), short_term_avg_ratio(0.0), long_term_avg_ratio(0.0), base_sigma(0.0), range_sigma(0.0), -chi_square_gate(0.0), uncertainty_threshold(0.0), max_object_distance(0.0, 0.0), centered_ratio(0.0), - pose_uncertainty(0.0), initial_avg_weight(true), initial_position(-1.0, -1.0) + chi_square_gate(0.0), uncertainty_threshold(0.0), max_object_distance(0.0, 0.0), centered_ratio(0.0), + pose_uncertainty(0.0), initial_avg_weight(true) { } @@ -75,11 +75,6 @@ void MCL::localize() prob = keisan::clamp(1.0 - short_term_avg / (long_term_avg + 1e-6), 0.0, 1.0); - double neff = calculate_neff(); - if (neff < neff_threshold_ratio * num_particles) { - resample_particles(); - } - estimate_position(); pose_uncertainty = calculate_pose_covariance(); @@ -92,6 +87,11 @@ void MCL::localize() } else { state = LOST; } + + if (state == UNCERTAIN || state == LOST) { + // resample_particles_wheel(); + resample_particles_low_variance(); + } } print_particles(); @@ -185,7 +185,7 @@ void MCL::calculate_weight() } } -void MCL::resample_particles() +void MCL::resample_particles_wheel() { std::vector new_particles(num_particles); std::uniform_int_distribution rand_index(0, num_particles - 1); @@ -194,14 +194,6 @@ void MCL::resample_particles() double beta = 0.0; double max_weight = 0.0; - // find the best particle - for (auto & p : particles) { - if (p.weight > max_weight) { - max_weight = p.weight; - best_particle = &p; - } - } - // determine resample interval area int interval_x[2] = {0, 900}; int interval_y[2] = {0, 600}; @@ -222,11 +214,12 @@ void MCL::resample_particles() std::normal_distribution noise_x(0.0, resample_sigma_x); std::normal_distribution noise_y(0.0, resample_sigma_y); + std::normal_distribution noise_theta(0.0, 5.0); for (int i = 0; i < num_particles; ++i) { if (rand_prob(rand_gen) < prob) { new_particles[i].position = keisan::Point2(xrg(rand_gen), yrg(rand_gen)); - new_particles[i].orientation = orientation; + new_particles[i].orientation = (orientation + keisan::make_degree(noise_theta(rand_gen))).normalize(); new_particles[i].weight = 0.0; } else { beta += rand_beta(rand_gen); @@ -239,13 +232,14 @@ void MCL::resample_particles() new_particles[i].position.x += noise_x(rand_gen); new_particles[i].position.y += noise_y(rand_gen); + new_particles[i].orientation = (orientation + keisan::make_degree(noise_theta(rand_gen))).normalize(); } // check is particle out of bound if (new_particles[i].position.x < 0.0 || new_particles[i].position.x > 900.0 || new_particles[i].position.y < 0.0 || new_particles[i].position.y > 600.0) { new_particles[i].position = keisan::Point2(xrg(rand_gen), yrg(rand_gen)); - new_particles[i].orientation = orientation; + new_particles[i].orientation = (orientation + keisan::make_degree(noise_theta(rand_gen))).normalize(); new_particles[i].weight = 0.0; } } @@ -253,11 +247,89 @@ void MCL::resample_particles() particles = new_particles; } -void MCL::estimate_position() { - if (best_particle == nullptr) { - return; +void MCL::resample_particles_low_variance() +{ + std::vector new_particles; + new_particles.reserve(num_particles); + + // Perform systematic sampling to reduce variance compared to random wheel sampling + std::uniform_real_distribution dist_u(0.0, 1.0 / num_particles); + double r = dist_u(rand_gen); + double c = particles[0].weight; + int i = 0; + + // Sample particles based on cumulative weight distribution + for (int m = 0; m < num_particles; ++m) { + double U = r + (double)m / num_particles; + + while (U > c && i < num_particles - 1) { + i++; + c += particles[i].weight; + } + + new_particles.push_back(particles[i]); + } + + // Random particle injection. Used for recovery when particle set collapses or localization is lost + int num_random = prob * num_particles; + + // Limit injection to avoid destroying the current distribution (max 20%) + num_random = std::min(num_random, (int)(0.2 * num_particles)); + + // Noise for injected particles (around estimated position) + std::normal_distribution noise_x(0.0, 50.0); + std::normal_distribution noise_y(0.0, 50.0); + std::normal_distribution noise_theta(0.0, 5.0); + + for (int i = 0; i < num_random; ++i) { + int idx = num_particles - 1 - i; + + // Inject particle near estimated position + new_particles[idx].position.x = estimated_position.x + noise_x(rand_gen); + new_particles[idx].position.y = estimated_position.y + noise_y(rand_gen); + + // Add orientation noise + new_particles[idx].orientation = + (orientation + keisan::make_degree(noise_theta(rand_gen))).normalize(); + + // Assign small weight so it does not dominate immediately + new_particles[idx].weight = 1e-3; } + // If localization is lost, inject more random particles across the field to increase chances of recovery + if (state == LOST) { + num_random = 0.2 * num_particles; + + std::uniform_int_distribution xrg(0, 900); + std::uniform_int_distribution yrg(0, 600); + + for (int i = 0; i < num_random; ++i) { + int idx = rand_gen() % num_particles; + + new_particles[idx].position.x = xrg(rand_gen); + new_particles[idx].position.y = yrg(rand_gen); + new_particles[idx].orientation = (orientation + keisan::make_degree(noise_theta(rand_gen))).normalize(); + + new_particles[idx].weight = 1e-3; + } + } + + // Maintain particle diversity and prevent collapse + std::normal_distribution jitter_x(0.0, 2.0); + std::normal_distribution jitter_y(0.0, 2.0); + + for (auto & p : new_particles) { + p.position.x += jitter_x(rand_gen); + p.position.y += jitter_y(rand_gen); + + // Clamp to map boundaries + p.position.x = keisan::clamp(p.position.x, 0.0, 900.0); + p.position.y = keisan::clamp(p.position.y, 0.0, 600.0); + } + + particles = new_particles; +} + void MCL::estimate_position() { double cluster_radius = 50.0; @@ -554,16 +626,6 @@ void MCL::print_particles() { } } - if (best_particle == nullptr) { - printf("Best particle: NULL\n"); - } else { - printf("Best particle: %.5f [%.2f, %.2f, %.2f]\n", - best_particle->weight, - best_particle->position.x, - best_particle->position.y, - best_particle->orientation.degree()); - } - printf("Num objects: %d\n", num_projected_objects); printf("Num particles: %d\n", num_particles); printf("Initial Localization %d\n", initial_localization); From 55207d6598bfb549ac2b4575326d9780476aeb31 Mon Sep 17 00:00:00 2001 From: mbsaloka Date: Mon, 6 Apr 2026 21:02:44 +0700 Subject: [PATCH 28/33] feat: subscribe to odometry and random inject near odometry when lost --- include/basho/localization/mcl.hpp | 3 +++ include/basho/node/basho_node.hpp | 1 + src/basho/localization/mcl.cpp | 19 ++++++++++++++++--- src/basho/node/basho_node.cpp | 10 ++++++++++ 4 files changed, 30 insertions(+), 3 deletions(-) diff --git a/include/basho/localization/mcl.hpp b/include/basho/localization/mcl.hpp index 050401b..bb2c678 100644 --- a/include/basho/localization/mcl.hpp +++ b/include/basho/localization/mcl.hpp @@ -81,6 +81,8 @@ class MCL void set_head_pan(const keisan::Angle & pan) { head_pan = pan; } void set_head_tilt(const keisan::Angle & tilt) { head_tilt = tilt; } + void set_odometry_position(const keisan::Point2 & odom) { odometry_position = odom; } + int num_particles; double neff_threshold_ratio; double min_centered_ratio; @@ -143,6 +145,7 @@ class MCL keisan::Point2 initial_position; keisan::Point2 estimated_position; + keisan::Point2 odometry_position; Field field; std::vector particles; diff --git a/include/basho/node/basho_node.hpp b/include/basho/node/basho_node.hpp index dfd7728..848a8c9 100644 --- a/include/basho/node/basho_node.hpp +++ b/include/basho/node/basho_node.hpp @@ -74,6 +74,7 @@ class BashoNode rclcpp::Subscription::SharedPtr measurement_status_subscriber; rclcpp::Subscription::SharedPtr projected_objects_subscriber; + rclcpp::Subscription::SharedPtr walking_status_subscriber; rclcpp::Subscription::SharedPtr delta_position_subscriber; rclcpp::Subscription::SharedPtr init_localization_subscriber; rclcpp::Subscription::SharedPtr head_data_subscriber; diff --git a/src/basho/localization/mcl.cpp b/src/basho/localization/mcl.cpp index 50b57d3..92cc8dc 100644 --- a/src/basho/localization/mcl.cpp +++ b/src/basho/localization/mcl.cpp @@ -298,13 +298,15 @@ void MCL::resample_particles_low_variance() // If localization is lost, inject more random particles across the field to increase chances of recovery if (state == LOST) { - num_random = 0.2 * num_particles; + int lost_inject_global = 0.2 * num_particles; + int lost_inject_odom = 0.1 * num_particles; std::uniform_int_distribution xrg(0, 900); std::uniform_int_distribution yrg(0, 600); - for (int i = 0; i < num_random; ++i) { - int idx = rand_gen() % num_particles; + // Global random injection + for (int i = num_random; i < num_random + lost_inject_global; ++i) { + int idx = num_particles - 1 - i; new_particles[idx].position.x = xrg(rand_gen); new_particles[idx].position.y = yrg(rand_gen); @@ -312,6 +314,17 @@ void MCL::resample_particles_low_variance() new_particles[idx].weight = 1e-3; } + + // Inject near odometry position + for (int i = num_random + lost_inject_global; i < num_random + lost_inject_global + lost_inject_odom; ++i) { + int idx = num_particles - 1 - i; + + new_particles[idx].position.x = odometry_position.x + noise_x(rand_gen); + new_particles[idx].position.y = odometry_position.y + noise_y(rand_gen); + new_particles[idx].orientation = (orientation + keisan::make_degree(noise_theta(rand_gen))).normalize(); + + new_particles[idx].weight = 1e-3; + } } // Maintain particle diversity and prevent collapse diff --git a/src/basho/node/basho_node.cpp b/src/basho/node/basho_node.cpp index 8ef8f30..54da123 100644 --- a/src/basho/node/basho_node.cpp +++ b/src/basho/node/basho_node.cpp @@ -61,6 +61,16 @@ BashoNode::BashoNode( mcl->set_delta_position({msg->x, msg->y}); }); + walking_status_subscriber = + node->create_subscription( + aruku::WalkingNode::status_topic(), + 10, + [this](const WalkingStatus::SharedPtr message) + { + auto mcl = this->localization->get_mcl(); + mcl->set_odometry_position({message->odometry.x, message->odometry.y}); + }); + projected_objects_subscriber = node->create_subscription( "/gyakuenki_cpp/projected_objects", From 2ffa9b3087816a611c155ba83df1ee67fba51910 Mon Sep 17 00:00:00 2001 From: mbsaloka Date: Mon, 6 Apr 2026 22:27:00 +0700 Subject: [PATCH 29/33] fix: estimate pose when no landmark detected --- src/basho/localization/mcl.cpp | 78 +++++++++++++++++----------------- 1 file changed, 39 insertions(+), 39 deletions(-) diff --git a/src/basho/localization/mcl.cpp b/src/basho/localization/mcl.cpp index 92cc8dc..a493fbb 100644 --- a/src/basho/localization/mcl.cpp +++ b/src/basho/localization/mcl.cpp @@ -74,24 +74,24 @@ void MCL::localize() long_term_avg += long_term_avg_ratio * (weight_avg - long_term_avg); prob = keisan::clamp(1.0 - short_term_avg / (long_term_avg + 1e-6), 0.0, 1.0); + } - estimate_position(); - pose_uncertainty = calculate_pose_covariance(); + estimate_position(); + pose_uncertainty = calculate_pose_covariance(); - if (centered_ratio < min_centered_ratio) { - state = LOST; - } else if (pose_uncertainty < uncertainty_threshold * 1.5) { - position = estimated_position; - apply_localization = true; - state = pose_uncertainty < uncertainty_threshold ? TRACKING : UNCERTAIN; - } else { - state = LOST; - } + if (centered_ratio < min_centered_ratio) { + state = LOST; + } else if (pose_uncertainty < uncertainty_threshold * 1.5) { + position = estimated_position; + apply_localization = true; + state = pose_uncertainty < uncertainty_threshold ? TRACKING : UNCERTAIN; + } else { + state = LOST; + } - if (state == UNCERTAIN || state == LOST) { - // resample_particles_wheel(); - resample_particles_low_variance(); - } + if (state == UNCERTAIN || state == LOST) { + // resample_particles_wheel(); + resample_particles_low_variance(); } print_particles(); @@ -277,8 +277,8 @@ void MCL::resample_particles_low_variance() num_random = std::min(num_random, (int)(0.2 * num_particles)); // Noise for injected particles (around estimated position) - std::normal_distribution noise_x(0.0, 50.0); - std::normal_distribution noise_y(0.0, 50.0); + std::normal_distribution noise_x(0.0, 100.0); + std::normal_distribution noise_y(0.0, 100.0); std::normal_distribution noise_theta(0.0, 5.0); for (int i = 0; i < num_random; ++i) { @@ -297,35 +297,35 @@ void MCL::resample_particles_low_variance() } // If localization is lost, inject more random particles across the field to increase chances of recovery - if (state == LOST) { - int lost_inject_global = 0.2 * num_particles; - int lost_inject_odom = 0.1 * num_particles; + // if (state == LOST) { + // int lost_inject_global = 0.1 * num_particles; + // int lost_inject_odom = 0.1 * num_particles; - std::uniform_int_distribution xrg(0, 900); - std::uniform_int_distribution yrg(0, 600); + // std::uniform_int_distribution xrg(0, 900); + // std::uniform_int_distribution yrg(0, 600); - // Global random injection - for (int i = num_random; i < num_random + lost_inject_global; ++i) { - int idx = num_particles - 1 - i; + // // Global random injection + // for (int i = num_random; i < num_random + lost_inject_global; ++i) { + // int idx = num_particles - 1 - i; - new_particles[idx].position.x = xrg(rand_gen); - new_particles[idx].position.y = yrg(rand_gen); - new_particles[idx].orientation = (orientation + keisan::make_degree(noise_theta(rand_gen))).normalize(); + // new_particles[idx].position.x = xrg(rand_gen); + // new_particles[idx].position.y = yrg(rand_gen); + // new_particles[idx].orientation = (orientation + keisan::make_degree(noise_theta(rand_gen))).normalize(); - new_particles[idx].weight = 1e-3; - } + // new_particles[idx].weight = 1e-3; + // } - // Inject near odometry position - for (int i = num_random + lost_inject_global; i < num_random + lost_inject_global + lost_inject_odom; ++i) { - int idx = num_particles - 1 - i; + // // Inject near odometry position + // for (int i = num_random + lost_inject_global; i < num_random + lost_inject_global + lost_inject_odom; ++i) { + // int idx = num_particles - 1 - i; - new_particles[idx].position.x = odometry_position.x + noise_x(rand_gen); - new_particles[idx].position.y = odometry_position.y + noise_y(rand_gen); - new_particles[idx].orientation = (orientation + keisan::make_degree(noise_theta(rand_gen))).normalize(); + // new_particles[idx].position.x = odometry_position.x + noise_x(rand_gen); + // new_particles[idx].position.y = odometry_position.y + noise_y(rand_gen); + // new_particles[idx].orientation = (orientation + keisan::make_degree(noise_theta(rand_gen))).normalize(); - new_particles[idx].weight = 1e-3; - } - } + // new_particles[idx].weight = 1e-3; + // } + // } // Maintain particle diversity and prevent collapse std::normal_distribution jitter_x(0.0, 2.0); From c1f8b4721cd6893f144d43acd277c49cced7a863 Mon Sep 17 00:00:00 2001 From: mbsaloka Date: Wed, 8 Apr 2026 23:23:05 +0700 Subject: [PATCH 30/33] fix: remove neff calculation --- include/basho/localization/mcl.hpp | 2 -- src/basho/localization/mcl.cpp | 23 ++++++----------------- src/basho/process/localization.cpp | 2 -- 3 files changed, 6 insertions(+), 21 deletions(-) diff --git a/include/basho/localization/mcl.hpp b/include/basho/localization/mcl.hpp index bb2c678..31efd40 100644 --- a/include/basho/localization/mcl.hpp +++ b/include/basho/localization/mcl.hpp @@ -84,7 +84,6 @@ class MCL void set_odometry_position(const keisan::Point2 & odom) { odometry_position = odom; } int num_particles; - double neff_threshold_ratio; double min_centered_ratio; double short_term_avg_ratio; double long_term_avg_ratio; @@ -127,7 +126,6 @@ class MCL const std::vector & detected_landmarks, const std::vector & landmarks); - double calculate_neff(); double calculate_pose_covariance(); bool is_landmark_in_fov(const keisan::Point2 & landmark, const Particle & particle); diff --git a/src/basho/localization/mcl.cpp b/src/basho/localization/mcl.cpp index a493fbb..fc67821 100644 --- a/src/basho/localization/mcl.cpp +++ b/src/basho/localization/mcl.cpp @@ -35,10 +35,9 @@ MCL::MCL() delta_position(0.0, 0.0), estimated_position(0.0, 0.0), print_debug(false), initial_position(-1.0, -1.0), weight_avg(0.0), initial_localization(true), apply_localization(false), prob(0.0), last_weight_avg(0.0), short_term_avg(0.0), long_term_avg(0.0), rand_gen(std::random_device{}()), pending_accept_count(0), - num_projected_objects(0), num_particles(0), neff_threshold_ratio(0.0), min_centered_ratio(0.0), - short_term_avg_ratio(0.0), long_term_avg_ratio(0.0), base_sigma(0.0), range_sigma(0.0), - chi_square_gate(0.0), uncertainty_threshold(0.0), max_object_distance(0.0, 0.0), centered_ratio(0.0), - pose_uncertainty(0.0), initial_avg_weight(true) + num_projected_objects(0), min_centered_ratio(0.0), pose_uncertainty(0.0), initial_avg_weight(true), + num_particles(0), short_term_avg_ratio(0.0), long_term_avg_ratio(0.0), base_sigma(0.0), range_sigma(0.0), + chi_square_gate(0.0), uncertainty_threshold(0.0), max_object_distance(0.0, 0.0), centered_ratio(0.0) { } @@ -58,6 +57,7 @@ void MCL::localize() bool evaluate_particles = num_projected_objects > 0; evaluate_particles &= fabs(orientation_roll.degree()) <= 30; evaluate_particles &= fabs(orientation_pitch.degree()) <= 30; + evaluate_particles &= fabs(head_pan.degree()) <= 60; apply_localization = false; @@ -277,8 +277,8 @@ void MCL::resample_particles_low_variance() num_random = std::min(num_random, (int)(0.2 * num_particles)); // Noise for injected particles (around estimated position) - std::normal_distribution noise_x(0.0, 100.0); - std::normal_distribution noise_y(0.0, 100.0); + std::normal_distribution noise_x(0.0, 50.0); + std::normal_distribution noise_y(0.0, 50.0); std::normal_distribution noise_theta(0.0, 5.0); for (int i = 0; i < num_random; ++i) { @@ -413,17 +413,6 @@ void MCL::estimate_position() centered_ratio = (double)best_cluster_particles.size() / num_particles; } -double MCL::calculate_neff() -{ - double sum_sq = 1e-9; - - for (auto & p : particles) { - sum_sq += p.weight * p.weight; - } - - return 1.0 / sum_sq; -} - double MCL::calculate_pose_covariance() { if (best_cluster_particles.empty()) { diff --git a/src/basho/process/localization.cpp b/src/basho/process/localization.cpp index a044949..023b8e1 100644 --- a/src/basho/process/localization.cpp +++ b/src/basho/process/localization.cpp @@ -66,7 +66,6 @@ void Localization::set_config(const nlohmann::json & json) double max_distance_y = 0.0; valid_section &= jitsuyo::assign_val(localization_section, "num_particles", mcl->num_particles); - valid_section &= jitsuyo::assign_val(localization_section, "neff_threshold_ratio", mcl->neff_threshold_ratio); valid_section &= jitsuyo::assign_val(localization_section, "min_centered_ratio", mcl->min_centered_ratio); valid_section &= jitsuyo::assign_val(localization_section, "short_term_avg_ratio", mcl->short_term_avg_ratio); valid_section &= jitsuyo::assign_val(localization_section, "long_term_avg_ratio", mcl->long_term_avg_ratio); @@ -100,7 +99,6 @@ void Localization::save_config() config["localization"] = { {"num_particles", mcl->num_particles}, - {"neff_threshold_ratio", mcl->neff_threshold_ratio}, {"min_centered_ratio", mcl->min_centered_ratio}, {"short_term_avg_ratio", mcl->short_term_avg_ratio}, {"long_term_avg_ratio", mcl->long_term_avg_ratio}, From d4dd29e2caabbcc176b6f3704a120177c1ae73b9 Mon Sep 17 00:00:00 2001 From: mbsaloka Date: Mon, 13 Apr 2026 14:40:17 +0700 Subject: [PATCH 31/33] feat: inject particles when state lost --- include/basho/localization/fusion.hpp | 24 +++++------ include/basho/localization/mcl.hpp | 4 ++ src/basho/localization/fusion.cpp | 58 ++++++++++++++------------- src/basho/localization/mcl.cpp | 49 +++++++++++----------- src/basho/process/localization.cpp | 2 +- 5 files changed, 73 insertions(+), 64 deletions(-) diff --git a/include/basho/localization/fusion.hpp b/include/basho/localization/fusion.hpp index ac03e83..0b8a5ff 100644 --- a/include/basho/localization/fusion.hpp +++ b/include/basho/localization/fusion.hpp @@ -27,24 +27,24 @@ namespace basho { class Fusion2D { public: - Fusion2D(); - ~Fusion2D() = default; + Fusion2D(); + ~Fusion2D() = default; - void init(double x, double y); - void predict(double dx, double dy); - void update(double mx, double my); + void init(double x, double y); + void predict(double dx, double dy); + void update(double mx, double my); - bool is_initialized() const { return initialized; } + bool is_initialized() const { return initialized; } - keisan::Point2 getPosition() const; + keisan::Point2 getPosition() const; private: - keisan::Matrix<2,1> Xk; // state [x, y] - keisan::Matrix<2,2> P; // covariance - keisan::Matrix<2,2> Q; // process noise - keisan::Matrix<2,2> R; // measurement noise + keisan::Matrix<2,1> Xk; // state [x, y] + keisan::Matrix<2,2> P; // covariance + keisan::Matrix<2,2> Q; // process noise + keisan::Matrix<2,2> R; // measurement noise - bool initialized = false; + bool initialized = false; }; } // namespace basho diff --git a/include/basho/localization/mcl.hpp b/include/basho/localization/mcl.hpp index 31efd40..25e37a4 100644 --- a/include/basho/localization/mcl.hpp +++ b/include/basho/localization/mcl.hpp @@ -73,6 +73,8 @@ class MCL keisan::Angle get_orientation() const { return orientation; } const std::vector & get_particles() const { return particles; } + keisan::Point2 get_odometry_position() const { return odometry_position; } + void set_delta_position(const keisan::Point2 & delta) { delta_position = delta; } void set_orientation(const keisan::Angle & yaw) { orientation = yaw; } void set_orientation_roll(const keisan::Angle & roll) { orientation_roll = roll; } @@ -168,6 +170,8 @@ class MCL double yvar; keisan::Matrix<2,2> pose_cov; double pose_uncertainty; + + int lost_count; }; } // namespace basho diff --git a/src/basho/localization/fusion.cpp b/src/basho/localization/fusion.cpp index 0f2caf9..7000d34 100644 --- a/src/basho/localization/fusion.cpp +++ b/src/basho/localization/fusion.cpp @@ -24,56 +24,58 @@ namespace basho { Fusion2D::Fusion2D() { - Xk = keisan::Matrix<2,1>::zero(); - P = keisan::Matrix<2,2>::identity() * 0.01; - Q = keisan::Matrix<2,2>::identity() * 0.01*0.01; - R = keisan::Matrix<2,2>::identity() * 0.5*0.5; + Xk = keisan::Matrix<2,1>::zero(); + P = keisan::Matrix<2,2>::identity() * 0.01; + Q = keisan::Matrix<2,2>::identity() * 0.01*0.01; + R = keisan::Matrix<2,2>::identity() * 0.5*0.5; + + initialized = false; } void Fusion2D::init(double x, double y) { - Xk[0][0] = x; - Xk[1][0] = y; + Xk[0][0] = x; + Xk[1][0] = y; - P = keisan::Matrix<2,2>::identity() * 0.01; + P = keisan::Matrix<2,2>::identity() * 0.01; - initialized = true; + initialized = true; } void Fusion2D::predict(double dx, double dy) { - // Predict state menggunakan delta odometry - Xk[0][0] += dx; - Xk[1][0] += dy; + // Predict state using delta odometry + Xk[0][0] += dx; + Xk[1][0] += dy; - // Predict covariance - P = P + Q; + // Predict covariance + P = P + Q; } void Fusion2D::update(double mx, double my) { - keisan::Matrix<2,1> z; - z[0][0] = mx; - z[1][0] = my; + keisan::Matrix<2,1> z; + z[0][0] = mx; + z[1][0] = my; - // Kalman Gain - keisan::Matrix<2,2> S = P + R; - keisan::Matrix<2,2> K = P * S.inverse2(); + // Kalman Gain + keisan::Matrix<2,2> S = P + R; + keisan::Matrix<2,2> K = P * S.inverse2(); - // Update state - Xk = Xk + K*(z - Xk); + // Update state + Xk = Xk + K*(z - Xk); - // Update covariance - keisan::Matrix<2,2> I = keisan::Matrix<2,2>::identity(); - P = (I - K)*P; + // Update covariance + keisan::Matrix<2,2> I = keisan::Matrix<2,2>::identity(); + P = (I - K)*P; } keisan::Point2 Fusion2D::getPosition() const { - keisan::Point2 pos; - pos.x = Xk[0][0]; - pos.y = Xk[1][0]; - return pos; + keisan::Point2 pos; + pos.x = Xk[0][0]; + pos.y = Xk[1][0]; + return pos; } } // namespace basho diff --git a/src/basho/localization/mcl.cpp b/src/basho/localization/mcl.cpp index fc67821..779d694 100644 --- a/src/basho/localization/mcl.cpp +++ b/src/basho/localization/mcl.cpp @@ -81,12 +81,15 @@ void MCL::localize() if (centered_ratio < min_centered_ratio) { state = LOST; + lost_count++; } else if (pose_uncertainty < uncertainty_threshold * 1.5) { position = estimated_position; apply_localization = true; state = pose_uncertainty < uncertainty_threshold ? TRACKING : UNCERTAIN; + lost_count = 0; } else { state = LOST; + lost_count++; } if (state == UNCERTAIN || state == LOST) { @@ -297,35 +300,35 @@ void MCL::resample_particles_low_variance() } // If localization is lost, inject more random particles across the field to increase chances of recovery - // if (state == LOST) { - // int lost_inject_global = 0.1 * num_particles; - // int lost_inject_odom = 0.1 * num_particles; + if (lost_count > 10) { + int lost_inject_global = 0.1 * num_particles; + int lost_inject_odom = 0.1 * num_particles; - // std::uniform_int_distribution xrg(0, 900); - // std::uniform_int_distribution yrg(0, 600); + std::uniform_int_distribution xrg(0, 900); + std::uniform_int_distribution yrg(0, 600); - // // Global random injection - // for (int i = num_random; i < num_random + lost_inject_global; ++i) { - // int idx = num_particles - 1 - i; + // Global random injection + for (int i = num_random; i < num_random + lost_inject_global; ++i) { + int idx = num_particles - 1 - i; - // new_particles[idx].position.x = xrg(rand_gen); - // new_particles[idx].position.y = yrg(rand_gen); - // new_particles[idx].orientation = (orientation + keisan::make_degree(noise_theta(rand_gen))).normalize(); + new_particles[idx].position.x = xrg(rand_gen); + new_particles[idx].position.y = yrg(rand_gen); + new_particles[idx].orientation = (orientation + keisan::make_degree(noise_theta(rand_gen))).normalize(); - // new_particles[idx].weight = 1e-3; - // } + new_particles[idx].weight = 1e-3; + } - // // Inject near odometry position - // for (int i = num_random + lost_inject_global; i < num_random + lost_inject_global + lost_inject_odom; ++i) { - // int idx = num_particles - 1 - i; + // Inject near odometry position + for (int i = num_random + lost_inject_global; i < num_random + lost_inject_global + lost_inject_odom; ++i) { + int idx = num_particles - 1 - i; - // new_particles[idx].position.x = odometry_position.x + noise_x(rand_gen); - // new_particles[idx].position.y = odometry_position.y + noise_y(rand_gen); - // new_particles[idx].orientation = (orientation + keisan::make_degree(noise_theta(rand_gen))).normalize(); + new_particles[idx].position.x = odometry_position.x + noise_x(rand_gen); + new_particles[idx].position.y = odometry_position.y + noise_y(rand_gen); + new_particles[idx].orientation = (orientation + keisan::make_degree(noise_theta(rand_gen))).normalize(); - // new_particles[idx].weight = 1e-3; - // } - // } + new_particles[idx].weight = 1e-3; + } + } // Maintain particle diversity and prevent collapse std::normal_distribution jitter_x(0.0, 2.0); @@ -445,7 +448,7 @@ double MCL::calculate_pose_covariance() double trace = pose_cov[0][0] + pose_cov[1][1]; double lambda_max = trace/2 + sqrt((trace*trace)/4 - det); - return lambda_max; + return sqrt(lambda_max); } double MCL::calculate_distance(const keisan::Point2 & point_1, const keisan::Point2 & point_2) diff --git a/src/basho/process/localization.cpp b/src/basho/process/localization.cpp index 023b8e1..d34b94d 100644 --- a/src/basho/process/localization.cpp +++ b/src/basho/process/localization.cpp @@ -135,7 +135,7 @@ void Localization::update() auto delta = mcl->get_delta_position(); if (!fusion.is_initialized()) { - auto odom_pos = mcl->get_estimated_position(); + auto odom_pos = mcl->get_odometry_position(); fusion.init(odom_pos.x, odom_pos.y); } From 9a2d2dd1d8dfce10b2133d61b8f04cf4b8f4cddb Mon Sep 17 00:00:00 2001 From: mbsaloka Date: Mon, 13 Apr 2026 22:07:15 +0700 Subject: [PATCH 32/33] feat: add goalpost trilateration for particle hypothesis injection --- include/basho/localization/mcl.hpp | 5 + src/basho/localization/mcl.cpp | 173 ++++++++++++++++++++++------- 2 files changed, 139 insertions(+), 39 deletions(-) diff --git a/include/basho/localization/mcl.hpp b/include/basho/localization/mcl.hpp index 25e37a4..28285fa 100644 --- a/include/basho/localization/mcl.hpp +++ b/include/basho/localization/mcl.hpp @@ -119,6 +119,9 @@ class MCL // Likelihood Calculations double calculate_distance(const keisan::Point2 & point_1, const keisan::Point2 & point_2); + keisan::Point2 rotate_to_global_frame(const keisan::Point2 & local_pos, + const keisan::Angle & theta); + double calculate_total_likelihood(const Particle & particle); double calculate_object_likelihood(const ProjectedObject & measurement, const keisan::Point2 & landmark, @@ -132,6 +135,8 @@ class MCL bool is_landmark_in_fov(const keisan::Point2 & landmark, const Particle & particle); + bool compute_goalpost_pose_hypothesis(keisan::Point2 & out_pos); + void print_particles(); keisan::Point2 position; diff --git a/src/basho/localization/mcl.cpp b/src/basho/localization/mcl.cpp index 779d694..b3e393f 100644 --- a/src/basho/localization/mcl.cpp +++ b/src/basho/localization/mcl.cpp @@ -92,7 +92,7 @@ void MCL::localize() lost_count++; } - if (state == UNCERTAIN || state == LOST) { + if ((state == UNCERTAIN || state == LOST) && evaluate_particles) { // resample_particles_wheel(); resample_particles_low_variance(); } @@ -109,8 +109,8 @@ void MCL::init_particles() estimated_position = initial_position; if (initial_position.x == -1 && initial_position.y == -1) { - std::uniform_int_distribution xrg(0, 900); - std::uniform_int_distribution yrg(0, 600); + std::uniform_int_distribution xrg(-10, 910); + std::uniform_int_distribution yrg(-10, 610); for (int i = 0; i < num_particles; ++i) { particles[i].position = keisan::Point2(xrg(rand_gen), yrg(rand_gen)); @@ -162,8 +162,8 @@ void MCL::update_motion() p.orientation = (orientation + keisan::make_degree(noise_theta(rand_gen))).normalize(); - p.position.x = keisan::clamp(p.position.x, 0.0, 900.0); - p.position.y = keisan::clamp(p.position.y, 0.0, 600.0); + p.position.x = keisan::clamp(p.position.x, -10.0, 910.0); + p.position.y = keisan::clamp(p.position.y, -10.0, 610.0); } estimated_position += delta_position; @@ -198,8 +198,8 @@ void MCL::resample_particles_wheel() double max_weight = 0.0; // determine resample interval area - int interval_x[2] = {0, 900}; - int interval_y[2] = {0, 600}; + int interval_x[2] = {-10, 910}; + int interval_y[2] = {-10, 610}; // resample particles std::uniform_real_distribution rand_prob(0.0, 1.0); @@ -280,9 +280,10 @@ void MCL::resample_particles_low_variance() num_random = std::min(num_random, (int)(0.2 * num_particles)); // Noise for injected particles (around estimated position) - std::normal_distribution noise_x(0.0, 50.0); - std::normal_distribution noise_y(0.0, 50.0); - std::normal_distribution noise_theta(0.0, 5.0); + // TOOD: get noise value from config + std::normal_distribution noise_x(0.0, 75.0); + std::normal_distribution noise_y(0.0, 75.0); + std::normal_distribution noise_theta(0.0, 8.0); for (int i = 0; i < num_random; ++i) { int idx = num_particles - 1 - i; @@ -299,27 +300,30 @@ void MCL::resample_particles_low_variance() new_particles[idx].weight = 1e-3; } - // If localization is lost, inject more random particles across the field to increase chances of recovery - if (lost_count > 10) { - int lost_inject_global = 0.1 * num_particles; - int lost_inject_odom = 0.1 * num_particles; + // If localization is lost, inject more random particles to increase chances of recovery + // int num_inject_global = 0; + int num_inject_odom = 0; + int num_inject_goalpost = 0; - std::uniform_int_distribution xrg(0, 900); - std::uniform_int_distribution yrg(0, 600); + if (state == LOST && lost_count > 10) { + // num_inject_global = 0.1 * num_particles; + num_inject_odom = 0.1 * num_particles; // Global random injection - for (int i = num_random; i < num_random + lost_inject_global; ++i) { - int idx = num_particles - 1 - i; + // std::uniform_int_distribution xrg(-10, 910); + // std::uniform_int_distribution yrg(-10, 610); + // for (int i = num_random; i < num_random + lost_inject_global; ++i) { + // int idx = num_particles - 1 - i; - new_particles[idx].position.x = xrg(rand_gen); - new_particles[idx].position.y = yrg(rand_gen); - new_particles[idx].orientation = (orientation + keisan::make_degree(noise_theta(rand_gen))).normalize(); + // new_particles[idx].position.x = xrg(rand_gen); + // new_particles[idx].position.y = yrg(rand_gen); + // new_particles[idx].orientation = (orientation + keisan::make_degree(noise_theta(rand_gen))).normalize(); - new_particles[idx].weight = 1e-3; - } + // new_particles[idx].weight = 1e-3; + // } // Inject near odometry position - for (int i = num_random + lost_inject_global; i < num_random + lost_inject_global + lost_inject_odom; ++i) { + for (int i = num_random; i < num_random + num_inject_odom; ++i) { int idx = num_particles - 1 - i; new_particles[idx].position.x = odometry_position.x + noise_x(rand_gen); @@ -330,6 +334,24 @@ void MCL::resample_particles_low_variance() } } + // Inject particles around goalpost hypothesis if available + if (projected_goalpost.size() >= 2) { + keisan::Point2 goalpost_hypothesis; + int num_inject_goalpost = 0.1 * num_particles; + + if (compute_goalpost_pose_hypothesis(goalpost_hypothesis)) { + for (int i = num_random + num_inject_odom; i < num_random + num_inject_goalpost + num_inject_goalpost; ++i) { + int idx = num_particles - 1 - i; + + new_particles[idx].position.x = goalpost_hypothesis.x + noise_x(rand_gen); + new_particles[idx].position.y = goalpost_hypothesis.y + noise_y(rand_gen); + new_particles[idx].orientation = (orientation + keisan::make_degree(noise_theta(rand_gen))).normalize(); + + new_particles[idx].weight = 1e-3; + } + } + } + // Maintain particle diversity and prevent collapse std::normal_distribution jitter_x(0.0, 2.0); std::normal_distribution jitter_y(0.0, 2.0); @@ -339,8 +361,8 @@ void MCL::resample_particles_low_variance() p.position.y += jitter_y(rand_gen); // Clamp to map boundaries - p.position.x = keisan::clamp(p.position.x, 0.0, 900.0); - p.position.y = keisan::clamp(p.position.y, 0.0, 600.0); + p.position.x = keisan::clamp(p.position.x, -10.0, 910.0); + p.position.y = keisan::clamp(p.position.y, -10.0, 610.0); } particles = new_particles; @@ -410,8 +432,8 @@ void MCL::estimate_position() estimated_position = best_cluster->mean; - estimated_position.x = keisan::clamp(estimated_position.x, 0.0, 900.0); - estimated_position.y = keisan::clamp(estimated_position.y, 0.0, 600.0); + estimated_position.x = keisan::clamp(estimated_position.x, -10.0, 910.0); + estimated_position.y = keisan::clamp(estimated_position.y, -10.0, 610.0); centered_ratio = (double)best_cluster_particles.size() / num_particles; } @@ -459,6 +481,15 @@ double MCL::calculate_distance(const keisan::Point2 & point_1, const keisan::Poi return sqrt(dx * dx + dy * dy); } +keisan::Point2 MCL::rotate_to_global_frame( + const keisan::Point2& local_pos, const keisan::Angle & theta) +{ + // Robot frame: X forward+, Y left+, CW positive + double x_g = local_pos.x * theta.cos() + local_pos.y * theta.sin(); + double y_g = local_pos.x * theta.sin() - local_pos.y * theta.cos(); + return {x_g, y_g}; +} + double MCL::calculate_total_likelihood(const Particle & particle) { double log_total = 0.0; @@ -517,11 +548,10 @@ double MCL::calculate_object_likelihood( double dx = measurement.position.x * 100; double dy = measurement.position.y * 100; - double x_rot = dx * particle.orientation.cos() + dy * particle.orientation.sin(); - double y_rot = dx * particle.orientation.sin() - dy * particle.orientation.cos(); + keisan::Point2 global_pos = rotate_to_global_frame({dx, dy}, particle.orientation); - double px = particle.position.x + x_rot; - double py = particle.position.y + y_rot; + double px = particle.position.x + global_pos.x; + double py = particle.position.y + global_pos.y; double ex = landmark.x - px; double ey = landmark.y - py; @@ -546,11 +576,10 @@ keisan::Matrix<27, 27> MCL::calculate_cost_matrix( double dx = detected_landmarks[i].position.x * 100; double dy = detected_landmarks[i].position.y * 100; - double x_rot = dx * particle.orientation.cos() + dy * particle.orientation.sin(); - double y_rot = dx * particle.orientation.sin() - dy * particle.orientation.cos(); + keisan::Point2 global_pos = rotate_to_global_frame({dx, dy}, particle.orientation); - double px = particle.position.x + x_rot; - double py = particle.position.y + y_rot; + double px = particle.position.x + global_pos.x; + double py = particle.position.y + global_pos.y; double dist = std::sqrt(dx*dx + dy*dy); double sigma = base_sigma + range_sigma * dist; @@ -582,8 +611,6 @@ keisan::Matrix<27, 27> MCL::calculate_cost_matrix( bool MCL::is_landmark_in_fov(const keisan::Point2 & landmark, const Particle & particle) { - static constexpr double FOV_HALF_DEG = 45.0; - double dx_world = landmark.x - particle.position.x; double dy_world = landmark.y - particle.position.y; @@ -597,7 +624,75 @@ bool MCL::is_landmark_in_fov(const keisan::Point2 & landmark, const Particle & p auto diff = (angle_to_landmark - gaze).normalize(); - return std::abs(diff.degree()) <= FOV_HALF_DEG; + return std::abs(diff.degree()) <= 45.0; +} + +bool MCL::compute_goalpost_pose_hypothesis(keisan::Point2 & out_pos) +{ + keisan::Point2 p1_local = {projected_goalpost[0].position.x * 100.0, + projected_goalpost[0].position.y * 100.0}; + keisan::Point2 p2_local = {projected_goalpost[1].position.x * 100.0, + projected_goalpost[1].position.y * 100.0}; + + double r1 = calculate_distance({0.0, 0.0}, p1_local); + double r2 = calculate_distance({0.0, 0.0}, p2_local); + + double dist_posts = calculate_distance(p1_local, p2_local); + if (dist_posts < 120.0 || dist_posts > 280.0) { + return false; + } + + keisan::Point2 p1_global_rel = rotate_to_global_frame(p1_local, orientation); + keisan::Point2 p2_global_rel = rotate_to_global_frame(p2_local, orientation); + + // Determine goal side (home/away) + double mid_global_rel_x = (p1_global_rel.x + p2_global_rel.x) * 0.5; + bool is_home_goal = mid_global_rel_x < 0.0; + + double goal_x = is_home_goal ? 0.0 : 900.0; + + // Assign lower/upper posts based on Y + // r_lower is distance to the post closer to the bottom of the field (y=210), + // r_upper is closer to top (y=390) + keisan::Point2 p_lower_local, p_upper_local; + double r_lower, r_upper; + + if (p1_global_rel.y < p2_global_rel.y) { + p_lower_local = p1_local; r_lower = r1; + p_upper_local = p2_local; r_upper = r2; + } else { + p_lower_local = p2_local; r_lower = r2; + p_upper_local = p1_local; r_upper = r1; + } + + // Trilateration calculation + // Known landmarks: + // A = (goal_x, 210) distance: r_lower + // B = (goal_x, 390) distance: r_upper + // (1) r_lower^2 = (x - goal_x)^2 + (y - 210)^2 + // (2) r_upper^2 = (x - goal_x)^2 + (y - 390)^2 + // => (1)-(2): r_lower^2 - r_upper^2 = (y-210)^2 - (y-390)^2 = 360y - 108000 + // => y = (r_lower^2 - r_upper^2 + 108000) / 360 + + double y_robot = (r_lower * r_lower - r_upper * r_upper + 108000.0) / 360.0; + + // From (1): (x - goal_x)^2 = r_lower^2 - (y - 210)^2 + double dy = y_robot - 210.0; + double x_sq = r_lower * r_lower - dy * dy; + + if (x_sq < 0.0) { + return false; + } + + double x_offset = sqrt(x_sq); + + // Robot assumed inside field + double x_robot = is_home_goal ? x_offset : (900.0 - x_offset); + + out_pos.x = x_robot; + out_pos.y = y_robot; + + return true; } void MCL::print_particles() { From 4b1af51c7a8a4d0be57750f94641664df86babe0 Mon Sep 17 00:00:00 2001 From: mbsaloka Date: Tue, 14 Apr 2026 20:38:39 +0700 Subject: [PATCH 33/33] chore: remove unused comment --- src/basho/localization/mcl.cpp | 19 +------------------ 1 file changed, 1 insertion(+), 18 deletions(-) diff --git a/src/basho/localization/mcl.cpp b/src/basho/localization/mcl.cpp index b3e393f..4461baa 100644 --- a/src/basho/localization/mcl.cpp +++ b/src/basho/localization/mcl.cpp @@ -300,29 +300,12 @@ void MCL::resample_particles_low_variance() new_particles[idx].weight = 1e-3; } - // If localization is lost, inject more random particles to increase chances of recovery - // int num_inject_global = 0; int num_inject_odom = 0; int num_inject_goalpost = 0; + // If localization is lost, inject random particles near odometry position to increase chances of recovery if (state == LOST && lost_count > 10) { - // num_inject_global = 0.1 * num_particles; num_inject_odom = 0.1 * num_particles; - - // Global random injection - // std::uniform_int_distribution xrg(-10, 910); - // std::uniform_int_distribution yrg(-10, 610); - // for (int i = num_random; i < num_random + lost_inject_global; ++i) { - // int idx = num_particles - 1 - i; - - // new_particles[idx].position.x = xrg(rand_gen); - // new_particles[idx].position.y = yrg(rand_gen); - // new_particles[idx].orientation = (orientation + keisan::make_degree(noise_theta(rand_gen))).normalize(); - - // new_particles[idx].weight = 1e-3; - // } - - // Inject near odometry position for (int i = num_random; i < num_random + num_inject_odom; ++i) { int idx = num_particles - 1 - i;