From 00555b0f617749378d276c6cf060badd31474404 Mon Sep 17 00:00:00 2001 From: ShotaAk Date: Sun, 11 Aug 2024 02:10:20 +0900 Subject: [PATCH 01/17] Add ball kalman filter --- consai_vision_tracker/CMakeLists.txt | 19 ++- .../ball_kalman_filter.hpp | 61 ++++++++++ .../tracker_component.hpp | 2 + consai_vision_tracker/package.xml | 1 + .../src/ball_kalman_filter.cpp | 112 ++++++++++++++++++ .../src/tracker_component.cpp | 15 ++- 6 files changed, 199 insertions(+), 11 deletions(-) create mode 100644 consai_vision_tracker/include/consai_vision_tracker/ball_kalman_filter.hpp create mode 100644 consai_vision_tracker/src/ball_kalman_filter.cpp diff --git a/consai_vision_tracker/CMakeLists.txt b/consai_vision_tracker/CMakeLists.txt index 72e12211..6b912f9f 100644 --- a/consai_vision_tracker/CMakeLists.txt +++ b/consai_vision_tracker/CMakeLists.txt @@ -20,20 +20,17 @@ find_package(ament_cmake REQUIRED) find_package(consai_msgs REQUIRED) find_package(consai_tools REQUIRED) find_package(consai_visualizer_msgs REQUIRED) +find_package(Eigen3 REQUIRED) find_package(rclcpp REQUIRED) find_package(rclcpp_components REQUIRED) find_package(robocup_ssl_msgs REQUIRED) pkg_check_modules(BFL REQUIRED orocos-bfl) -include_directories( - include - ${BFL_INCLUDE_DIRS} -) - # Tracker Component add_library(tracker_component SHARED src/tracker_component.cpp src/ball_tracker.cpp + src/ball_kalman_filter.cpp src/robot_tracker.cpp src/visualization_data_handler.cpp ) @@ -43,10 +40,17 @@ ament_target_dependencies(tracker_component consai_msgs consai_tools consai_visualizer_msgs + Eigen3 rclcpp rclcpp_components robocup_ssl_msgs ) +target_include_directories(tracker_component PUBLIC + "$" + "$" + "$" + "$" +) target_link_libraries(tracker_component ${BFL_LIBRARIES} ) @@ -61,7 +65,10 @@ ament_export_dependencies(rclcpp) ament_export_dependencies(rclcpp_components) ament_export_dependencies(robocup_ssl_msgs) -ament_export_include_directories(include) +ament_export_include_directories( + include + ${EIGEN3_INCLUDE_DIR} +) ament_export_libraries(tracker_component) # Install diff --git a/consai_vision_tracker/include/consai_vision_tracker/ball_kalman_filter.hpp b/consai_vision_tracker/include/consai_vision_tracker/ball_kalman_filter.hpp new file mode 100644 index 00000000..7b353cfa --- /dev/null +++ b/consai_vision_tracker/include/consai_vision_tracker/ball_kalman_filter.hpp @@ -0,0 +1,61 @@ +// Copyright 2024 Roots +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef CONSAI_VISION_TRACKER__BALL_KALMAN_FILTER_HPP_ +#define CONSAI_VISION_TRACKER__BALL_KALMAN_FILTER_HPP_ + +#include +#include + +#include "robocup_ssl_msgs/msg/detection_ball.hpp" +#include "robocup_ssl_msgs/msg/tracked_ball.hpp" + + +namespace consai_vision_tracker +{ + +using DetectionBall = robocup_ssl_msgs::msg::DetectionBall; +using TrackedBall = robocup_ssl_msgs::msg::TrackedBall; +using Vector4d = Eigen::Vector4d; +using Vector2d = Eigen::Vector2d; +using Matrix4d = Eigen::Matrix4d; +using Matrix2d = Eigen::Matrix2d; +using Matrix24d = Eigen::Matrix; +using Matrix42d = Eigen::Matrix; + +class BallKalmanFilter +{ + public: + explicit BallKalmanFilter(const double dt = 0.01); + + void push_back_observation(const DetectionBall & ball); + TrackedBall update(const bool use_uncertain_sys_model); + TrackedBall get_estimation(void) const; + + private: + std::vector ball_observations_; + TrackedBall prev_tracked_ball_; + + Vector4d x_; + + Matrix4d F_; + Matrix24d H_; + Matrix4d Q_; + Matrix2d R_; + Matrix4d P_; +}; + +} // namespace consai_vision_tracker + +#endif // CONSAI_VISION_TRACKER__BALL_KALMAN_FILTER_HPP_ diff --git a/consai_vision_tracker/include/consai_vision_tracker/tracker_component.hpp b/consai_vision_tracker/include/consai_vision_tracker/tracker_component.hpp index 1d7ea385..c3b1409a 100644 --- a/consai_vision_tracker/include/consai_vision_tracker/tracker_component.hpp +++ b/consai_vision_tracker/include/consai_vision_tracker/tracker_component.hpp @@ -22,6 +22,7 @@ #include "consai_msgs/msg/robot_local_velocity.hpp" #include "consai_vision_tracker/visibility_control.h" #include "consai_vision_tracker/ball_tracker.hpp" +#include "consai_vision_tracker/ball_kalman_filter.hpp" #include "consai_vision_tracker/robot_tracker.hpp" #include "consai_vision_tracker/visualization_data_handler.hpp" #include "robocup_ssl_msgs/msg/detection_ball.hpp" @@ -65,6 +66,7 @@ class Tracker : public rclcpp::Node rclcpp::Publisher::SharedPtr pub_tracked_; rclcpp::Publisher::SharedPtr pub_robot_velocities_; std::shared_ptr ball_tracker_; + std::unique_ptr ball_kalman_filter_; std::vector> blue_robot_tracker_; std::vector> yellow_robot_tracker_; std::shared_ptr vis_data_handler_; diff --git a/consai_vision_tracker/package.xml b/consai_vision_tracker/package.xml index be137746..935d199c 100644 --- a/consai_vision_tracker/package.xml +++ b/consai_vision_tracker/package.xml @@ -13,6 +13,7 @@ consai_msgs consai_tools consai_visualizer_msgs + eigen rclcpp rclcpp_components robocup_ssl_msgs diff --git a/consai_vision_tracker/src/ball_kalman_filter.cpp b/consai_vision_tracker/src/ball_kalman_filter.cpp new file mode 100644 index 00000000..64510a29 --- /dev/null +++ b/consai_vision_tracker/src/ball_kalman_filter.cpp @@ -0,0 +1,112 @@ +// Copyright 2024 Roots +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "consai_vision_tracker/ball_kalman_filter.hpp" +#include "robocup_ssl_msgs/msg/vector3.hpp" + +namespace consai_vision_tracker +{ + +using SSLVector3 = robocup_ssl_msgs::msg::Vector3; + +BallKalmanFilter::BallKalmanFilter(const double dt) +{ + // State vector + // x_ = (x, y, vx, vy) + x_ = Vector4d::Zero(); + + // State transition matrix + F_ = Matrix4d::Identity(); + F_(0, 2) = dt; + F_(1, 3) = dt; + + // Observation matrix + H_ = Matrix24d::Zero(); + H_(0, 0) = 1.0; + H_(1, 1) = 1.0; + + // Process noise covariance matrix + // Q_ = diag(qx, qy, qvx, qvy) + Q_ = Matrix4d::Identity() * 0.01; + + // Observation noise covariance matrix + // R_ = diag(rx, ry) + R_ = Matrix2d::Identity() * 0.1; + + // Error covariance matrix + P_ = Matrix4d::Identity(); +} + +void BallKalmanFilter::push_back_observation(const DetectionBall & ball) +{ + TrackedBall observation; + observation.pos.x = ball.x * 0.001; // mm to meters + observation.pos.y = ball.y * 0.001; // mm to meters + ball_observations_.push_back(observation); +} + +TrackedBall BallKalmanFilter::update(const bool use_uncertain_sys_model) +{ + Vector2d z; + + // TODO(): Fix me + if (ball_observations_.empty()) { + z << x_[0], x_[1]; + } else { + z[0] = ball_observations_.back().pos.x; + z[1] = ball_observations_.back().pos.y; + ball_observations_.clear(); + } + + // Prediction step + Vector4d x_pred = F_ * x_; + Matrix4d P_pred = F_ * P_ * F_.transpose() + Q_; + + // Calculate innovation + Vector2d innovation = z - H_ * x_pred; + + // Calculate innovation covariance matrix + Matrix2d S = H_ * P_pred * H_.transpose() + R_; + + // Calculate chi-square value + double chi_square = innovation.transpose() * S.inverse() * innovation; + + // Check outlier + // TODO(): Implement outlier detection + + // Update step + Matrix42d K = P_pred * H_.transpose() * S.inverse(); + x_ = x_pred + K * innovation; + P_ = (Matrix4d::Identity() - K * H_) * P_pred; + + return get_estimation(); +} + +TrackedBall BallKalmanFilter::get_estimation(void) const +{ + TrackedBall estimation; + estimation.pos.x = x_[0]; + estimation.pos.y = x_[1]; + + SSLVector3 vel; + vel.x = x_[2]; + vel.y = x_[3]; + estimation.vel.push_back(vel); + estimation.visibility.push_back(1.0); + + return estimation; +} + + +} // namespace consai_vision_tracker diff --git a/consai_vision_tracker/src/tracker_component.cpp b/consai_vision_tracker/src/tracker_component.cpp index f0afdd81..e21bc0cd 100644 --- a/consai_vision_tracker/src/tracker_component.cpp +++ b/consai_vision_tracker/src/tracker_component.cpp @@ -41,7 +41,8 @@ Tracker::Tracker(const rclcpp::NodeOptions & options) { const auto UPDATE_RATE = 0.01s; - ball_tracker_ = std::make_shared(UPDATE_RATE.count()); + // ball_tracker_ = std::make_shared(UPDATE_RATE.count()); + ball_kalman_filter_ = std::make_unique(UPDATE_RATE.count()); for (int i = 0; i < 16; i++) { blue_robot_tracker_.push_back( std::make_shared( @@ -107,7 +108,8 @@ void Tracker::on_timer() return false; }; - const auto ball = ball_tracker_->prev_estimation(); + // const auto ball = ball_tracker_->prev_estimation(); + const auto ball = ball_kalman_filter_->get_estimation(); bool use_uncertain_sys_model = false; if (tools::is_visible(ball)) { @@ -117,7 +119,8 @@ void Tracker::on_timer() ball_is_near_a_robot(yellow_robot_tracker_, ball_pose); } - tracked_msg->balls.push_back(ball_tracker_->update(use_uncertain_sys_model)); + // tracked_msg->balls.push_back(ball_tracker_->update(use_uncertain_sys_model)); + tracked_msg->balls.push_back(ball_kalman_filter_->update(use_uncertain_sys_model)); tracked_msg = vis_data_handler_->publish_vis_tracked(std::move(tracked_msg)); @@ -128,7 +131,8 @@ void Tracker::on_timer() void Tracker::callback_detection(const DetectionFrame::SharedPtr msg) { for (const auto & ball : msg->balls) { - ball_tracker_->push_back_observation(ball); + // ball_tracker_->push_back_observation(ball); + ball_kalman_filter_->push_back_observation(ball); } for (const auto & blue_robot : msg->robots_blue) { @@ -152,7 +156,8 @@ void Tracker::callback_detection_invert(const DetectionFrame::SharedPtr msg) for (auto && ball : msg->balls) { invert_ball(ball); - ball_tracker_->push_back_observation(ball); + // ball_tracker_->push_back_observation(ball); + ball_kalman_filter_->push_back_observation(ball); } for (auto && blue_robot : msg->robots_blue) { From 6d66bd156fc96a2110bcb9504886b94fb29a206b Mon Sep 17 00:00:00 2001 From: ShotaAk Date: Sun, 11 Aug 2024 03:01:39 +0900 Subject: [PATCH 02/17] Update. need to fix is_outlier check --- .../ball_kalman_filter.hpp | 20 ++++-- .../src/ball_kalman_filter.cpp | 62 ++++++++++++++++--- 2 files changed, 70 insertions(+), 12 deletions(-) diff --git a/consai_vision_tracker/include/consai_vision_tracker/ball_kalman_filter.hpp b/consai_vision_tracker/include/consai_vision_tracker/ball_kalman_filter.hpp index 7b353cfa..b8c60f2a 100644 --- a/consai_vision_tracker/include/consai_vision_tracker/ball_kalman_filter.hpp +++ b/consai_vision_tracker/include/consai_vision_tracker/ball_kalman_filter.hpp @@ -37,18 +37,30 @@ using Matrix42d = Eigen::Matrix; class BallKalmanFilter { public: - explicit BallKalmanFilter(const double dt = 0.01); + explicit BallKalmanFilter( + const double dt = 0.01, const double lifetime = 2.0, + const double visibility_increase_rate = 5.0, + const double outlier_time_limit = 0.1); void push_back_observation(const DetectionBall & ball); TrackedBall update(const bool use_uncertain_sys_model); TrackedBall get_estimation(void) const; private: - std::vector ball_observations_; - TrackedBall prev_tracked_ball_; + Vector2d make_observation(void) const; + bool is_outlier(const double chi_squared_value) const; + void reset_estimation(const Vector2d & observation); - Vector4d x_; + const double VISIBILITY_CONTROL_VALUE_; + const double VISIBILITY_INCREASE_RATE_; + const int OUTLIER_COUNT_THRESHOLD_; + + double visibility_ = 1.0; + int outlier_count_ = 0; + std::vector ball_observations_; + + Vector4d x_; Matrix4d F_; Matrix24d H_; Matrix4d Q_; diff --git a/consai_vision_tracker/src/ball_kalman_filter.cpp b/consai_vision_tracker/src/ball_kalman_filter.cpp index 64510a29..e2cde988 100644 --- a/consai_vision_tracker/src/ball_kalman_filter.cpp +++ b/consai_vision_tracker/src/ball_kalman_filter.cpp @@ -20,7 +20,13 @@ namespace consai_vision_tracker using SSLVector3 = robocup_ssl_msgs::msg::Vector3; -BallKalmanFilter::BallKalmanFilter(const double dt) +BallKalmanFilter::BallKalmanFilter( + const double dt, const double lifetime, + const double visibility_increase_rate, + const double outlier_time_limit) : + VISIBILITY_CONTROL_VALUE_(dt / lifetime), + VISIBILITY_INCREASE_RATE_(visibility_increase_rate), + OUTLIER_COUNT_THRESHOLD_(outlier_time_limit / dt) { // State vector // x_ = (x, y, vx, vy) @@ -58,14 +64,13 @@ void BallKalmanFilter::push_back_observation(const DetectionBall & ball) TrackedBall BallKalmanFilter::update(const bool use_uncertain_sys_model) { - Vector2d z; + Vector2d z(x_[0], x_[1]); - // TODO(): Fix me if (ball_observations_.empty()) { - z << x_[0], x_[1]; + visibility_ = std::max(0.0, visibility_ - VISIBILITY_CONTROL_VALUE_); } else { - z[0] = ball_observations_.back().pos.x; - z[1] = ball_observations_.back().pos.y; + z = make_observation(); + visibility_ = std::min(1.0, visibility_ + VISIBILITY_INCREASE_RATE_ * VISIBILITY_CONTROL_VALUE_); ball_observations_.clear(); } @@ -83,7 +88,15 @@ TrackedBall BallKalmanFilter::update(const bool use_uncertain_sys_model) double chi_square = innovation.transpose() * S.inverse() * innovation; // Check outlier - // TODO(): Implement outlier detection + if (is_outlier(chi_square)) { + outlier_count_++; + if (outlier_count_ > OUTLIER_COUNT_THRESHOLD_) { + reset_estimation(z); + } + return get_estimation(); + } else { + outlier_count_ = 0; + } // Update step Matrix42d K = P_pred * H_.transpose() * S.inverse(); @@ -103,10 +116,43 @@ TrackedBall BallKalmanFilter::get_estimation(void) const vel.x = x_[2]; vel.y = x_[3]; estimation.vel.push_back(vel); - estimation.visibility.push_back(1.0); + estimation.visibility.push_back(visibility_); return estimation; } +Vector2d BallKalmanFilter::make_observation(void) const +{ + if (ball_observations_.empty()) { + return Vector2d(x_[0], x_[1]); + } + + Vector2d mean_observation(0.0, 0.0); + for (const auto & observation : ball_observations_) { + mean_observation[0] += observation.pos.x; + mean_observation[1] += observation.pos.y; + } + mean_observation /= ball_observations_.size(); + return mean_observation; +} + +bool BallKalmanFilter::is_outlier(const double chi_squared_value) const +{ + constexpr double THRESHOLD = 5.991; // 2 degrees of freedom, 0.05 significance level + + if (chi_squared_value > THRESHOLD) { + return true; + } + return false; +} + +void BallKalmanFilter::reset_estimation(const Vector2d & observation) +{ + x_ << observation[0], observation[1], 0.0, 0.0; + P_ = Matrix4d::Identity(); + visibility_ = 1.0; + outlier_count_ = 0; +} + } // namespace consai_vision_tracker From 9ea01580ebb0f98a5b6885179a59b08b23cfe393 Mon Sep 17 00:00:00 2001 From: ShotaAk Date: Sun, 11 Aug 2024 13:52:29 +0900 Subject: [PATCH 03/17] Implement ball kalman filter --- .../ball_kalman_filter.hpp | 1 + .../src/ball_kalman_filter.cpp | 80 +++++++++++++------ 2 files changed, 57 insertions(+), 24 deletions(-) diff --git a/consai_vision_tracker/include/consai_vision_tracker/ball_kalman_filter.hpp b/consai_vision_tracker/include/consai_vision_tracker/ball_kalman_filter.hpp index b8c60f2a..6bc9236d 100644 --- a/consai_vision_tracker/include/consai_vision_tracker/ball_kalman_filter.hpp +++ b/consai_vision_tracker/include/consai_vision_tracker/ball_kalman_filter.hpp @@ -64,6 +64,7 @@ class BallKalmanFilter Matrix4d F_; Matrix24d H_; Matrix4d Q_; + Matrix4d Q_uncertain_; Matrix2d R_; Matrix4d P_; }; diff --git a/consai_vision_tracker/src/ball_kalman_filter.cpp b/consai_vision_tracker/src/ball_kalman_filter.cpp index e2cde988..47f9c4c7 100644 --- a/consai_vision_tracker/src/ball_kalman_filter.cpp +++ b/consai_vision_tracker/src/ball_kalman_filter.cpp @@ -15,6 +15,8 @@ #include "consai_vision_tracker/ball_kalman_filter.hpp" #include "robocup_ssl_msgs/msg/vector3.hpp" +#include + namespace consai_vision_tracker { @@ -43,12 +45,30 @@ BallKalmanFilter::BallKalmanFilter( H_(1, 1) = 1.0; // Process noise covariance matrix - // Q_ = diag(qx, qy, qvx, qvy) - Q_ = Matrix4d::Identity() * 0.01; + auto gen_Q = [&](const double max_acc) { + // Process noise depends on the maximum acceleration + const double sigma_vel = max_acc * dt; + const double sigma_pos = 0.5 * max_acc * dt * dt; + const double var_vel = sigma_vel * sigma_vel; + const double var_pos = sigma_pos * sigma_pos; + const double var_vel_pos = sigma_vel * sigma_pos; + + Matrix4d Q; + Q << var_pos, 0.0, var_vel_pos, 0.0, + 0.0, var_pos, 0.0, var_vel_pos, + var_vel_pos, 0.0, var_vel, 0.0, + 0.0, var_vel_pos, 0.0, var_vel; + return Q; + }; + Q_ = gen_Q(0.1 / dt); // 0.1 m/s^2 + Q_uncertain_ = gen_Q(0.5 / dt); // 0.5 m/s^2 // Observation noise covariance matrix - // R_ = diag(rx, ry) - R_ = Matrix2d::Identity() * 0.1; + auto gen_R = [&](const double std_dev) { + const double var = std_dev * std_dev; + return Eigen::DiagonalMatrix(var, var); + }; + R_ = gen_R(0.03); // 0.03 meters // Error covariance matrix P_ = Matrix4d::Identity(); @@ -64,40 +84,53 @@ void BallKalmanFilter::push_back_observation(const DetectionBall & ball) TrackedBall BallKalmanFilter::update(const bool use_uncertain_sys_model) { - Vector2d z(x_[0], x_[1]); - - if (ball_observations_.empty()) { - visibility_ = std::max(0.0, visibility_ - VISIBILITY_CONTROL_VALUE_); - } else { - z = make_observation(); - visibility_ = std::min(1.0, visibility_ + VISIBILITY_INCREASE_RATE_ * VISIBILITY_CONTROL_VALUE_); - ball_observations_.clear(); + // Select process noise covariance matrix + Matrix4d Q = Q_; + if (use_uncertain_sys_model) { + Q = Q_uncertain_; } // Prediction step Vector4d x_pred = F_ * x_; - Matrix4d P_pred = F_ * P_ * F_.transpose() + Q_; - - // Calculate innovation - Vector2d innovation = z - H_ * x_pred; + Matrix4d P_pred = F_ * P_ * F_.transpose() + Q; // Calculate innovation covariance matrix Matrix2d S = H_ * P_pred * H_.transpose() + R_; - // Calculate chi-square value - double chi_square = innovation.transpose() * S.inverse() * innovation; + // Remove outliers + for (auto it = ball_observations_.begin(); it != ball_observations_.end(); ) { + Vector2d z(it->pos.x, it->pos.y); + Vector2d innovation = z - H_ * x_pred; + double chi_square = innovation.transpose() * S.inverse() * innovation; + + if (!is_outlier(chi_square)) { + outlier_count_ = 0; + it++; + continue; + } - // Check outlier - if (is_outlier(chi_square)) { outlier_count_++; - if (outlier_count_ > OUTLIER_COUNT_THRESHOLD_) { + if (outlier_count_ < OUTLIER_COUNT_THRESHOLD_) { + it = ball_observations_.erase(it); + } else { reset_estimation(z); + it++; } - return get_estimation(); + } + + // Make observation + Vector2d z(x_[0], x_[1]); + if (ball_observations_.empty()) { + visibility_ = std::max(0.0, visibility_ - VISIBILITY_CONTROL_VALUE_); } else { - outlier_count_ = 0; + z = make_observation(); + visibility_ = std::min(1.0, visibility_ + VISIBILITY_INCREASE_RATE_ * VISIBILITY_CONTROL_VALUE_); + ball_observations_.clear(); } + // Calculate innovation + Vector2d innovation = z - H_ * x_pred; + // Update step Matrix42d K = P_pred * H_.transpose() * S.inverse(); x_ = x_pred + K * innovation; @@ -151,7 +184,6 @@ void BallKalmanFilter::reset_estimation(const Vector2d & observation) x_ << observation[0], observation[1], 0.0, 0.0; P_ = Matrix4d::Identity(); visibility_ = 1.0; - outlier_count_ = 0; } From 54fc1fec8859c3af11b5dd933a0230e33d6da874 Mon Sep 17 00:00:00 2001 From: ShotaAk Date: Sun, 11 Aug 2024 17:33:10 +0900 Subject: [PATCH 04/17] Use ball kalman filter instead of ball_tracker --- .../ball_kalman_filter.hpp | 5 +- .../tracker_component.hpp | 2 - .../src/ball_kalman_filter.cpp | 101 +++++++++++------- .../src/tracker_component.cpp | 15 ++- 4 files changed, 74 insertions(+), 49 deletions(-) diff --git a/consai_vision_tracker/include/consai_vision_tracker/ball_kalman_filter.hpp b/consai_vision_tracker/include/consai_vision_tracker/ball_kalman_filter.hpp index 6bc9236d..8b3c27e7 100644 --- a/consai_vision_tracker/include/consai_vision_tracker/ball_kalman_filter.hpp +++ b/consai_vision_tracker/include/consai_vision_tracker/ball_kalman_filter.hpp @@ -45,12 +45,15 @@ class BallKalmanFilter void push_back_observation(const DetectionBall & ball); TrackedBall update(const bool use_uncertain_sys_model); TrackedBall get_estimation(void) const; + void update_noise_covariance_matrix( + const double q_max_acc, const double q_uncertain_max_acc, const double r_pos_stddev); private: Vector2d make_observation(void) const; bool is_outlier(const double chi_squared_value) const; - void reset_estimation(const Vector2d & observation); + void reset_x_and_p(const Vector2d & observation); + const double DT_; const double VISIBILITY_CONTROL_VALUE_; const double VISIBILITY_INCREASE_RATE_; const int OUTLIER_COUNT_THRESHOLD_; diff --git a/consai_vision_tracker/include/consai_vision_tracker/tracker_component.hpp b/consai_vision_tracker/include/consai_vision_tracker/tracker_component.hpp index c3b1409a..37b5e451 100644 --- a/consai_vision_tracker/include/consai_vision_tracker/tracker_component.hpp +++ b/consai_vision_tracker/include/consai_vision_tracker/tracker_component.hpp @@ -21,7 +21,6 @@ #include "consai_msgs/msg/robot_local_velocities.hpp" #include "consai_msgs/msg/robot_local_velocity.hpp" #include "consai_vision_tracker/visibility_control.h" -#include "consai_vision_tracker/ball_tracker.hpp" #include "consai_vision_tracker/ball_kalman_filter.hpp" #include "consai_vision_tracker/robot_tracker.hpp" #include "consai_vision_tracker/visualization_data_handler.hpp" @@ -65,7 +64,6 @@ class Tracker : public rclcpp::Node rclcpp::Subscription::SharedPtr sub_geometry_; rclcpp::Publisher::SharedPtr pub_tracked_; rclcpp::Publisher::SharedPtr pub_robot_velocities_; - std::shared_ptr ball_tracker_; std::unique_ptr ball_kalman_filter_; std::vector> blue_robot_tracker_; std::vector> yellow_robot_tracker_; diff --git a/consai_vision_tracker/src/ball_kalman_filter.cpp b/consai_vision_tracker/src/ball_kalman_filter.cpp index 47f9c4c7..7ef5b6d5 100644 --- a/consai_vision_tracker/src/ball_kalman_filter.cpp +++ b/consai_vision_tracker/src/ball_kalman_filter.cpp @@ -26,18 +26,16 @@ BallKalmanFilter::BallKalmanFilter( const double dt, const double lifetime, const double visibility_increase_rate, const double outlier_time_limit) : + DT_(dt), VISIBILITY_CONTROL_VALUE_(dt / lifetime), VISIBILITY_INCREASE_RATE_(visibility_increase_rate), OUTLIER_COUNT_THRESHOLD_(outlier_time_limit / dt) { - // State vector - // x_ = (x, y, vx, vy) - x_ = Vector4d::Zero(); // State transition matrix F_ = Matrix4d::Identity(); - F_(0, 2) = dt; - F_(1, 3) = dt; + F_(0, 2) = DT_; + F_(1, 3) = DT_; // Observation matrix H_ = Matrix24d::Zero(); @@ -45,33 +43,13 @@ BallKalmanFilter::BallKalmanFilter( H_(1, 1) = 1.0; // Process noise covariance matrix - auto gen_Q = [&](const double max_acc) { - // Process noise depends on the maximum acceleration - const double sigma_vel = max_acc * dt; - const double sigma_pos = 0.5 * max_acc * dt * dt; - const double var_vel = sigma_vel * sigma_vel; - const double var_pos = sigma_pos * sigma_pos; - const double var_vel_pos = sigma_vel * sigma_pos; - - Matrix4d Q; - Q << var_pos, 0.0, var_vel_pos, 0.0, - 0.0, var_pos, 0.0, var_vel_pos, - var_vel_pos, 0.0, var_vel, 0.0, - 0.0, var_vel_pos, 0.0, var_vel; - return Q; - }; - Q_ = gen_Q(0.1 / dt); // 0.1 m/s^2 - Q_uncertain_ = gen_Q(0.5 / dt); // 0.5 m/s^2 - // Observation noise covariance matrix - auto gen_R = [&](const double std_dev) { - const double var = std_dev * std_dev; - return Eigen::DiagonalMatrix(var, var); - }; - R_ = gen_R(0.03); // 0.03 meters + update_noise_covariance_matrix(0.1, 0.5, 0.03); + // State vector + // x_ = (x, y, vx, vy) // Error covariance matrix - P_ = Matrix4d::Identity(); + reset_x_and_p(Vector2d::Zero()); } void BallKalmanFilter::push_back_observation(const DetectionBall & ball) @@ -90,18 +68,27 @@ TrackedBall BallKalmanFilter::update(const bool use_uncertain_sys_model) Q = Q_uncertain_; } - // Prediction step - Vector4d x_pred = F_ * x_; - Matrix4d P_pred = F_ * P_ * F_.transpose() + Q; + Vector4d x_pred; + Matrix4d P_pred; - // Calculate innovation covariance matrix - Matrix2d S = H_ * P_pred * H_.transpose() + R_; + auto prediction = [&](void) { + x_pred = F_ * x_; + P_pred = F_ * P_ * F_.transpose() + Q; + }; + + auto calc_chi_square = [&](const Vector2d & z) { + Matrix2d S = H_ * P_pred * H_.transpose() + R_; + Vector2d innovation = z - H_ * x_pred; + return innovation.transpose() * S.inverse() * innovation; + }; + + // Prediction step + prediction(); // Remove outliers for (auto it = ball_observations_.begin(); it != ball_observations_.end(); ) { - Vector2d z(it->pos.x, it->pos.y); - Vector2d innovation = z - H_ * x_pred; - double chi_square = innovation.transpose() * S.inverse() * innovation; + const Vector2d z(it->pos.x, it->pos.y); + const double chi_square = calc_chi_square(z); if (!is_outlier(chi_square)) { outlier_count_ = 0; @@ -113,7 +100,8 @@ TrackedBall BallKalmanFilter::update(const bool use_uncertain_sys_model) if (outlier_count_ < OUTLIER_COUNT_THRESHOLD_) { it = ball_observations_.erase(it); } else { - reset_estimation(z); + reset_x_and_p(z); + prediction(); // Use new state vector it++; } } @@ -130,6 +118,8 @@ TrackedBall BallKalmanFilter::update(const bool use_uncertain_sys_model) // Calculate innovation Vector2d innovation = z - H_ * x_pred; + // Calculate innovation covariance matrix + Matrix2d S = H_ * P_pred * H_.transpose() + R_; // Update step Matrix42d K = P_pred * H_.transpose() * S.inverse(); @@ -154,6 +144,36 @@ TrackedBall BallKalmanFilter::get_estimation(void) const return estimation; } +void BallKalmanFilter::update_noise_covariance_matrix( + const double q_max_acc, const double q_uncertain_max_acc, const double r_pos_stddev) +{ + // Process noise covariance matrix + auto gen_Q = [this](const double max_acc) { + // Process noise depends on the maximum acceleration + const double sigma_vel = max_acc * DT_; + const double sigma_pos = 0.5 * max_acc * DT_ * DT_; + const double var_vel = sigma_vel * sigma_vel; + const double var_pos = sigma_pos * sigma_pos; + const double var_vel_pos = sigma_vel * sigma_pos; + + Matrix4d Q; + Q << var_pos, 0.0, var_vel_pos, 0.0, + 0.0, var_pos, 0.0, var_vel_pos, + var_vel_pos, 0.0, var_vel, 0.0, + 0.0, var_vel_pos, 0.0, var_vel; + return Q; + }; + Q_ = gen_Q(q_max_acc); // m/s^2 + Q_uncertain_ = gen_Q(q_uncertain_max_acc); // m/s^2 + + // Observation noise covariance matrix + auto gen_R = [&](const double std_dev) { + const double var = std_dev * std_dev; + return Eigen::DiagonalMatrix(var, var); + }; + R_ = gen_R(r_pos_stddev); // meters +} + Vector2d BallKalmanFilter::make_observation(void) const { if (ball_observations_.empty()) { @@ -179,11 +199,10 @@ bool BallKalmanFilter::is_outlier(const double chi_squared_value) const return false; } -void BallKalmanFilter::reset_estimation(const Vector2d & observation) +void BallKalmanFilter::reset_x_and_p(const Vector2d & observation) { x_ << observation[0], observation[1], 0.0, 0.0; - P_ = Matrix4d::Identity(); - visibility_ = 1.0; + P_ = Matrix4d::Identity() * 10.0; } diff --git a/consai_vision_tracker/src/tracker_component.cpp b/consai_vision_tracker/src/tracker_component.cpp index e21bc0cd..336f1c8c 100644 --- a/consai_vision_tracker/src/tracker_component.cpp +++ b/consai_vision_tracker/src/tracker_component.cpp @@ -41,8 +41,11 @@ Tracker::Tracker(const rclcpp::NodeOptions & options) { const auto UPDATE_RATE = 0.01s; - // ball_tracker_ = std::make_shared(UPDATE_RATE.count()); ball_kalman_filter_ = std::make_unique(UPDATE_RATE.count()); + declare_parameter("ball_kalman_filter/q_acc", 4.0); + declare_parameter("ball_kalman_filter/q_uncertain_acc", 50.0); + declare_parameter("ball_kalman_filter/r_pos_stddev", 0.05); + for (int i = 0; i < 16; i++) { blue_robot_tracker_.push_back( std::make_shared( @@ -81,6 +84,12 @@ void Tracker::on_timer() auto tracked_msg = std::make_unique(); auto robot_vel_msg = std::make_unique(); + // TODO: use parameter callback + ball_kalman_filter_->update_noise_covariance_matrix( + get_parameter("ball_kalman_filter/q_acc").get_value(), + get_parameter("ball_kalman_filter/q_uncertain_acc").get_value(), + get_parameter("ball_kalman_filter/r_pos_stddev").get_value()); + for (auto && tracker : blue_robot_tracker_) { tracked_msg->robots.push_back(tracker->update()); robot_vel_msg->velocities.push_back(tracker->calc_local_velocity()); @@ -108,7 +117,6 @@ void Tracker::on_timer() return false; }; - // const auto ball = ball_tracker_->prev_estimation(); const auto ball = ball_kalman_filter_->get_estimation(); bool use_uncertain_sys_model = false; @@ -119,7 +127,6 @@ void Tracker::on_timer() ball_is_near_a_robot(yellow_robot_tracker_, ball_pose); } - // tracked_msg->balls.push_back(ball_tracker_->update(use_uncertain_sys_model)); tracked_msg->balls.push_back(ball_kalman_filter_->update(use_uncertain_sys_model)); tracked_msg = vis_data_handler_->publish_vis_tracked(std::move(tracked_msg)); @@ -131,7 +138,6 @@ void Tracker::on_timer() void Tracker::callback_detection(const DetectionFrame::SharedPtr msg) { for (const auto & ball : msg->balls) { - // ball_tracker_->push_back_observation(ball); ball_kalman_filter_->push_back_observation(ball); } @@ -156,7 +162,6 @@ void Tracker::callback_detection_invert(const DetectionFrame::SharedPtr msg) for (auto && ball : msg->balls) { invert_ball(ball); - // ball_tracker_->push_back_observation(ball); ball_kalman_filter_->push_back_observation(ball); } From 7304f163a39d57e64efd5f411c0c278ea232ecec Mon Sep 17 00:00:00 2001 From: ShotaAk Date: Mon, 12 Aug 2024 14:48:25 +0900 Subject: [PATCH 05/17] Fix build error on Jazzy --- .../include/consai_robot_controller/tactic/ball_boy_tactics.hpp | 1 + 1 file changed, 1 insertion(+) diff --git a/consai_robot_controller/include/consai_robot_controller/tactic/ball_boy_tactics.hpp b/consai_robot_controller/include/consai_robot_controller/tactic/ball_boy_tactics.hpp index e9ba424f..416ac4b6 100644 --- a/consai_robot_controller/include/consai_robot_controller/tactic/ball_boy_tactics.hpp +++ b/consai_robot_controller/include/consai_robot_controller/tactic/ball_boy_tactics.hpp @@ -16,6 +16,7 @@ #define CONSAI_ROBOT_CONTROLLER__TACTIC__BALL_BOY_TACTICS_HPP_ #include +#include #include #include "consai_msgs/msg/state2_d.hpp" From 672806c0a8b81c85bf80778405ab9bc0833533e1 Mon Sep 17 00:00:00 2001 From: ShotaAk Date: Tue, 13 Aug 2024 10:19:55 +0900 Subject: [PATCH 06/17] Add robot_kalman_filter --- consai_vision_tracker/CMakeLists.txt | 10 +- .../robot_kalman_filter.hpp | 87 +++++ .../tracker_component.hpp | 6 +- consai_vision_tracker/package.xml | 2 +- .../src/robot_kalman_filter.cpp | 344 ++++++++++++++++++ .../src/tracker_component.cpp | 54 ++- 6 files changed, 475 insertions(+), 28 deletions(-) create mode 100644 consai_vision_tracker/include/consai_vision_tracker/robot_kalman_filter.hpp create mode 100644 consai_vision_tracker/src/robot_kalman_filter.cpp diff --git a/consai_vision_tracker/CMakeLists.txt b/consai_vision_tracker/CMakeLists.txt index 6b912f9f..c9e36bc1 100644 --- a/consai_vision_tracker/CMakeLists.txt +++ b/consai_vision_tracker/CMakeLists.txt @@ -24,14 +24,14 @@ find_package(Eigen3 REQUIRED) find_package(rclcpp REQUIRED) find_package(rclcpp_components REQUIRED) find_package(robocup_ssl_msgs REQUIRED) -pkg_check_modules(BFL REQUIRED orocos-bfl) +# pkg_check_modules(BFL REQUIRED orocos-bfl) # Tracker Component add_library(tracker_component SHARED src/tracker_component.cpp - src/ball_tracker.cpp src/ball_kalman_filter.cpp - src/robot_tracker.cpp + src/robot_kalman_filter.cpp + # src/robot_tracker.cpp src/visualization_data_handler.cpp ) target_compile_definitions(tracker_component @@ -49,10 +49,10 @@ target_include_directories(tracker_component PUBLIC "$" "$" "$" - "$" + # "$" ) target_link_libraries(tracker_component - ${BFL_LIBRARIES} + # ${BFL_LIBRARIES} ) rclcpp_components_register_nodes(tracker_component "consai_vision_tracker::Tracker") diff --git a/consai_vision_tracker/include/consai_vision_tracker/robot_kalman_filter.hpp b/consai_vision_tracker/include/consai_vision_tracker/robot_kalman_filter.hpp new file mode 100644 index 00000000..7edb608d --- /dev/null +++ b/consai_vision_tracker/include/consai_vision_tracker/robot_kalman_filter.hpp @@ -0,0 +1,87 @@ +// Copyright 2024 Roots +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef CONSAI_VISION_TRACKER__ROBOT_KALMAN_FILTER_HPP_ +#define CONSAI_VISION_TRACKER__ROBOT_KALMAN_FILTER_HPP_ + +#include +#include + +#include "consai_msgs/msg/robot_local_velocity.hpp" +#include "robocup_ssl_msgs/msg/detection_robot.hpp" +#include "robocup_ssl_msgs/msg/robot_id.hpp" +#include "robocup_ssl_msgs/msg/tracked_robot.hpp" + + +namespace consai_vision_tracker +{ + +using DetectionRobot = robocup_ssl_msgs::msg::DetectionRobot; +using RobotId = robocup_ssl_msgs::msg::RobotId; +using TrackedRobot = robocup_ssl_msgs::msg::TrackedRobot; +using RobotLocalVelocity = consai_msgs::msg::RobotLocalVelocity; +using Vector6d = Eigen::Matrix; +using Vector3d = Eigen::Vector3d; +using Matrix3d = Eigen::Matrix3d; +using Matrix6d = Eigen::Matrix; +using Matrix36d = Eigen::Matrix; + +class RobotKalmanFilter +{ + public: + explicit RobotKalmanFilter( + const int team_color, const int id, + const double dt = 0.01, const double lifetime = 2.0, + const double visibility_increase_rate = 5.0, + const double outlier_time_limit = 0.1); + + void push_back_observation(const DetectionRobot & robot); + TrackedRobot update(void); + TrackedRobot get_estimation(void) const; + void update_noise_covariance_matrix( + const double q_max_acc_xy, const double q_max_acc_theta, + const double r_pos_stddev_xy, const double r_pos_stddev_theta); + RobotLocalVelocity calc_local_velocity(void); + + private: + Vector6d predict_state(void) const; + Matrix6d jacobian_F(void) const; + Vector3d make_observation(void) const; + bool is_outlier(const double chi_squared_value) const; + void reset_x_and_p(const Vector3d & observation); + double normalize_angle(const double angle) const; + + const double DT_; + const double VISIBILITY_CONTROL_VALUE_; + const double VISIBILITY_INCREASE_RATE_; + const int OUTLIER_COUNT_THRESHOLD_; + const double OMEGA_ZERO_ = 1.0e-6; + + double visibility_ = 1.0; + int outlier_count_ = 0; + + RobotId robot_id_; + std::vector robot_observations_; + + Vector6d x_; + Matrix6d F_; + Matrix36d H_; + Matrix6d Q_; + Matrix3d R_; + Matrix6d P_; +}; + +} // namespace consai_vision_tracker + +#endif // CONSAI_VISION_TRACKER__ROBOT_KALMAN_FILTER_HPP_ diff --git a/consai_vision_tracker/include/consai_vision_tracker/tracker_component.hpp b/consai_vision_tracker/include/consai_vision_tracker/tracker_component.hpp index 37b5e451..e770b472 100644 --- a/consai_vision_tracker/include/consai_vision_tracker/tracker_component.hpp +++ b/consai_vision_tracker/include/consai_vision_tracker/tracker_component.hpp @@ -22,7 +22,7 @@ #include "consai_msgs/msg/robot_local_velocity.hpp" #include "consai_vision_tracker/visibility_control.h" #include "consai_vision_tracker/ball_kalman_filter.hpp" -#include "consai_vision_tracker/robot_tracker.hpp" +#include "consai_vision_tracker/robot_kalman_filter.hpp" #include "consai_vision_tracker/visualization_data_handler.hpp" #include "robocup_ssl_msgs/msg/detection_ball.hpp" #include "robocup_ssl_msgs/msg/detection_frame.hpp" @@ -65,8 +65,8 @@ class Tracker : public rclcpp::Node rclcpp::Publisher::SharedPtr pub_tracked_; rclcpp::Publisher::SharedPtr pub_robot_velocities_; std::unique_ptr ball_kalman_filter_; - std::vector> blue_robot_tracker_; - std::vector> yellow_robot_tracker_; + std::vector> blue_robot_kalman_filter_; + std::vector> yellow_robot_kalman_filter_; std::shared_ptr vis_data_handler_; }; diff --git a/consai_vision_tracker/package.xml b/consai_vision_tracker/package.xml index 935d199c..48e6abca 100644 --- a/consai_vision_tracker/package.xml +++ b/consai_vision_tracker/package.xml @@ -17,7 +17,7 @@ rclcpp rclcpp_components robocup_ssl_msgs - liborocos-bfl-dev + ament_lint_auto ament_lint_common diff --git a/consai_vision_tracker/src/robot_kalman_filter.cpp b/consai_vision_tracker/src/robot_kalman_filter.cpp new file mode 100644 index 00000000..3562c7d6 --- /dev/null +++ b/consai_vision_tracker/src/robot_kalman_filter.cpp @@ -0,0 +1,344 @@ +// Copyright 2024 Roots +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "consai_vision_tracker/robot_kalman_filter.hpp" +#include "robocup_ssl_msgs/msg/vector2.hpp" + +#include + +namespace consai_vision_tracker +{ + +using Matrix63d = Eigen::Matrix; +using SSLVector2 = robocup_ssl_msgs::msg::Vector2; + +RobotKalmanFilter::RobotKalmanFilter( + const int team_color, const int id, + const double dt, const double lifetime, + const double visibility_increase_rate, + const double outlier_time_limit) : + DT_(dt), + VISIBILITY_CONTROL_VALUE_(dt / lifetime), + VISIBILITY_INCREASE_RATE_(visibility_increase_rate), + OUTLIER_COUNT_THRESHOLD_(outlier_time_limit / dt) +{ + robot_id_.team_color = team_color; + robot_id_.id = id; + + // Observation matrix + H_ = Matrix36d::Zero(); + H_(0, 0) = 1.0; + H_(1, 1) = 1.0; + H_(2, 2) = 1.0; + + // Process noise covariance matrix + // Observation noise covariance matrix + update_noise_covariance_matrix( + 50.0, // xy m/s^2 + 30.0, // theta rad/s^2 + 0.05, // xy m + 0.05 // theta rad + ); + + // State vector + // x_ = (x, y, theta, vx, vy, vtheta) + // Error covariance matrix + reset_x_and_p(Vector3d::Zero()); +} + +void RobotKalmanFilter::push_back_observation(const DetectionRobot & robot) +{ + // Empty orientation data means that the data is not available + if (robot.orientation.size() == 0) { + return; + } + + TrackedRobot observation; + observation.pos.x = robot.x * 0.001; // mm to meters + observation.pos.y = robot.y * 0.001; // mm to meters + observation.orientation = robot.orientation[0]; + robot_observations_.push_back(observation); +} + +TrackedRobot RobotKalmanFilter::update(void) +{ + + Vector6d x_pred; + Matrix6d P_pred; + + auto prediction = [&, this](void) { + x_pred = predict_state(); + P_pred = F_ * P_ * F_.transpose() + Q_; + }; + + auto calc_innovation = [&](const Vector3d & z) { + Vector3d innovation = z - H_ * x_pred; + innovation(2) = normalize_angle(z(2) - (H_ * x_pred)(2)); + return innovation; + }; + + auto calc_chi_square = [&](const Vector3d & z) { + Matrix3d S = H_ * P_pred * H_.transpose() + R_; + Vector3d innovation = calc_innovation(z); + return innovation.transpose() * S.inverse() * innovation; + }; + + // Prediction step + prediction(); + + // Remove outliers + for (auto it = robot_observations_.begin(); it != robot_observations_.end(); ) { + const Vector3d z(it->pos.x, it->pos.y, it->orientation); + const double chi_square = calc_chi_square(z); + + if (!is_outlier(chi_square)) { + outlier_count_ = 0; + it++; + continue; + } + + outlier_count_++; + if (outlier_count_ < OUTLIER_COUNT_THRESHOLD_) { + it = robot_observations_.erase(it); + } else { + reset_x_and_p(z); + prediction(); + it++; + } + } + + // Make observation + Vector3d z(x_[0], x_[1], x_[2]); + if (robot_observations_.empty()) { + visibility_ = std::max(0.0, visibility_ - VISIBILITY_CONTROL_VALUE_); + } else { + z = make_observation(); + visibility_ = std::min(1.0, visibility_ + VISIBILITY_INCREASE_RATE_ * VISIBILITY_CONTROL_VALUE_); + robot_observations_.clear(); + } + + // Calculate innovation + Vector3d innovation = calc_innovation(z); + // Calculate innovation covariance matrix + Matrix3d S = H_ * P_pred * H_.transpose() + R_; + + // Update step + Matrix63d K = P_pred * H_.transpose() * S.inverse(); + x_ = x_pred + K * innovation; + P_ = (Matrix6d::Identity() - K * H_) * P_pred; + + return get_estimation(); +} + +TrackedRobot RobotKalmanFilter::get_estimation(void) const +{ + TrackedRobot estimation; + estimation.robot_id = robot_id_; + estimation.pos.x = x_(0); + estimation.pos.y = x_(1); + estimation.orientation = x_(2); + + SSLVector2 vel; + vel.x = x_(3); + vel.y = x_(4); + estimation.vel.push_back(vel); + estimation.vel_angular.push_back(x_(5)); + estimation.visibility.push_back(visibility_); + + return estimation; +} + +void RobotKalmanFilter::update_noise_covariance_matrix( + const double q_max_acc_xy, const double q_max_acc_theta, + const double r_pos_stddev_xy, const double r_pos_stddev_theta) +{ + // Process noise covariance matrix + auto gen_Q = [this](const double q_max_acc_xy, const double q_max_acc_theta) { + // Process noise depends on the maximum acceleration + const double sigma_vel_xy = q_max_acc_xy * DT_; + const double sigma_vel_theta = q_max_acc_theta * DT_; + const double sigma_pos_xy = 0.5 * q_max_acc_xy * DT_ * DT_; + const double sigma_pos_theta = 0.5 * q_max_acc_theta * DT_ * DT_; + const double var_vel_xy = sigma_vel_xy * sigma_vel_xy; + const double var_vel_theta = sigma_vel_theta * sigma_vel_theta; + const double var_pos_xy = sigma_pos_xy * sigma_pos_xy; + const double var_pos_theta = sigma_pos_theta * sigma_pos_theta; + + Matrix6d Q = Matrix6d::Zero(); + Q(0, 0) = var_pos_xy; + Q(1, 1) = var_pos_xy; + Q(2, 2) = var_pos_theta; + Q(3, 3) = var_vel_xy; + Q(4, 4) = var_vel_xy; + Q(5, 5) = var_vel_theta; + return Q; + }; + Q_ = gen_Q(q_max_acc_xy, q_max_acc_theta); + + // Observation noise covariance matrix + auto gen_R = [&](const double std_dev_xy, const double std_dev_theta) { + const double var_xy = std_dev_xy * std_dev_xy; + const double var_theta = std_dev_theta * std_dev_theta; + return Eigen::DiagonalMatrix(var_xy, var_xy, var_theta); + }; + R_ = gen_R(r_pos_stddev_xy, r_pos_stddev_theta); +} + +RobotLocalVelocity RobotKalmanFilter::calc_local_velocity(void) +{ + auto local_velocity = RobotLocalVelocity(); + local_velocity.robot_id = robot_id_; + + const auto estimation = get_estimation(); + const auto theta = estimation.orientation; + const auto world_vel = estimation.vel[0]; + + auto local_vel = world_vel; + local_vel.x = world_vel.x * std::cos(theta) + world_vel.y * std::sin(theta); + local_vel.y = -(world_vel.x * std::sin(theta) - world_vel.y * std::cos(theta)); + local_velocity.velocity.x = local_vel.x; + local_velocity.velocity.y = local_vel.y; + local_velocity.norm = std::sqrt(std::pow(local_vel.x, 2) + std::pow(local_vel.y, 2)); + local_velocity.velocity.theta = estimation.vel_angular[0]; + + return local_velocity; +} + +Vector6d RobotKalmanFilter::predict_state(void) const +{ + // TODO: Use input data to predict state + Vector6d x_pred; + + const double x = x_(0); + const double y = x_(1); + const double theta = x_(2); + const double vx = x_(3); + const double vy = x_(4); + const double omega = x_(5); + + if (fabs(omega) > OMEGA_ZERO_) { + x_pred(0) = x + (vx / omega) * (std::sin(theta + omega * DT_) - std::sin(theta)) + + (vy / omega) * (std::cos(theta) - std::cos(theta + omega * DT_)); + x_pred(1) = y + (vx / omega) * (std::cos(theta) - std::cos(theta + omega * DT_)) + + (vy / omega) * (std::sin(theta + omega * DT_) - std::sin(theta)); + } else { + x_pred(0) = x + vx * DT_; + x_pred(1) = y + vy * DT_; + } + + x_pred(2) = theta + omega * DT_; + x_pred(3) = vx; + x_pred(4) = vy; + x_pred(5) = omega; + + return x_pred; +} + +Matrix6d RobotKalmanFilter::jacobian_F(void) const +{ + // TODO: Use input data to calculate jacobian matrix + Matrix6d F = Matrix6d::Identity(); + + const double theta = x_(2); + const double vx = x_(3); + const double vy = x_(4); + const double omega = x_(5); + + if (fabs(omega) > OMEGA_ZERO_) { + const double VX_O = vx / omega; + const double VY_O = vy / omega; + const double VX_OO = vx / (omega * omega); + const double VY_OO = vy / (omega * omega); + const double INV_O = 1.0 / omega; + const double ALPHA_THETA = theta + omega * DT_; + + F(0, 2) = VX_O * (std::cos(ALPHA_THETA) - std::cos(theta)) + - VY_O * (std::sin(theta) - std::sin(ALPHA_THETA)); // d(x)/d(theta) + F(0, 3) = INV_O * (std::sin(ALPHA_THETA) - std::sin(theta)); // d(x)/d(vx) + F(0, 4) = INV_O * (std::cos(theta) - std::cos(ALPHA_THETA)); // d(x)/d(vy) + F(0, 5) = -VX_OO * (std::sin(ALPHA_THETA) - std::sin(theta)) + + VX_O * DT_ * std::cos(ALPHA_THETA) + - VY_OO * (std::cos(theta) - std::cos(ALPHA_THETA)) + + VY_O * DT_ * std::sin(ALPHA_THETA); // d(x)/d(omega); + + F(1, 2) = VX_O - (std::sin(theta) - std::sin(ALPHA_THETA)) + + VY_O * (std::cos(ALPHA_THETA) - std::cos(theta)); // d(y)/d(theta) + F(1, 3) = INV_O * (std::cos(theta) - std::cos(ALPHA_THETA)); // d(y)/d(vx) + F(1, 4) = INV_O * (std::sin(ALPHA_THETA) - std::sin(theta)); // d(y)/d(vy) + F(1, 5) = -VX_OO * (std::cos(theta) - std::cos(ALPHA_THETA)) + + VX_O * DT_ * std::sin(ALPHA_THETA) + - VY_OO * (std::sin(ALPHA_THETA) - std::sin(theta)) + + VY_O * DT_ * std::cos(ALPHA_THETA); // d(y)/d(omega); + + F(2, 5) = DT_; // d(theta)/d(omega) + } else { + F(0, 3) = DT_; + F(1, 4) = DT_; + F(2, 5) = DT_; + } + + return F; +} + +Vector3d RobotKalmanFilter::make_observation(void) const +{ + if (robot_observations_.empty()) { + return Vector3d(x_[0], x_[1], x_[2]); + } + + Vector3d mean_observation(0.0, 0.0, 0.0); + double sum_cos = 0.0; + double sum_sin = 0.0; + for (const auto & observation : robot_observations_) { + mean_observation(0) += observation.pos.x; + mean_observation(1) += observation.pos.y; + sum_cos += std::cos(observation.orientation); + sum_sin += std::sin(observation.orientation); + } + mean_observation(0) /= robot_observations_.size(); + mean_observation(1) /= robot_observations_.size(); + sum_cos /= robot_observations_.size(); + sum_sin /= robot_observations_.size(); + mean_observation(2) = std::atan2(sum_sin, sum_cos); + + return mean_observation; +} + +bool RobotKalmanFilter::is_outlier(const double chi_squared_value) const +{ + // 3 degrees of freedom (x, y, theta), 0.05 significance level + constexpr double THRESHOLD = 7.815; + + if (chi_squared_value > THRESHOLD) { + return true; + } + return false; +} + +void RobotKalmanFilter::reset_x_and_p(const Vector3d & observation) +{ + x_ << observation[0], observation[1], observation[2], 0.0, 0.0, 0.0; + P_ = Matrix6d::Identity() * 10.0; +} + +double RobotKalmanFilter::normalize_angle(const double angle) const +{ + // Normalize angle to -pi ~ pi + double result = angle; + while (result > M_PI) result -= 2.0 * M_PI; + while (result < -M_PI) result += 2.0 * M_PI; + return result; +} + +} // namespace consai_vision_tracker diff --git a/consai_vision_tracker/src/tracker_component.cpp b/consai_vision_tracker/src/tracker_component.cpp index 336f1c8c..91aae8d0 100644 --- a/consai_vision_tracker/src/tracker_component.cpp +++ b/consai_vision_tracker/src/tracker_component.cpp @@ -45,14 +45,18 @@ Tracker::Tracker(const rclcpp::NodeOptions & options) declare_parameter("ball_kalman_filter/q_acc", 4.0); declare_parameter("ball_kalman_filter/q_uncertain_acc", 50.0); declare_parameter("ball_kalman_filter/r_pos_stddev", 0.05); + declare_parameter("robot_kalman_filter/q_max_acc_xy", 50.0); + declare_parameter("robot_kalman_filter/q_max_acc_theta", 3.0); + declare_parameter("robot_kalman_filter/r_pos_stddev_xy", 0.05); + declare_parameter("robot_kalman_filter/r_pos_stddev_theta", 0.03); for (int i = 0; i < 16; i++) { - blue_robot_tracker_.push_back( - std::make_shared( + blue_robot_kalman_filter_.push_back( + std::make_unique( RobotId::TEAM_COLOR_BLUE, i, UPDATE_RATE.count())); - yellow_robot_tracker_.push_back( - std::make_shared( + yellow_robot_kalman_filter_.push_back( + std::make_unique( RobotId::TEAM_COLOR_YELLOW, i, UPDATE_RATE.count())); } @@ -90,22 +94,34 @@ void Tracker::on_timer() get_parameter("ball_kalman_filter/q_uncertain_acc").get_value(), get_parameter("ball_kalman_filter/r_pos_stddev").get_value()); - for (auto && tracker : blue_robot_tracker_) { - tracked_msg->robots.push_back(tracker->update()); - robot_vel_msg->velocities.push_back(tracker->calc_local_velocity()); + // TODO: use parameter callback + for (auto && filter : blue_robot_kalman_filter_) { + filter->update_noise_covariance_matrix( + get_parameter("robot_kalman_filter/q_max_acc_xy").get_value(), + get_parameter("robot_kalman_filter/q_max_acc_theta").get_value(), + get_parameter("robot_kalman_filter/r_pos_stddev_xy").get_value(), + get_parameter("robot_kalman_filter/r_pos_stddev_theta").get_value()); + tracked_msg->robots.push_back(filter->update()); + robot_vel_msg->velocities.push_back(filter->calc_local_velocity()); } - for (auto && tracker : yellow_robot_tracker_) { - tracked_msg->robots.push_back(tracker->update()); - robot_vel_msg->velocities.push_back(tracker->calc_local_velocity()); + // TODO: use parameter callback + for (auto && filter : yellow_robot_kalman_filter_) { + filter->update_noise_covariance_matrix( + get_parameter("robot_kalman_filter/q_max_acc_xy").get_value(), + get_parameter("robot_kalman_filter/q_max_acc_theta").get_value(), + get_parameter("robot_kalman_filter/r_pos_stddev_xy").get_value(), + get_parameter("robot_kalman_filter/r_pos_stddev_theta").get_value()); + tracked_msg->robots.push_back(filter->update()); + robot_vel_msg->velocities.push_back(filter->calc_local_velocity()); } auto ball_is_near_a_robot = [this]( - const decltype(blue_robot_tracker_) & trackers, + const decltype(blue_robot_kalman_filter_) & filters, const State & ball_pose) { const auto NEAR_DISTANCE = 0.5; - for (auto && tracker : trackers) { - const auto robot = tracker->prev_estimation(); + for (auto && filter : filters) { + const auto robot = filter->get_estimation(); if (!tools::is_visible(robot)) { continue; } @@ -123,8 +139,8 @@ void Tracker::on_timer() if (tools::is_visible(ball)) { const auto ball_pose = tools::pose_state(ball); use_uncertain_sys_model = - ball_is_near_a_robot(blue_robot_tracker_, ball_pose) || - ball_is_near_a_robot(yellow_robot_tracker_, ball_pose); + ball_is_near_a_robot(blue_robot_kalman_filter_, ball_pose) || + ball_is_near_a_robot(yellow_robot_kalman_filter_, ball_pose); } tracked_msg->balls.push_back(ball_kalman_filter_->update(use_uncertain_sys_model)); @@ -143,13 +159,13 @@ void Tracker::callback_detection(const DetectionFrame::SharedPtr msg) for (const auto & blue_robot : msg->robots_blue) { if (blue_robot.robot_id.size() > 0) { - blue_robot_tracker_[blue_robot.robot_id[0]]->push_back_observation(blue_robot); + blue_robot_kalman_filter_[blue_robot.robot_id[0]]->push_back_observation(blue_robot); } } for (const auto & yellow_robot : msg->robots_yellow) { if (yellow_robot.robot_id.size() > 0) { - yellow_robot_tracker_[yellow_robot.robot_id[0]]->push_back_observation(yellow_robot); + yellow_robot_kalman_filter_[yellow_robot.robot_id[0]]->push_back_observation(yellow_robot); } } @@ -168,14 +184,14 @@ void Tracker::callback_detection_invert(const DetectionFrame::SharedPtr msg) for (auto && blue_robot : msg->robots_blue) { if (blue_robot.robot_id.size() > 0) { invert_robot(blue_robot); - blue_robot_tracker_[blue_robot.robot_id[0]]->push_back_observation(blue_robot); + blue_robot_kalman_filter_[blue_robot.robot_id[0]]->push_back_observation(blue_robot); } } for (auto && yellow_robot : msg->robots_yellow) { if (yellow_robot.robot_id.size() > 0) { invert_robot(yellow_robot); - yellow_robot_tracker_[yellow_robot.robot_id[0]]->push_back_observation(yellow_robot); + yellow_robot_kalman_filter_[yellow_robot.robot_id[0]]->push_back_observation(yellow_robot); } } From 12153960b32829c03c45eb3fb1eda35fe889999d Mon Sep 17 00:00:00 2001 From: ShotaAk Date: Tue, 13 Aug 2024 13:40:55 +0900 Subject: [PATCH 07/17] Update --- .../robot_kalman_filter.hpp | 2 +- .../src/ball_kalman_filter.cpp | 3 +- .../src/robot_kalman_filter.cpp | 29 +++++++++++++------ .../src/tracker_component.cpp | 8 ++--- 4 files changed, 27 insertions(+), 15 deletions(-) diff --git a/consai_vision_tracker/include/consai_vision_tracker/robot_kalman_filter.hpp b/consai_vision_tracker/include/consai_vision_tracker/robot_kalman_filter.hpp index 7edb608d..258c7292 100644 --- a/consai_vision_tracker/include/consai_vision_tracker/robot_kalman_filter.hpp +++ b/consai_vision_tracker/include/consai_vision_tracker/robot_kalman_filter.hpp @@ -52,7 +52,7 @@ class RobotKalmanFilter void update_noise_covariance_matrix( const double q_max_acc_xy, const double q_max_acc_theta, const double r_pos_stddev_xy, const double r_pos_stddev_theta); - RobotLocalVelocity calc_local_velocity(void); + RobotLocalVelocity calc_local_velocity(void) const; private: Vector6d predict_state(void) const; diff --git a/consai_vision_tracker/src/ball_kalman_filter.cpp b/consai_vision_tracker/src/ball_kalman_filter.cpp index 7ef5b6d5..6bfd73aa 100644 --- a/consai_vision_tracker/src/ball_kalman_filter.cpp +++ b/consai_vision_tracker/src/ball_kalman_filter.cpp @@ -79,7 +79,8 @@ TrackedBall BallKalmanFilter::update(const bool use_uncertain_sys_model) auto calc_chi_square = [&](const Vector2d & z) { Matrix2d S = H_ * P_pred * H_.transpose() + R_; Vector2d innovation = z - H_ * x_pred; - return innovation.transpose() * S.inverse() * innovation; + double chi_square = innovation.transpose() * S.inverse() * innovation; + return chi_square; }; // Prediction step diff --git a/consai_vision_tracker/src/robot_kalman_filter.cpp b/consai_vision_tracker/src/robot_kalman_filter.cpp index 3562c7d6..3dc29214 100644 --- a/consai_vision_tracker/src/robot_kalman_filter.cpp +++ b/consai_vision_tracker/src/robot_kalman_filter.cpp @@ -73,7 +73,6 @@ void RobotKalmanFilter::push_back_observation(const DetectionRobot & robot) TrackedRobot RobotKalmanFilter::update(void) { - Vector6d x_pred; Matrix6d P_pred; @@ -91,7 +90,8 @@ TrackedRobot RobotKalmanFilter::update(void) auto calc_chi_square = [&](const Vector3d & z) { Matrix3d S = H_ * P_pred * H_.transpose() + R_; Vector3d innovation = calc_innovation(z); - return innovation.transpose() * S.inverse() * innovation; + double chi_square = innovation.transpose() * S.inverse() * innovation; + return chi_square; }; // Prediction step @@ -113,7 +113,7 @@ TrackedRobot RobotKalmanFilter::update(void) it = robot_observations_.erase(it); } else { reset_x_and_p(z); - prediction(); + prediction(); // Use new state vector it++; } } @@ -170,10 +170,14 @@ void RobotKalmanFilter::update_noise_covariance_matrix( const double sigma_vel_theta = q_max_acc_theta * DT_; const double sigma_pos_xy = 0.5 * q_max_acc_xy * DT_ * DT_; const double sigma_pos_theta = 0.5 * q_max_acc_theta * DT_ * DT_; - const double var_vel_xy = sigma_vel_xy * sigma_vel_xy; - const double var_vel_theta = sigma_vel_theta * sigma_vel_theta; - const double var_pos_xy = sigma_pos_xy * sigma_pos_xy; - const double var_pos_theta = sigma_pos_theta * sigma_pos_theta; + + const double var_vel_xy = std::pow(sigma_vel_xy, 2); + const double var_vel_theta = std::pow(sigma_vel_theta, 2); + const double var_pos_xy = std::pow(sigma_pos_xy, 2); + const double var_pos_theta = std::pow(sigma_pos_theta, 2); + + const double sigma_vel_pos_xy = sigma_vel_xy * sigma_pos_xy; + const double sigma_vel_pos_theta = sigma_vel_theta * sigma_pos_theta; Matrix6d Q = Matrix6d::Zero(); Q(0, 0) = var_pos_xy; @@ -182,6 +186,13 @@ void RobotKalmanFilter::update_noise_covariance_matrix( Q(3, 3) = var_vel_xy; Q(4, 4) = var_vel_xy; Q(5, 5) = var_vel_theta; + + Q(0, 3) = sigma_vel_pos_xy; + Q(1, 4) = sigma_vel_pos_xy; + Q(2, 5) = sigma_vel_pos_theta; + Q(3, 0) = sigma_vel_pos_xy; + Q(4, 1) = sigma_vel_pos_xy; + Q(5, 2) = sigma_vel_pos_theta; return Q; }; Q_ = gen_Q(q_max_acc_xy, q_max_acc_theta); @@ -195,7 +206,7 @@ void RobotKalmanFilter::update_noise_covariance_matrix( R_ = gen_R(r_pos_stddev_xy, r_pos_stddev_theta); } -RobotLocalVelocity RobotKalmanFilter::calc_local_velocity(void) +RobotLocalVelocity RobotKalmanFilter::calc_local_velocity(void) const { auto local_velocity = RobotLocalVelocity(); local_velocity.robot_id = robot_id_; @@ -237,7 +248,7 @@ Vector6d RobotKalmanFilter::predict_state(void) const x_pred(1) = y + vy * DT_; } - x_pred(2) = theta + omega * DT_; + x_pred(2) = normalize_angle(theta + omega * DT_); x_pred(3) = vx; x_pred(4) = vy; x_pred(5) = omega; diff --git a/consai_vision_tracker/src/tracker_component.cpp b/consai_vision_tracker/src/tracker_component.cpp index 91aae8d0..cdbd2563 100644 --- a/consai_vision_tracker/src/tracker_component.cpp +++ b/consai_vision_tracker/src/tracker_component.cpp @@ -42,11 +42,11 @@ Tracker::Tracker(const rclcpp::NodeOptions & options) const auto UPDATE_RATE = 0.01s; ball_kalman_filter_ = std::make_unique(UPDATE_RATE.count()); - declare_parameter("ball_kalman_filter/q_acc", 4.0); - declare_parameter("ball_kalman_filter/q_uncertain_acc", 50.0); + declare_parameter("ball_kalman_filter/q_acc", 5.0); + declare_parameter("ball_kalman_filter/q_uncertain_acc", 30.0); declare_parameter("ball_kalman_filter/r_pos_stddev", 0.05); - declare_parameter("robot_kalman_filter/q_max_acc_xy", 50.0); - declare_parameter("robot_kalman_filter/q_max_acc_theta", 3.0); + declare_parameter("robot_kalman_filter/q_max_acc_xy", 100.0); + declare_parameter("robot_kalman_filter/q_max_acc_theta", 300.0); declare_parameter("robot_kalman_filter/r_pos_stddev_xy", 0.05); declare_parameter("robot_kalman_filter/r_pos_stddev_theta", 0.03); From 7f59a2243f1d9cbea496bf94f021c0fa9ab33094 Mon Sep 17 00:00:00 2001 From: ShotaAk Date: Tue, 13 Aug 2024 14:40:10 +0900 Subject: [PATCH 08/17] RoboCup SSL robot is the linear system --- .../src/robot_kalman_filter.cpp | 22 +++++++++++++++---- .../src/tracker_component.cpp | 6 ++--- 2 files changed, 21 insertions(+), 7 deletions(-) diff --git a/consai_vision_tracker/src/robot_kalman_filter.cpp b/consai_vision_tracker/src/robot_kalman_filter.cpp index 3dc29214..5d60a22d 100644 --- a/consai_vision_tracker/src/robot_kalman_filter.cpp +++ b/consai_vision_tracker/src/robot_kalman_filter.cpp @@ -20,6 +20,8 @@ namespace consai_vision_tracker { +using Vector2d = Eigen::Vector2d; +using Matrix2d = Eigen::Matrix2d; using Matrix63d = Eigen::Matrix; using SSLVector2 = robocup_ssl_msgs::msg::Vector2; @@ -36,6 +38,12 @@ RobotKalmanFilter::RobotKalmanFilter( robot_id_.team_color = team_color; robot_id_.id = id; + // State transition matrix + F_ = Matrix6d::Identity(); + F_(0, 3) = DT_; + F_(1, 4) = DT_; + F_(2, 5) = DT_; + // Observation matrix H_ = Matrix36d::Zero(); H_(0, 0) = 1.0; @@ -77,7 +85,8 @@ TrackedRobot RobotKalmanFilter::update(void) Matrix6d P_pred; auto prediction = [&, this](void) { - x_pred = predict_state(); + x_pred = F_ * x_; + x_pred(2) = normalize_angle(x_pred(2)); P_pred = F_ * P_ * F_.transpose() + Q_; }; @@ -90,7 +99,12 @@ TrackedRobot RobotKalmanFilter::update(void) auto calc_chi_square = [&](const Vector3d & z) { Matrix3d S = H_ * P_pred * H_.transpose() + R_; Vector3d innovation = calc_innovation(z); - double chi_square = innovation.transpose() * S.inverse() * innovation; + + // Use x and y only + Matrix2d S_xy = S.block<2, 2>(0, 0); + Vector2d innovation_xy = innovation.block<2, 1>(0, 0); + + double chi_square = innovation_xy.transpose() * S_xy.inverse() * innovation_xy; return chi_square; }; @@ -136,6 +150,7 @@ TrackedRobot RobotKalmanFilter::update(void) // Update step Matrix63d K = P_pred * H_.transpose() * S.inverse(); x_ = x_pred + K * innovation; + x_(2) = normalize_angle(x_(2)); P_ = (Matrix6d::Identity() - K * H_) * P_pred; return get_estimation(); @@ -328,8 +343,7 @@ Vector3d RobotKalmanFilter::make_observation(void) const bool RobotKalmanFilter::is_outlier(const double chi_squared_value) const { - // 3 degrees of freedom (x, y, theta), 0.05 significance level - constexpr double THRESHOLD = 7.815; + constexpr double THRESHOLD = 5.991; // 2 degrees of freedom, 0.05 significance level if (chi_squared_value > THRESHOLD) { return true; diff --git a/consai_vision_tracker/src/tracker_component.cpp b/consai_vision_tracker/src/tracker_component.cpp index cdbd2563..592abb47 100644 --- a/consai_vision_tracker/src/tracker_component.cpp +++ b/consai_vision_tracker/src/tracker_component.cpp @@ -45,10 +45,10 @@ Tracker::Tracker(const rclcpp::NodeOptions & options) declare_parameter("ball_kalman_filter/q_acc", 5.0); declare_parameter("ball_kalman_filter/q_uncertain_acc", 30.0); declare_parameter("ball_kalman_filter/r_pos_stddev", 0.05); - declare_parameter("robot_kalman_filter/q_max_acc_xy", 100.0); - declare_parameter("robot_kalman_filter/q_max_acc_theta", 300.0); + declare_parameter("robot_kalman_filter/q_max_acc_xy", 10.0); + declare_parameter("robot_kalman_filter/q_max_acc_theta", 6.0); declare_parameter("robot_kalman_filter/r_pos_stddev_xy", 0.05); - declare_parameter("robot_kalman_filter/r_pos_stddev_theta", 0.03); + declare_parameter("robot_kalman_filter/r_pos_stddev_theta", 0.06); for (int i = 0; i < 16; i++) { blue_robot_kalman_filter_.push_back( From 2f6c7fad6ed5d9b67c9e5240251e8255a8fedcc8 Mon Sep 17 00:00:00 2001 From: ShotaAk Date: Tue, 13 Aug 2024 15:01:53 +0900 Subject: [PATCH 09/17] Remove unused files --- consai_vision_tracker/CMakeLists.txt | 6 - .../consai_vision_tracker/ball_tracker.hpp | 70 ---- .../robot_kalman_filter.hpp | 2 - .../consai_vision_tracker/robot_tracker.hpp | 74 ---- consai_vision_tracker/package.xml | 1 - consai_vision_tracker/src/ball_tracker.cpp | 272 -------------- .../src/robot_kalman_filter.cpp | 76 ---- consai_vision_tracker/src/robot_tracker.cpp | 353 ------------------ .../src/tracker_component.cpp | 22 +- 9 files changed, 14 insertions(+), 862 deletions(-) delete mode 100644 consai_vision_tracker/include/consai_vision_tracker/ball_tracker.hpp delete mode 100644 consai_vision_tracker/include/consai_vision_tracker/robot_tracker.hpp delete mode 100644 consai_vision_tracker/src/ball_tracker.cpp delete mode 100644 consai_vision_tracker/src/robot_tracker.cpp diff --git a/consai_vision_tracker/CMakeLists.txt b/consai_vision_tracker/CMakeLists.txt index c9e36bc1..e472b717 100644 --- a/consai_vision_tracker/CMakeLists.txt +++ b/consai_vision_tracker/CMakeLists.txt @@ -24,14 +24,12 @@ find_package(Eigen3 REQUIRED) find_package(rclcpp REQUIRED) find_package(rclcpp_components REQUIRED) find_package(robocup_ssl_msgs REQUIRED) -# pkg_check_modules(BFL REQUIRED orocos-bfl) # Tracker Component add_library(tracker_component SHARED src/tracker_component.cpp src/ball_kalman_filter.cpp src/robot_kalman_filter.cpp - # src/robot_tracker.cpp src/visualization_data_handler.cpp ) target_compile_definitions(tracker_component @@ -49,10 +47,6 @@ target_include_directories(tracker_component PUBLIC "$" "$" "$" - # "$" -) -target_link_libraries(tracker_component - # ${BFL_LIBRARIES} ) rclcpp_components_register_nodes(tracker_component "consai_vision_tracker::Tracker") diff --git a/consai_vision_tracker/include/consai_vision_tracker/ball_tracker.hpp b/consai_vision_tracker/include/consai_vision_tracker/ball_tracker.hpp deleted file mode 100644 index b34ca922..00000000 --- a/consai_vision_tracker/include/consai_vision_tracker/ball_tracker.hpp +++ /dev/null @@ -1,70 +0,0 @@ -// Copyright 2021 Roots -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef CONSAI_VISION_TRACKER__BALL_TRACKER_HPP_ -#define CONSAI_VISION_TRACKER__BALL_TRACKER_HPP_ - -#include -#include -#include -#include -#include -#include -#include - -#include "robocup_ssl_msgs/msg/detection_ball.hpp" -#include "robocup_ssl_msgs/msg/tracked_ball.hpp" - -namespace consai_vision_tracker -{ - -using DetectionBall = robocup_ssl_msgs::msg::DetectionBall; -using TrackedBall = robocup_ssl_msgs::msg::TrackedBall; -using ConditionalGaussian = BFL::LinearAnalyticConditionalGaussian; -using SystemModelGaussianUncertainty = BFL::LinearAnalyticSystemModelGaussianUncertainty; -using MeasurementModelGaussianUncertainty = BFL::LinearAnalyticMeasurementModelGaussianUncertainty; -using Gaussian = BFL::Gaussian; -using ExtendedKalmanFilter = BFL::ExtendedKalmanFilter; - -class BallTracker -{ -public: - explicit BallTracker(const double dt = 0.01); - - void push_back_observation(const DetectionBall & ball); - TrackedBall update(const bool use_uncertain_sys_model); - TrackedBall prev_estimation(void) const {return prev_tracked_ball_;} - -private: - void reset_prior(); - bool is_outlier(const TrackedBall & observation) const; - - std::vector ball_observations_; - TrackedBall prev_tracked_ball_; - - std::shared_ptr sys_uncertain_pdf_; - std::shared_ptr sys_uncertain_model_; - std::shared_ptr sys_certain_pdf_; - std::shared_ptr sys_certain_model_; - std::shared_ptr meas_pdf_; - std::shared_ptr meas_model_; - std::shared_ptr prior_; - std::shared_ptr filter_; - - int outlier_count_ = 0; -}; - -} // namespace consai_vision_tracker - -#endif // CONSAI_VISION_TRACKER__BALL_TRACKER_HPP_ diff --git a/consai_vision_tracker/include/consai_vision_tracker/robot_kalman_filter.hpp b/consai_vision_tracker/include/consai_vision_tracker/robot_kalman_filter.hpp index 258c7292..a04f9c13 100644 --- a/consai_vision_tracker/include/consai_vision_tracker/robot_kalman_filter.hpp +++ b/consai_vision_tracker/include/consai_vision_tracker/robot_kalman_filter.hpp @@ -55,8 +55,6 @@ class RobotKalmanFilter RobotLocalVelocity calc_local_velocity(void) const; private: - Vector6d predict_state(void) const; - Matrix6d jacobian_F(void) const; Vector3d make_observation(void) const; bool is_outlier(const double chi_squared_value) const; void reset_x_and_p(const Vector3d & observation); diff --git a/consai_vision_tracker/include/consai_vision_tracker/robot_tracker.hpp b/consai_vision_tracker/include/consai_vision_tracker/robot_tracker.hpp deleted file mode 100644 index 30f75f2d..00000000 --- a/consai_vision_tracker/include/consai_vision_tracker/robot_tracker.hpp +++ /dev/null @@ -1,74 +0,0 @@ -// Copyright 2021 Roots -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef CONSAI_VISION_TRACKER__ROBOT_TRACKER_HPP_ -#define CONSAI_VISION_TRACKER__ROBOT_TRACKER_HPP_ - -#include -#include -#include -#include -#include -#include -#include - -#include "consai_msgs/msg/robot_local_velocity.hpp" -#include "robocup_ssl_msgs/msg/detection_robot.hpp" -#include "robocup_ssl_msgs/msg/tracked_robot.hpp" - -namespace consai_vision_tracker -{ - -using RobotLocalVelocity = consai_msgs::msg::RobotLocalVelocity; -using DetectionRobot = robocup_ssl_msgs::msg::DetectionRobot; -using TrackedRobot = robocup_ssl_msgs::msg::TrackedRobot; -using ConditionalGaussian = BFL::LinearAnalyticConditionalGaussian; -using SystemModelGaussianUncertainty = BFL::LinearAnalyticSystemModelGaussianUncertainty; -using MeasurementModelGaussianUncertainty = BFL::LinearAnalyticMeasurementModelGaussianUncertainty; -using Gaussian = BFL::Gaussian; -using ExtendedKalmanFilter = BFL::ExtendedKalmanFilter; - -class RobotTracker -{ -public: - RobotTracker(const int team_color, const int id, const double dt = 0.01); - - void push_back_observation(const DetectionRobot & robot); - TrackedRobot update(); - RobotLocalVelocity calc_local_velocity(); - TrackedRobot prev_estimation(void) const {return prev_tracked_robot_;} - -private: - void reset_prior(); - bool is_outlier(const TrackedRobot & observation) const; - void correct_orientation_overflow_of_prior(); - double normalize_orientation(double orientation) const; - double normalize_orientation(const double from, const double to) const; - - std::vector robot_observations_; - TrackedRobot prev_tracked_robot_; - - std::shared_ptr sys_pdf_; - std::shared_ptr sys_model_; - std::shared_ptr meas_pdf_; - std::shared_ptr meas_model_; - std::shared_ptr prior_; - std::shared_ptr filter_; - - int outlier_count_ = 0; -}; - -} // namespace consai_vision_tracker - -#endif // CONSAI_VISION_TRACKER__ROBOT_TRACKER_HPP_ diff --git a/consai_vision_tracker/package.xml b/consai_vision_tracker/package.xml index 48e6abca..c32cc20d 100644 --- a/consai_vision_tracker/package.xml +++ b/consai_vision_tracker/package.xml @@ -17,7 +17,6 @@ rclcpp rclcpp_components robocup_ssl_msgs - ament_lint_auto ament_lint_common diff --git a/consai_vision_tracker/src/ball_tracker.cpp b/consai_vision_tracker/src/ball_tracker.cpp deleted file mode 100644 index 0ccee988..00000000 --- a/consai_vision_tracker/src/ball_tracker.cpp +++ /dev/null @@ -1,272 +0,0 @@ -// Copyright 2021 Roots -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include -#include -#include -#include - -#include "consai_vision_tracker/ball_tracker.hpp" -#include "robocup_ssl_msgs/msg/vector3.hpp" - -namespace consai_vision_tracker -{ - -using ColumnVector = BFL::ColumnVector; -using Matrix = BFL::Matrix; -using MatrixWrapper = BFL::Matrix_Wrapper; -using SymmetricMatrix = BFL::SymmetricMatrix; -using Vector3 = robocup_ssl_msgs::msg::Vector3; - -// visibilityの操作量 -// 追跡対象の情報が来ないとき、この操作量だけvisibilityを小さくする -// 操作量 = 1.0 / (trackerの更新周波数 * 消失判定までに掛ける時間) -// 例：trackerが100Hzで更新されるとき、操作量が0.002であれば、 -// 追跡対象が消失してから5秒かけてvisibilityが1.0から0.0になる -static const double VISIBILITY_CONTROL_VALUE = 0.005; - -static constexpr auto FIELD_MAX_X = 6.5; -static constexpr auto FIELD_MAX_Y = 5.0; - -BallTracker::BallTracker(const double dt) -{ - // visibilityはoptionalなので、ここでデフォルト値を設定しておく - prev_tracked_ball_.visibility.push_back(1.0); - // velocityはoptionalなので、ここでデフォルト値を設定しておく - prev_tracked_ball_.vel.push_back(Vector3()); - - // システムモデル - // pos(t+1) = pos(t) + vel(t)*dt + undefined_input(t) + noise - // pos = (x, y) - Matrix A(4, 4); - A(1, 1) = 1.0; A(1, 2) = 0.0; A(1, 3) = dt; A(1, 4) = 0.0; - A(2, 1) = 0.0; A(2, 2) = 1.0; A(2, 3) = 0.0; A(2, 4) = dt; - A(3, 1) = 0.0; A(3, 2) = 0.0; A(3, 3) = 1.0; A(3, 4) = 0.0; - A(4, 1) = 0.0; A(4, 2) = 0.0; A(4, 3) = 0.0; A(4, 4) = 1.0; - - // 入力は未実装 - Matrix B(4, 2); - B(1, 1) = 0.0; B(1, 2) = 0.0; - B(2, 1) = 0.0; B(2, 2) = 0.0; - B(3, 1) = 0.0; B(3, 2) = 0.0; - B(4, 1) = 0.0; B(4, 2) = 0.0; - - std::vector AB(2); - AB[0] = A; - AB[1] = B; - - auto gen_uncertainty = [&](const double max_linear_acc_mps) { - const auto max_linear_movement_in_dt = max_linear_acc_mps / 2 * std::pow(dt, 2); // [m] - const auto max_linear_acc_in_dt = max_linear_acc_mps * dt; // [m/s] - // 位置、速度の変化をシステムノイズで表現する - // システムノイズの平均値 - ColumnVector sys_noise_mu(4); - sys_noise_mu = 0.0; - - // システムノイズの分散 - SymmetricMatrix sys_noise_cov(4); - sys_noise_cov = 0.0; - sys_noise_cov(1, 1) = std::pow(max_linear_movement_in_dt, 2); - sys_noise_cov(2, 2) = std::pow(max_linear_movement_in_dt, 2); - sys_noise_cov(3, 3) = std::pow(max_linear_acc_in_dt, 2); - sys_noise_cov(4, 4) = std::pow(max_linear_acc_in_dt, 2); - return Gaussian(sys_noise_mu, sys_noise_cov); - }; - - // システムノイズを大きくして不確かさを増やす - sys_uncertain_pdf_ = std::make_shared(AB, gen_uncertainty(0.5 / dt)); - sys_uncertain_model_ = std::make_shared(sys_uncertain_pdf_.get()); - - // システムノイズを小さくして不確かさを減らす - sys_certain_pdf_ = std::make_shared(AB, gen_uncertainty(0.1 / dt)); - sys_certain_model_ = std::make_shared(sys_certain_pdf_.get()); - - // 観測モデル - // ~pos(t) = pos(t) + noise - // pos = (x, y) - Matrix H(2, 4); - H = 0.0; - H(1, 1) = 1.0; - H(2, 2) = 1.0; - - // 観測ノイズの平均値 - ColumnVector meas_noise_mu(2); - meas_noise_mu = 0.0; - - // 観測ノイズの分散 - SymmetricMatrix meas_noise_cov(2); - meas_noise_cov(1, 1) = std::pow(0.05, 2); - meas_noise_cov(2, 2) = std::pow(0.05, 2); - Gaussian measurement_uncertainty(meas_noise_mu, meas_noise_cov); - - meas_pdf_ = std::make_shared(H, measurement_uncertainty); - meas_model_ = - std::make_shared(meas_pdf_.get()); - - // 事前分布 - ColumnVector prior_mu(4); - prior_mu = 0.0; - - SymmetricMatrix prior_cov(4); - prior_cov = 0.0; - prior_cov(1, 1) = 100.0; - prior_cov(2, 2) = 100.0; - prior_cov(3, 3) = 100.0; - prior_cov(4, 4) = 100.0; - - prior_ = std::make_shared(prior_mu, prior_cov); - - // カルマンフィルタの生成 - filter_ = std::make_shared(prior_.get()); -} - -void BallTracker::push_back_observation(const DetectionBall & ball) -{ - TrackedBall observation; - observation.pos.x = ball.x * 0.001; // mm to meters - observation.pos.y = ball.y * 0.001; // mm to meters - ball_observations_.push_back(observation); -} - -TrackedBall BallTracker::update(const bool use_uncertain_sys_model) -{ - constexpr auto OUTLIER_COUNT_THRESHOLD = 10; - - // 観測値から外れ値を取り除く - for (auto it = ball_observations_.begin(); it != ball_observations_.end(); ) { - if (is_outlier(*it)) { - // 外れ値が連続できたら、観測値をそのまま使用する（誘拐対応） - outlier_count_++; - if (outlier_count_ > OUTLIER_COUNT_THRESHOLD) { - break; - } - it = ball_observations_.erase(it); - } else { - outlier_count_ = 0; - ++it; - } - } - - auto size = ball_observations_.size(); - if (size == 0) { - // 観測値が無い場合の処理 - // visibilityを下げる - // prev_tracked_ball_.visibility[0] -= VISIBILITY_CONTROL_VALUE; - prev_tracked_ball_.visibility[0] = 1.0; - if (prev_tracked_ball_.visibility[0] <= 0) { - // visibilityが0になったらカルマンフィルタの演算を実行しない - prev_tracked_ball_.visibility[0] = 0.0; - reset_prior(); - return prev_tracked_ball_; - } - - } else { - // 観測値があればvisibilityをn倍のレートで上げる - prev_tracked_ball_.visibility[0] += VISIBILITY_CONTROL_VALUE * 5.0; - if (prev_tracked_ball_.visibility[0] > 1.0) { - prev_tracked_ball_.visibility[0] = 1.0; - } - - // 観測値が複数ある場合は、その平均値をもとめる - // この処理で観測値を全て削除する - ColumnVector mean_observation(2); - mean_observation(1) = 0.0; - mean_observation(2) = 0.0; - - for (auto it = ball_observations_.begin(); it != ball_observations_.end(); ) { - mean_observation(1) += it->pos.x; - mean_observation(2) += it->pos.y; - it = ball_observations_.erase(it); - } - mean_observation(1) /= size; - mean_observation(2) /= size; - - filter_->Update(meas_model_.get(), mean_observation); - } - - // 事後分布から予測値を取得 - auto expected_value = filter_->PostGet()->ExpectedValueGet(); - // prev_tracked_ball_.pos.x = expected_value(1); - // prev_tracked_ball_.pos.y = expected_value(2); - prev_tracked_ball_.pos.x = std::clamp(expected_value(1), -FIELD_MAX_X, FIELD_MAX_X); - prev_tracked_ball_.pos.y = std::clamp(expected_value(2), -FIELD_MAX_Y, FIELD_MAX_Y); - prev_tracked_ball_.vel[0].x = expected_value(3); - prev_tracked_ball_.vel[0].y = expected_value(4); - - // 次の状態を予測する - // 例えば、ボールの加速度が入力値になる - // 入力値は未実装なので0とする - ColumnVector input(2); - input(1) = 0; - input(2) = 0; - - if (use_uncertain_sys_model) { - filter_->Update(sys_uncertain_model_.get(), input); - } else { - filter_->Update(sys_certain_model_.get(), input); - } - - return prev_tracked_ball_; -} - -void BallTracker::reset_prior() -{ - // 事前分布を初期化する - ColumnVector prior_mu(4); - prior_mu = 0.0; - - SymmetricMatrix prior_cov(4); - prior_cov = 0.0; - prior_cov(1, 1) = 100.0; - prior_cov(2, 2) = 100.0; - prior_cov(3, 3) = 100.0; - prior_cov(4, 4) = 100.0; - - prior_->ExpectedValueSet(prior_mu); - prior_->CovarianceSet(prior_cov); - filter_->Reset(prior_.get()); -} - -bool BallTracker::is_outlier(const TrackedBall & observation) const -{ - // 観測が外れ値かどうか判定する - // Reference: https://myenigma.hatenablog.com/entry/20140825/1408975706 - const double THRESHOLD = 5.99; // 自由度2、棄却率5%のしきい値 - - auto expected_value = filter_->PostGet()->ExpectedValueGet(); - auto covariance = filter_->PostGet()->CovarianceGet(); - - // マハラノビス距離を求める - double diff_x = observation.pos.x - expected_value(1); - double diff_y = observation.pos.y - expected_value(2); - double covariance_x = covariance(1, 1); - double covariance_y = covariance(2, 2); - - // 0 除算を避ける - if (std::fabs(covariance_x) < 1E-15 || std::fabs(covariance_y) < 1E-15) { - return false; - } - - double mahalanobis = std::sqrt( - std::pow(diff_x, 2) / covariance_x + std::pow( - diff_y, - 2) / covariance_y); - if (mahalanobis > THRESHOLD) { - return true; - } - - return false; -} - -} // namespace consai_vision_tracker diff --git a/consai_vision_tracker/src/robot_kalman_filter.cpp b/consai_vision_tracker/src/robot_kalman_filter.cpp index 5d60a22d..62b1bdda 100644 --- a/consai_vision_tracker/src/robot_kalman_filter.cpp +++ b/consai_vision_tracker/src/robot_kalman_filter.cpp @@ -241,82 +241,6 @@ RobotLocalVelocity RobotKalmanFilter::calc_local_velocity(void) const return local_velocity; } -Vector6d RobotKalmanFilter::predict_state(void) const -{ - // TODO: Use input data to predict state - Vector6d x_pred; - - const double x = x_(0); - const double y = x_(1); - const double theta = x_(2); - const double vx = x_(3); - const double vy = x_(4); - const double omega = x_(5); - - if (fabs(omega) > OMEGA_ZERO_) { - x_pred(0) = x + (vx / omega) * (std::sin(theta + omega * DT_) - std::sin(theta)) - + (vy / omega) * (std::cos(theta) - std::cos(theta + omega * DT_)); - x_pred(1) = y + (vx / omega) * (std::cos(theta) - std::cos(theta + omega * DT_)) - + (vy / omega) * (std::sin(theta + omega * DT_) - std::sin(theta)); - } else { - x_pred(0) = x + vx * DT_; - x_pred(1) = y + vy * DT_; - } - - x_pred(2) = normalize_angle(theta + omega * DT_); - x_pred(3) = vx; - x_pred(4) = vy; - x_pred(5) = omega; - - return x_pred; -} - -Matrix6d RobotKalmanFilter::jacobian_F(void) const -{ - // TODO: Use input data to calculate jacobian matrix - Matrix6d F = Matrix6d::Identity(); - - const double theta = x_(2); - const double vx = x_(3); - const double vy = x_(4); - const double omega = x_(5); - - if (fabs(omega) > OMEGA_ZERO_) { - const double VX_O = vx / omega; - const double VY_O = vy / omega; - const double VX_OO = vx / (omega * omega); - const double VY_OO = vy / (omega * omega); - const double INV_O = 1.0 / omega; - const double ALPHA_THETA = theta + omega * DT_; - - F(0, 2) = VX_O * (std::cos(ALPHA_THETA) - std::cos(theta)) - - VY_O * (std::sin(theta) - std::sin(ALPHA_THETA)); // d(x)/d(theta) - F(0, 3) = INV_O * (std::sin(ALPHA_THETA) - std::sin(theta)); // d(x)/d(vx) - F(0, 4) = INV_O * (std::cos(theta) - std::cos(ALPHA_THETA)); // d(x)/d(vy) - F(0, 5) = -VX_OO * (std::sin(ALPHA_THETA) - std::sin(theta)) - + VX_O * DT_ * std::cos(ALPHA_THETA) - - VY_OO * (std::cos(theta) - std::cos(ALPHA_THETA)) - + VY_O * DT_ * std::sin(ALPHA_THETA); // d(x)/d(omega); - - F(1, 2) = VX_O - (std::sin(theta) - std::sin(ALPHA_THETA)) - + VY_O * (std::cos(ALPHA_THETA) - std::cos(theta)); // d(y)/d(theta) - F(1, 3) = INV_O * (std::cos(theta) - std::cos(ALPHA_THETA)); // d(y)/d(vx) - F(1, 4) = INV_O * (std::sin(ALPHA_THETA) - std::sin(theta)); // d(y)/d(vy) - F(1, 5) = -VX_OO * (std::cos(theta) - std::cos(ALPHA_THETA)) - + VX_O * DT_ * std::sin(ALPHA_THETA) - - VY_OO * (std::sin(ALPHA_THETA) - std::sin(theta)) - + VY_O * DT_ * std::cos(ALPHA_THETA); // d(y)/d(omega); - - F(2, 5) = DT_; // d(theta)/d(omega) - } else { - F(0, 3) = DT_; - F(1, 4) = DT_; - F(2, 5) = DT_; - } - - return F; -} - Vector3d RobotKalmanFilter::make_observation(void) const { if (robot_observations_.empty()) { diff --git a/consai_vision_tracker/src/robot_tracker.cpp b/consai_vision_tracker/src/robot_tracker.cpp deleted file mode 100644 index da076900..00000000 --- a/consai_vision_tracker/src/robot_tracker.cpp +++ /dev/null @@ -1,353 +0,0 @@ -// Copyright 2021 Roots -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include -#include -#include -#include - -#include "consai_vision_tracker/robot_tracker.hpp" -#include "robocup_ssl_msgs/msg/vector2.hpp" - -namespace consai_vision_tracker -{ - -using ColumnVector = BFL::ColumnVector; -using Matrix = BFL::Matrix; -using MatrixWrapper = BFL::Matrix_Wrapper; -using SymmetricMatrix = BFL::SymmetricMatrix; -using Vector2 = robocup_ssl_msgs::msg::Vector2; - -// visibilityの操作量 -// 追跡対象の情報が来ないとき、この操作量だけvisibilityを小さくする -// 操作量 = 1.0 / (trackerの更新周波数 * 消失判定までに掛ける時間) -// 例：trackerが100Hzで更新されるとき、操作量が0.002であれば、 -// 追跡対象が消失してから5秒かけてvisibilityが1.0から0.0になる -static const double VISIBILITY_CONTROL_VALUE = 0.005; - -RobotTracker::RobotTracker(const int team_color, const int id, const double dt) -{ - prev_tracked_robot_.robot_id.team_color = team_color; - prev_tracked_robot_.robot_id.id = id; - // visibilityはoptionalなので、ここでデフォルト値を設定しておく - prev_tracked_robot_.visibility.push_back(0.0); - // velocityはoptionalなので、ここでデフォルト値を設定しておく - prev_tracked_robot_.vel.push_back(Vector2()); - prev_tracked_robot_.vel_angular.push_back(0.0); - - // システムモデル - // pos(t+1) = pos(t) + vel(t)*dt + undefined_input(t) + noise - // pos = (x, y, theta) - Matrix A(6, 6); - A(1, 1) = 1.0; A(1, 2) = 0.0; A(1, 3) = 0.0; A(1, 4) = dt; A(1, 5) = 0.0; A(1, 6) = 0.0; - A(2, 1) = 0.0; A(2, 2) = 1.0; A(2, 3) = 0.0; A(2, 4) = 0.0; A(2, 5) = dt; A(2, 6) = 0.0; - A(3, 1) = 0.0; A(3, 2) = 0.0; A(3, 3) = 1.0; A(3, 4) = 0.0; A(3, 5) = 0.0; A(3, 6) = dt; - A(4, 1) = 0.0; A(4, 2) = 0.0; A(4, 3) = 0.0; A(4, 4) = 1.0; A(4, 5) = 0.0; A(4, 6) = 0.0; - A(5, 1) = 0.0; A(5, 2) = 0.0; A(5, 3) = 0.0; A(5, 4) = 0.0; A(5, 5) = 1.0; A(5, 6) = 0.0; - A(6, 1) = 0.0; A(6, 2) = 0.0; A(6, 3) = 0.0; A(6, 4) = 0.0; A(6, 5) = 0.0; A(6, 6) = 1.0; - - // 入力は未実装 - Matrix B(6, 3); - B(1, 1) = 0.0; B(1, 2) = 0.0; B(1, 3) = 0.0; - B(2, 1) = 0.0; B(2, 2) = 0.0; B(2, 3) = 0.0; - B(3, 1) = 0.0; B(3, 2) = 0.0; B(3, 3) = 0.0; - B(4, 1) = 0.0; B(4, 2) = 0.0; B(4, 3) = 0.0; - B(5, 1) = 0.0; B(5, 2) = 0.0; B(5, 3) = 0.0; - B(6, 1) = 0.0; B(6, 2) = 0.0; B(6, 3) = 0.0; - - std::vector AB(2); - AB[0] = A; - AB[1] = B; - - // システムノイズの平均値 - ColumnVector sys_noise_mu(6); - sys_noise_mu = 0.0; - - // 位置、速度の変化をのシステムノイズで表現する（つまりめちゃくちゃノイズがでかい） - // 0 m/s から、いきなり1.0 m/sに変化しうる、ということ - // 例：1.0[m/s] / 0.001[s] = 100 [m/ss] - const double MAX_LINEAR_ACC_MPS = 0.1 * 1.0 / dt; - // 例：1.0[rad/s] / 0.001[s] = 100 [rad/ss] - const double MAX_ANGULAR_ACC_RADPS = 0.05 * M_PI / dt; - const double MAX_LINEAR_ACCEL_IN_DT = MAX_LINEAR_ACC_MPS * dt; // [m/s] - const double MAX_ANGULAR_ACCEL_IN_DT = MAX_ANGULAR_ACC_RADPS * dt; // [rad/s] - const double MAX_LINEAR_MOVEMENT_IN_DT = MAX_LINEAR_ACC_MPS / 2 * std::pow(dt, 2); // [m] - const double MAX_ANGULAR_MOVEMENT_IN_DT = MAX_ANGULAR_ACC_RADPS / 2 * std::pow(dt, 2); // [rad] - - // システムノイズの分散 - SymmetricMatrix sys_noise_cov(6); - sys_noise_cov = 0.0; - sys_noise_cov(1, 1) = std::pow(MAX_LINEAR_MOVEMENT_IN_DT, 2); - sys_noise_cov(2, 2) = std::pow(MAX_LINEAR_MOVEMENT_IN_DT, 2); - sys_noise_cov(3, 3) = std::pow(MAX_ANGULAR_MOVEMENT_IN_DT, 2); - sys_noise_cov(4, 4) = std::pow(MAX_LINEAR_ACCEL_IN_DT, 2); - sys_noise_cov(5, 5) = std::pow(MAX_LINEAR_ACCEL_IN_DT, 2); - sys_noise_cov(6, 6) = std::pow(MAX_ANGULAR_ACCEL_IN_DT, 2); - - Gaussian system_uncertainty(sys_noise_mu, sys_noise_cov); - sys_pdf_ = std::make_shared(AB, system_uncertainty); - sys_model_ = std::make_shared(sys_pdf_.get()); - - // 観測モデル - // ~pos(t) = pos(t) + noise - // pos = (x, y, theta) - Matrix H(3, 6); - H = 0.0; - H(1, 1) = 1.0; - H(2, 2) = 1.0; - H(3, 3) = 1.0; - - // 観測ノイズの平均値 - ColumnVector meas_noise_mu(3); - meas_noise_mu = 0.0; - - // 観測ノイズの分散 - SymmetricMatrix meas_noise_cov(3); - meas_noise_cov(1, 1) = std::pow(0.02, 2); - meas_noise_cov(2, 2) = std::pow(0.02, 2); - meas_noise_cov(3, 3) = std::pow(0.02 * M_PI, 2); - Gaussian measurement_uncertainty(meas_noise_mu, meas_noise_cov); - - meas_pdf_ = std::make_shared(H, measurement_uncertainty); - meas_model_ = - std::make_shared(meas_pdf_.get()); - - // 事前分布 - ColumnVector prior_mu(6); - prior_mu = 0.0; - - SymmetricMatrix prior_cov(6); - prior_cov = 0.0; - prior_cov(1, 1) = 100.0; - prior_cov(2, 2) = 100.0; - prior_cov(3, 3) = 100.0; - prior_cov(4, 4) = 100.0; - prior_cov(5, 5) = 100.0; - prior_cov(6, 6) = 100.0; - - prior_ = std::make_shared(prior_mu, prior_cov); - - // カルマンフィルタの生成 - filter_ = std::make_shared(prior_.get()); -} - -void RobotTracker::push_back_observation(const DetectionRobot & robot) -{ - // 角度データを含まない場合は、そのデータを参照しない - if (robot.orientation.size() == 0) { - return; - } - - TrackedRobot observation; - observation.pos.x = robot.x * 0.001; // mm to meters - observation.pos.y = robot.y * 0.001; // mm to meters - observation.orientation = robot.orientation[0]; - robot_observations_.push_back(observation); -} - -TrackedRobot RobotTracker::update() -{ - constexpr auto OUTLIER_COUNT_THRESHOLD = 10; - - // 観測値から外れ値を取り除く - for (auto it = robot_observations_.begin(); it != robot_observations_.end(); ) { - if (is_outlier(*it)) { - // 外れ値が連続できたら、観測値をそのまま使用する（誘拐対応） - outlier_count_++; - if (outlier_count_ > OUTLIER_COUNT_THRESHOLD) { - break; - } - it = robot_observations_.erase(it); - } else { - outlier_count_ = 0; - ++it; - } - } - - auto size = robot_observations_.size(); - if (size == 0) { - // 観測値が無い場合の処理 - // visibilityを下げる - prev_tracked_robot_.visibility[0] -= VISIBILITY_CONTROL_VALUE; - if (prev_tracked_robot_.visibility[0] <= 0) { - // visibilityが0になったらカルマンフィルタの演算を実行しない - prev_tracked_robot_.visibility[0] = 0.0; - reset_prior(); - return prev_tracked_robot_; - } - - } else { - // 観測値があればvisibilityをn倍のレートで上げる - prev_tracked_robot_.visibility[0] += VISIBILITY_CONTROL_VALUE * 5.0; - if (prev_tracked_robot_.visibility[0] > 1.0) { - prev_tracked_robot_.visibility[0] = 1.0; - } - - // 観測値が複数ある場合は、その平均値をもとめる - // この処理で観測値を全て削除する - ColumnVector mean_observation(3); - mean_observation(1) = 0.0; - mean_observation(2) = 0.0; - double sum_x = 0.0; - double sum_y = 0.0; - - for (auto it = robot_observations_.begin(); it != robot_observations_.end(); ) { - mean_observation(1) += it->pos.x; - mean_observation(2) += it->pos.y; - // 角度は-pi ~ piの範囲なので、2次元ベクトルに変換してから平均値を求める - sum_x += std::cos(it->orientation); - sum_y += std::sin(it->orientation); - it = robot_observations_.erase(it); - } - mean_observation(1) /= size; - mean_observation(2) /= size; - sum_x /= size; - sum_y /= size; - mean_observation(3) = std::fmod(std::atan2(sum_y, sum_x), M_PI); - - // 観測値と前回の予測値がpi, -pi付近にあるとき、 - // ２つの角度の差分が大きくならないように、観測値の符号と値を調節する - mean_observation(3) = normalize_orientation( - filter_->PostGet()->ExpectedValueGet()(3), mean_observation(3)); - - filter_->Update(meas_model_.get(), mean_observation); - correct_orientation_overflow_of_prior(); - } - // 事後分布から予測値を取得 - auto expected_value = filter_->PostGet()->ExpectedValueGet(); - prev_tracked_robot_.pos.x = expected_value(1); - prev_tracked_robot_.pos.y = expected_value(2); - prev_tracked_robot_.orientation = expected_value(3); - prev_tracked_robot_.vel[0].x = expected_value(4); - prev_tracked_robot_.vel[0].y = expected_value(5); - prev_tracked_robot_.vel_angular[0] = expected_value(6); - - // 次の状態を予測する - // 例えば、ロボットの加速度が入力値になる - // 入力値は未実装なので0とする - ColumnVector input(3); - input(1) = 0; - input(2) = 0; - input(3) = 0; - filter_->Update(sys_model_.get(), input); - correct_orientation_overflow_of_prior(); - - return prev_tracked_robot_; -} - -RobotLocalVelocity RobotTracker::calc_local_velocity() -{ - // ロボット座標系でのロボット速度を計算する - auto local_velocity = RobotLocalVelocity(); - local_velocity.robot_id = prev_tracked_robot_.robot_id; - - if (prev_tracked_robot_.vel.size() > 0) { - auto theta = prev_tracked_robot_.orientation; - auto world_vel = prev_tracked_robot_.vel[0]; - auto local_vel = world_vel; - local_vel.x = world_vel.x * std::cos(theta) + world_vel.y * std::sin(theta); - local_vel.y = -(world_vel.x * std::sin(theta) - world_vel.y * std::cos(theta)); - local_velocity.velocity.x = local_vel.x; - local_velocity.velocity.y = local_vel.y; - local_velocity.norm = std::sqrt(std::pow(local_vel.x, 2) + std::pow(local_vel.y, 2)); - } - if (prev_tracked_robot_.vel_angular.size() > 0) { - local_velocity.velocity.theta = prev_tracked_robot_.vel_angular[0]; - } - return local_velocity; -} - -void RobotTracker::reset_prior() -{ - // 事前分布を初期化する - ColumnVector prior_mu(6); - prior_mu = 0.0; - - SymmetricMatrix prior_cov(6); - prior_cov = 0.0; - prior_cov(1, 1) = 100.0; - prior_cov(2, 2) = 100.0; - prior_cov(3, 3) = 100.0; - prior_cov(4, 4) = 100.0; - prior_cov(5, 5) = 100.0; - prior_cov(6, 6) = 100.0; - - prior_->ExpectedValueSet(prior_mu); - prior_->CovarianceSet(prior_cov); - filter_->Reset(prior_.get()); -} - -bool RobotTracker::is_outlier(const TrackedRobot & observation) const -{ - // 観測が外れ値かどうか判定する - // Reference: https://myenigma.hatenablog.com/entry/20140825/1408975706 - const double THRESHOLD = 5.99; // 自由度2、棄却率5%のしきい値 - - auto expected_value = filter_->PostGet()->ExpectedValueGet(); - auto covariance = filter_->PostGet()->CovarianceGet(); - - // マハラノビス距離を求める - double diff_x = observation.pos.x - expected_value(1); - double diff_y = observation.pos.y - expected_value(2); - double covariance_x = covariance(1, 1); - double covariance_y = covariance(2, 2); - - // 0 除算を避ける - if (std::fabs(covariance_x) < 1E-15 || std::fabs(covariance_y) < 1E-15) { - return false; - } - - double mahalanobis = std::sqrt( - std::pow(diff_x, 2) / covariance_x + std::pow( - diff_y, - 2) / covariance_y); - if (mahalanobis > THRESHOLD) { - return true; - } - - return false; -} - -void RobotTracker::correct_orientation_overflow_of_prior() -{ - // 事後分布の角度を取得し、-pi ~ piの範囲に収め、事前分布にセットする - auto expected_value = filter_->PostGet()->ExpectedValueGet(); - auto covariance = filter_->PostGet()->CovarianceGet(); - - if (expected_value(3) < -M_PI || expected_value(3) > M_PI) { - expected_value(3) = normalize_orientation(expected_value(3)); - - prior_->ExpectedValueSet(expected_value); - prior_->CovarianceSet(covariance); - - filter_->Reset(prior_.get()); - } -} - -double RobotTracker::normalize_orientation(double orientation) const -{ - // 角度を-pi ~ piの範囲に収める - while (orientation >= M_PI) {orientation -= 2.0 * M_PI;} - while (orientation <= -M_PI) {orientation += 2.0 * M_PI;} - return orientation; -} - -double RobotTracker::normalize_orientation(const double from, const double to) const -{ - // fromからtoへ連続に角度が変化するようにtoの符号と大きさを変更する - // from(150 deg) -> to(-150 deg) => from(150 deg) -> to(210 deg) - return from + normalize_orientation(to - from); -} - -} // namespace consai_vision_tracker diff --git a/consai_vision_tracker/src/tracker_component.cpp b/consai_vision_tracker/src/tracker_component.cpp index 592abb47..b2b9e3c9 100644 --- a/consai_vision_tracker/src/tracker_component.cpp +++ b/consai_vision_tracker/src/tracker_component.cpp @@ -81,12 +81,6 @@ Tracker::Tracker(const rclcpp::NodeOptions & options) sub_geometry_ = create_subscription( "geometry", 10, std::bind(&Tracker::callback_geometry, this, _1)); -} - -void Tracker::on_timer() -{ - auto tracked_msg = std::make_unique(); - auto robot_vel_msg = std::make_unique(); // TODO: use parameter callback ball_kalman_filter_->update_noise_covariance_matrix( @@ -101,8 +95,6 @@ void Tracker::on_timer() get_parameter("robot_kalman_filter/q_max_acc_theta").get_value(), get_parameter("robot_kalman_filter/r_pos_stddev_xy").get_value(), get_parameter("robot_kalman_filter/r_pos_stddev_theta").get_value()); - tracked_msg->robots.push_back(filter->update()); - robot_vel_msg->velocities.push_back(filter->calc_local_velocity()); } // TODO: use parameter callback @@ -112,6 +104,20 @@ void Tracker::on_timer() get_parameter("robot_kalman_filter/q_max_acc_theta").get_value(), get_parameter("robot_kalman_filter/r_pos_stddev_xy").get_value(), get_parameter("robot_kalman_filter/r_pos_stddev_theta").get_value()); + } +} + +void Tracker::on_timer() +{ + auto tracked_msg = std::make_unique(); + auto robot_vel_msg = std::make_unique(); + + for (auto && filter : blue_robot_kalman_filter_) { + tracked_msg->robots.push_back(filter->update()); + robot_vel_msg->velocities.push_back(filter->calc_local_velocity()); + } + + for (auto && filter : yellow_robot_kalman_filter_) { tracked_msg->robots.push_back(filter->update()); robot_vel_msg->velocities.push_back(filter->calc_local_velocity()); } From 17c5ee8790506eae6c71c3a2a40a89b56acc5a5e Mon Sep 17 00:00:00 2001 From: ShotaAk Date: Tue, 13 Aug 2024 15:35:51 +0900 Subject: [PATCH 10/17] Update for lint --- .../ball_kalman_filter.hpp | 6 +- .../robot_kalman_filter.hpp | 6 +- .../src/ball_kalman_filter.cpp | 57 ++++----- .../src/robot_kalman_filter.cpp | 113 +++++++++--------- 4 files changed, 92 insertions(+), 90 deletions(-) diff --git a/consai_vision_tracker/include/consai_vision_tracker/ball_kalman_filter.hpp b/consai_vision_tracker/include/consai_vision_tracker/ball_kalman_filter.hpp index 8b3c27e7..268fa9e2 100644 --- a/consai_vision_tracker/include/consai_vision_tracker/ball_kalman_filter.hpp +++ b/consai_vision_tracker/include/consai_vision_tracker/ball_kalman_filter.hpp @@ -36,7 +36,7 @@ using Matrix42d = Eigen::Matrix; class BallKalmanFilter { - public: +public: explicit BallKalmanFilter( const double dt = 0.01, const double lifetime = 2.0, const double visibility_increase_rate = 5.0, @@ -48,7 +48,7 @@ class BallKalmanFilter void update_noise_covariance_matrix( const double q_max_acc, const double q_uncertain_max_acc, const double r_pos_stddev); - private: +private: Vector2d make_observation(void) const; bool is_outlier(const double chi_squared_value) const; void reset_x_and_p(const Vector2d & observation); @@ -62,7 +62,7 @@ class BallKalmanFilter int outlier_count_ = 0; std::vector ball_observations_; - + Vector4d x_; Matrix4d F_; Matrix24d H_; diff --git a/consai_vision_tracker/include/consai_vision_tracker/robot_kalman_filter.hpp b/consai_vision_tracker/include/consai_vision_tracker/robot_kalman_filter.hpp index a04f9c13..a25ba6c2 100644 --- a/consai_vision_tracker/include/consai_vision_tracker/robot_kalman_filter.hpp +++ b/consai_vision_tracker/include/consai_vision_tracker/robot_kalman_filter.hpp @@ -39,7 +39,7 @@ using Matrix36d = Eigen::Matrix; class RobotKalmanFilter { - public: +public: explicit RobotKalmanFilter( const int team_color, const int id, const double dt = 0.01, const double lifetime = 2.0, @@ -54,7 +54,7 @@ class RobotKalmanFilter const double r_pos_stddev_xy, const double r_pos_stddev_theta); RobotLocalVelocity calc_local_velocity(void) const; - private: +private: Vector3d make_observation(void) const; bool is_outlier(const double chi_squared_value) const; void reset_x_and_p(const Vector3d & observation); @@ -71,7 +71,7 @@ class RobotKalmanFilter RobotId robot_id_; std::vector robot_observations_; - + Vector6d x_; Matrix6d F_; Matrix36d H_; diff --git a/consai_vision_tracker/src/ball_kalman_filter.cpp b/consai_vision_tracker/src/ball_kalman_filter.cpp index 6bfd73aa..bb4e6621 100644 --- a/consai_vision_tracker/src/ball_kalman_filter.cpp +++ b/consai_vision_tracker/src/ball_kalman_filter.cpp @@ -25,8 +25,8 @@ using SSLVector3 = robocup_ssl_msgs::msg::Vector3; BallKalmanFilter::BallKalmanFilter( const double dt, const double lifetime, const double visibility_increase_rate, - const double outlier_time_limit) : - DT_(dt), + const double outlier_time_limit) +:DT_(dt), VISIBILITY_CONTROL_VALUE_(dt / lifetime), VISIBILITY_INCREASE_RATE_(visibility_increase_rate), OUTLIER_COUNT_THRESHOLD_(outlier_time_limit / dt) @@ -72,16 +72,16 @@ TrackedBall BallKalmanFilter::update(const bool use_uncertain_sys_model) Matrix4d P_pred; auto prediction = [&](void) { - x_pred = F_ * x_; - P_pred = F_ * P_ * F_.transpose() + Q; - }; + x_pred = F_ * x_; + P_pred = F_ * P_ * F_.transpose() + Q; + }; auto calc_chi_square = [&](const Vector2d & z) { - Matrix2d S = H_ * P_pred * H_.transpose() + R_; - Vector2d innovation = z - H_ * x_pred; - double chi_square = innovation.transpose() * S.inverse() * innovation; - return chi_square; - }; + Matrix2d S = H_ * P_pred * H_.transpose() + R_; + Vector2d innovation = z - H_ * x_pred; + double chi_square = innovation.transpose() * S.inverse() * innovation; + return chi_square; + }; // Prediction step prediction(); @@ -113,7 +113,8 @@ TrackedBall BallKalmanFilter::update(const bool use_uncertain_sys_model) visibility_ = std::max(0.0, visibility_ - VISIBILITY_CONTROL_VALUE_); } else { z = make_observation(); - visibility_ = std::min(1.0, visibility_ + VISIBILITY_INCREASE_RATE_ * VISIBILITY_CONTROL_VALUE_); + visibility_ = std::min(1.0, + visibility_ + VISIBILITY_INCREASE_RATE_ * VISIBILITY_CONTROL_VALUE_); ball_observations_.clear(); } @@ -146,32 +147,32 @@ TrackedBall BallKalmanFilter::get_estimation(void) const } void BallKalmanFilter::update_noise_covariance_matrix( - const double q_max_acc, const double q_uncertain_max_acc, const double r_pos_stddev) + const double q_max_acc, const double q_uncertain_max_acc, const double r_pos_stddev) { // Process noise covariance matrix auto gen_Q = [this](const double max_acc) { // Process noise depends on the maximum acceleration - const double sigma_vel = max_acc * DT_; - const double sigma_pos = 0.5 * max_acc * DT_ * DT_; - const double var_vel = sigma_vel * sigma_vel; - const double var_pos = sigma_pos * sigma_pos; - const double var_vel_pos = sigma_vel * sigma_pos; - - Matrix4d Q; - Q << var_pos, 0.0, var_vel_pos, 0.0, - 0.0, var_pos, 0.0, var_vel_pos, - var_vel_pos, 0.0, var_vel, 0.0, - 0.0, var_vel_pos, 0.0, var_vel; - return Q; - }; + const double sigma_vel = max_acc * DT_; + const double sigma_pos = 0.5 * max_acc * DT_ * DT_; + const double var_vel = sigma_vel * sigma_vel; + const double var_pos = sigma_pos * sigma_pos; + const double var_vel_pos = sigma_vel * sigma_pos; + + Matrix4d Q; + Q << var_pos, 0.0, var_vel_pos, 0.0, + 0.0, var_pos, 0.0, var_vel_pos, + var_vel_pos, 0.0, var_vel, 0.0, + 0.0, var_vel_pos, 0.0, var_vel; + return Q; + }; Q_ = gen_Q(q_max_acc); // m/s^2 Q_uncertain_ = gen_Q(q_uncertain_max_acc); // m/s^2 // Observation noise covariance matrix auto gen_R = [&](const double std_dev) { - const double var = std_dev * std_dev; - return Eigen::DiagonalMatrix(var, var); - }; + const double var = std_dev * std_dev; + return Eigen::DiagonalMatrix(var, var); + }; R_ = gen_R(r_pos_stddev); // meters } diff --git a/consai_vision_tracker/src/robot_kalman_filter.cpp b/consai_vision_tracker/src/robot_kalman_filter.cpp index 62b1bdda..800d4960 100644 --- a/consai_vision_tracker/src/robot_kalman_filter.cpp +++ b/consai_vision_tracker/src/robot_kalman_filter.cpp @@ -29,8 +29,8 @@ RobotKalmanFilter::RobotKalmanFilter( const int team_color, const int id, const double dt, const double lifetime, const double visibility_increase_rate, - const double outlier_time_limit) : - DT_(dt), + const double outlier_time_limit) +:DT_(dt), VISIBILITY_CONTROL_VALUE_(dt / lifetime), VISIBILITY_INCREASE_RATE_(visibility_increase_rate), OUTLIER_COUNT_THRESHOLD_(outlier_time_limit / dt) @@ -57,7 +57,7 @@ RobotKalmanFilter::RobotKalmanFilter( 30.0, // theta rad/s^2 0.05, // xy m 0.05 // theta rad - ); + ); // State vector // x_ = (x, y, theta, vx, vy, vtheta) @@ -85,28 +85,28 @@ TrackedRobot RobotKalmanFilter::update(void) Matrix6d P_pred; auto prediction = [&, this](void) { - x_pred = F_ * x_; - x_pred(2) = normalize_angle(x_pred(2)); - P_pred = F_ * P_ * F_.transpose() + Q_; - }; + x_pred = F_ * x_; + x_pred(2) = normalize_angle(x_pred(2)); + P_pred = F_ * P_ * F_.transpose() + Q_; + }; auto calc_innovation = [&](const Vector3d & z) { - Vector3d innovation = z - H_ * x_pred; - innovation(2) = normalize_angle(z(2) - (H_ * x_pred)(2)); - return innovation; - }; + Vector3d innovation = z - H_ * x_pred; + innovation(2) = normalize_angle(z(2) - (H_ * x_pred)(2)); + return innovation; + }; auto calc_chi_square = [&](const Vector3d & z) { - Matrix3d S = H_ * P_pred * H_.transpose() + R_; - Vector3d innovation = calc_innovation(z); + Matrix3d S = H_ * P_pred * H_.transpose() + R_; + Vector3d innovation = calc_innovation(z); // Use x and y only - Matrix2d S_xy = S.block<2, 2>(0, 0); - Vector2d innovation_xy = innovation.block<2, 1>(0, 0); + Matrix2d S_xy = S.block<2, 2>(0, 0); + Vector2d innovation_xy = innovation.block<2, 1>(0, 0); - double chi_square = innovation_xy.transpose() * S_xy.inverse() * innovation_xy; - return chi_square; - }; + double chi_square = innovation_xy.transpose() * S_xy.inverse() * innovation_xy; + return chi_square; + }; // Prediction step prediction(); @@ -138,7 +138,8 @@ TrackedRobot RobotKalmanFilter::update(void) visibility_ = std::max(0.0, visibility_ - VISIBILITY_CONTROL_VALUE_); } else { z = make_observation(); - visibility_ = std::min(1.0, visibility_ + VISIBILITY_INCREASE_RATE_ * VISIBILITY_CONTROL_VALUE_); + visibility_ = std::min(1.0, + visibility_ + VISIBILITY_INCREASE_RATE_ * VISIBILITY_CONTROL_VALUE_); robot_observations_.clear(); } @@ -175,49 +176,49 @@ TrackedRobot RobotKalmanFilter::get_estimation(void) const } void RobotKalmanFilter::update_noise_covariance_matrix( - const double q_max_acc_xy, const double q_max_acc_theta, - const double r_pos_stddev_xy, const double r_pos_stddev_theta) + const double q_max_acc_xy, const double q_max_acc_theta, + const double r_pos_stddev_xy, const double r_pos_stddev_theta) { // Process noise covariance matrix auto gen_Q = [this](const double q_max_acc_xy, const double q_max_acc_theta) { // Process noise depends on the maximum acceleration - const double sigma_vel_xy = q_max_acc_xy * DT_; - const double sigma_vel_theta = q_max_acc_theta * DT_; - const double sigma_pos_xy = 0.5 * q_max_acc_xy * DT_ * DT_; - const double sigma_pos_theta = 0.5 * q_max_acc_theta * DT_ * DT_; - - const double var_vel_xy = std::pow(sigma_vel_xy, 2); - const double var_vel_theta = std::pow(sigma_vel_theta, 2); - const double var_pos_xy = std::pow(sigma_pos_xy, 2); - const double var_pos_theta = std::pow(sigma_pos_theta, 2); - - const double sigma_vel_pos_xy = sigma_vel_xy * sigma_pos_xy; - const double sigma_vel_pos_theta = sigma_vel_theta * sigma_pos_theta; - - Matrix6d Q = Matrix6d::Zero(); - Q(0, 0) = var_pos_xy; - Q(1, 1) = var_pos_xy; - Q(2, 2) = var_pos_theta; - Q(3, 3) = var_vel_xy; - Q(4, 4) = var_vel_xy; - Q(5, 5) = var_vel_theta; - - Q(0, 3) = sigma_vel_pos_xy; - Q(1, 4) = sigma_vel_pos_xy; - Q(2, 5) = sigma_vel_pos_theta; - Q(3, 0) = sigma_vel_pos_xy; - Q(4, 1) = sigma_vel_pos_xy; - Q(5, 2) = sigma_vel_pos_theta; - return Q; - }; + const double sigma_vel_xy = q_max_acc_xy * DT_; + const double sigma_vel_theta = q_max_acc_theta * DT_; + const double sigma_pos_xy = 0.5 * q_max_acc_xy * DT_ * DT_; + const double sigma_pos_theta = 0.5 * q_max_acc_theta * DT_ * DT_; + + const double var_vel_xy = std::pow(sigma_vel_xy, 2); + const double var_vel_theta = std::pow(sigma_vel_theta, 2); + const double var_pos_xy = std::pow(sigma_pos_xy, 2); + const double var_pos_theta = std::pow(sigma_pos_theta, 2); + + const double sigma_vel_pos_xy = sigma_vel_xy * sigma_pos_xy; + const double sigma_vel_pos_theta = sigma_vel_theta * sigma_pos_theta; + + Matrix6d Q = Matrix6d::Zero(); + Q(0, 0) = var_pos_xy; + Q(1, 1) = var_pos_xy; + Q(2, 2) = var_pos_theta; + Q(3, 3) = var_vel_xy; + Q(4, 4) = var_vel_xy; + Q(5, 5) = var_vel_theta; + + Q(0, 3) = sigma_vel_pos_xy; + Q(1, 4) = sigma_vel_pos_xy; + Q(2, 5) = sigma_vel_pos_theta; + Q(3, 0) = sigma_vel_pos_xy; + Q(4, 1) = sigma_vel_pos_xy; + Q(5, 2) = sigma_vel_pos_theta; + return Q; + }; Q_ = gen_Q(q_max_acc_xy, q_max_acc_theta); // Observation noise covariance matrix auto gen_R = [&](const double std_dev_xy, const double std_dev_theta) { - const double var_xy = std_dev_xy * std_dev_xy; - const double var_theta = std_dev_theta * std_dev_theta; - return Eigen::DiagonalMatrix(var_xy, var_xy, var_theta); - }; + const double var_xy = std_dev_xy * std_dev_xy; + const double var_theta = std_dev_theta * std_dev_theta; + return Eigen::DiagonalMatrix(var_xy, var_xy, var_theta); + }; R_ = gen_R(r_pos_stddev_xy, r_pos_stddev_theta); } @@ -285,8 +286,8 @@ double RobotKalmanFilter::normalize_angle(const double angle) const { // Normalize angle to -pi ~ pi double result = angle; - while (result > M_PI) result -= 2.0 * M_PI; - while (result < -M_PI) result += 2.0 * M_PI; + while (result > M_PI) {result -= 2.0 * M_PI;} + while (result < -M_PI) {result += 2.0 * M_PI;} return result; } From 3ce8ee41e94d0b21ae39518ac2da7f1956c1c9db Mon Sep 17 00:00:00 2001 From: ShotaAk Date: Wed, 14 Aug 2024 23:27:28 +0900 Subject: [PATCH 11/17] Fix workflow --- .github/workflows/scenario_test.yaml | 10 +++++++--- 1 file changed, 7 insertions(+), 3 deletions(-) diff --git a/.github/workflows/scenario_test.yaml b/.github/workflows/scenario_test.yaml index 141365e2..49d3b5ca 100644 --- a/.github/workflows/scenario_test.yaml +++ b/.github/workflows/scenario_test.yaml @@ -126,7 +126,11 @@ jobs: run: | source env/bin/activate pytest ${{ matrix.env.TEST_NAME }} --vision_port=10020 --logging --log_recorder=./ssl-log-recorder - echo "LOG_EXISTS=$(ls *.log.gz 2> /dev/null)" >> $GITHUB_ENV + if ls *.log.gz 1> /dev/null 2>&1; then + echo "LOG_EXISTS=true" >> $GITHUB_ENV + else + echo "LOG_EXISTS=false" >> $GITHUB_ENV + fi - name: Get and print logs if: always() @@ -138,14 +142,14 @@ jobs: - name: Upload artifacts id: upload-artifact - if: always() && env.LOG_EXISTS != '' + if: always() && env.LOG_EXISTS == 'true' uses: actions/upload-artifact@v4 with: name: ${{ matrix.env.TEST_NAME}}-failure-logs path: '*.log.gz' - name: Comment PR with Artifact URL - if: always() && env.LOG_EXISTS != '' + if: always() && env.LOG_EXISTS == 'true' uses: peter-evans/create-or-update-comment@v4 with: issue-number: ${{ github.event.pull_request.number }} From 545f3492abda93e2d82ff22e5c8b9a56822651ca Mon Sep 17 00:00:00 2001 From: ShotaAk Date: Thu, 15 Aug 2024 09:01:18 +0900 Subject: [PATCH 12/17] Update CI --- .github/workflows/scenario_test.yaml | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/.github/workflows/scenario_test.yaml b/.github/workflows/scenario_test.yaml index 49d3b5ca..30d33a10 100644 --- a/.github/workflows/scenario_test.yaml +++ b/.github/workflows/scenario_test.yaml @@ -126,6 +126,10 @@ jobs: run: | source env/bin/activate pytest ${{ matrix.env.TEST_NAME }} --vision_port=10020 --logging --log_recorder=./ssl-log-recorder + + - name: Check for log files + if: always() + run: | if ls *.log.gz 1> /dev/null 2>&1; then echo "LOG_EXISTS=true" >> $GITHUB_ENV else From a1cb77d11f733fbba207d1b429617075a69196a1 Mon Sep 17 00:00:00 2001 From: ShotaAk Date: Thu, 15 Aug 2024 09:14:25 +0900 Subject: [PATCH 13/17] Fix CI again --- .github/workflows/scenario_test.yaml | 21 +++++++++++---------- 1 file changed, 11 insertions(+), 10 deletions(-) diff --git a/.github/workflows/scenario_test.yaml b/.github/workflows/scenario_test.yaml index 30d33a10..76cf4976 100644 --- a/.github/workflows/scenario_test.yaml +++ b/.github/workflows/scenario_test.yaml @@ -61,16 +61,16 @@ jobs: fail-fast: false matrix: env: - - {CONSAI_YELLOW: "false", CONSAI_INVERT: "false", TEST_NAME: "tests/test_scenario_ball_placement.py"} - - {CONSAI_YELLOW: "false", CONSAI_INVERT: "false", TEST_NAME: "tests/test_scenario_force_start.py"} - - {CONSAI_YELLOW: "false", CONSAI_INVERT: "false", TEST_NAME: "tests/test_scenario_free_kick.py"} - - {CONSAI_YELLOW: "false", CONSAI_INVERT: "false", TEST_NAME: "tests/test_scenario_halt.py"} - - {CONSAI_YELLOW: "false", CONSAI_INVERT: "false", TEST_NAME: "tests/test_scenario_kickoff.py"} - - {CONSAI_YELLOW: "false", CONSAI_INVERT: "false", TEST_NAME: "tests/test_scenario_obstacle_avoidance.py"} - - {CONSAI_YELLOW: "false", CONSAI_INVERT: "false", TEST_NAME: "tests/test_scenario_penalty.py"} - - {CONSAI_YELLOW: "false", CONSAI_INVERT: "false", TEST_NAME: "tests/test_scenario_stop.py"} - - {CONSAI_YELLOW: "false", CONSAI_INVERT: "false", TEST_NAME: "tests/test_scenario_timeout.py"} - - {CONSAI_YELLOW: "true", CONSAI_INVERT: "true", TEST_NAME: "tests/yellow_invert/test_scenario_yellow_invert_kickoff.py"} + - {CONSAI_YELLOW: "false", CONSAI_INVERT: "false", TEST_NAME: "test_scenario_ball_placement.py"} + - {CONSAI_YELLOW: "false", CONSAI_INVERT: "false", TEST_NAME: "test_scenario_force_start.py"} + - {CONSAI_YELLOW: "false", CONSAI_INVERT: "false", TEST_NAME: "test_scenario_free_kick.py"} + - {CONSAI_YELLOW: "false", CONSAI_INVERT: "false", TEST_NAME: "test_scenario_halt.py"} + - {CONSAI_YELLOW: "false", CONSAI_INVERT: "false", TEST_NAME: "test_scenario_kickoff.py"} + - {CONSAI_YELLOW: "false", CONSAI_INVERT: "false", TEST_NAME: "test_scenario_obstacle_avoidance.py"} + - {CONSAI_YELLOW: "false", CONSAI_INVERT: "false", TEST_NAME: "test_scenario_penalty.py"} + - {CONSAI_YELLOW: "false", CONSAI_INVERT: "false", TEST_NAME: "test_scenario_stop.py"} + - {CONSAI_YELLOW: "false", CONSAI_INVERT: "false", TEST_NAME: "test_scenario_timeout.py"} + - {CONSAI_YELLOW: "true", CONSAI_INVERT: "true", TEST_NAME: "yellow_invert/test_scenario_yellow_invert_kickoff.py"} runs-on: ubuntu-latest @@ -125,6 +125,7 @@ jobs: - name: Run pytest run: | source env/bin/activate + cd tests pytest ${{ matrix.env.TEST_NAME }} --vision_port=10020 --logging --log_recorder=./ssl-log-recorder - name: Check for log files From e03e795f4989679032a87425e0ac9219dde61892 Mon Sep 17 00:00:00 2001 From: ShotaAk Date: Thu, 15 Aug 2024 09:32:30 +0900 Subject: [PATCH 14/17] Fix tests dir of CI --- .github/workflows/scenario_test.yaml | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/.github/workflows/scenario_test.yaml b/.github/workflows/scenario_test.yaml index 76cf4976..63d88f73 100644 --- a/.github/workflows/scenario_test.yaml +++ b/.github/workflows/scenario_test.yaml @@ -119,6 +119,7 @@ jobs: - name: Download logger run: | + cd tests curl -L https://github.com/RoboCup-SSL/ssl-go-tools/releases/download/v1.5.2/ssl-log-recorder_v1.5.2_linux_amd64 -o ssl-log-recorder chmod +x ssl-log-recorder @@ -131,7 +132,7 @@ jobs: - name: Check for log files if: always() run: | - if ls *.log.gz 1> /dev/null 2>&1; then + if ls tests/*.log.gz 1> /dev/null 2>&1; then echo "LOG_EXISTS=true" >> $GITHUB_ENV else echo "LOG_EXISTS=false" >> $GITHUB_ENV @@ -151,7 +152,7 @@ jobs: uses: actions/upload-artifact@v4 with: name: ${{ matrix.env.TEST_NAME}}-failure-logs - path: '*.log.gz' + path: 'tests/*.log.gz' - name: Comment PR with Artifact URL if: always() && env.LOG_EXISTS == 'true' From 535772bcebeb126d54e50a628ebdfba183f7cb30 Mon Sep 17 00:00:00 2001 From: ShotaAk Date: Thu, 15 Aug 2024 09:40:17 +0900 Subject: [PATCH 15/17] Use O3 option for build --- consai_robot_controller/CMakeLists.txt | 2 ++ consai_vision_tracker/CMakeLists.txt | 2 ++ 2 files changed, 4 insertions(+) diff --git a/consai_robot_controller/CMakeLists.txt b/consai_robot_controller/CMakeLists.txt index ca5706be..553d9fa9 100644 --- a/consai_robot_controller/CMakeLists.txt +++ b/consai_robot_controller/CMakeLists.txt @@ -15,6 +15,8 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wall -Wextra -Wpedantic) endif() +set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -O3 -march=native -mtune=native") + # find dependencies find_package(ament_cmake REQUIRED) find_package(consai_frootspi_msgs REQUIRED) diff --git a/consai_vision_tracker/CMakeLists.txt b/consai_vision_tracker/CMakeLists.txt index e472b717..81397291 100644 --- a/consai_vision_tracker/CMakeLists.txt +++ b/consai_vision_tracker/CMakeLists.txt @@ -15,6 +15,8 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wall -Wextra -Wpedantic) endif() +set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -O3 -march=native -mtune=native") + # find dependencies find_package(ament_cmake REQUIRED) find_package(consai_msgs REQUIRED) From 9d3b2acddbdc3dd6d7da71d6d82750d30e3dbf40 Mon Sep 17 00:00:00 2001 From: ShotaAk Date: Sat, 9 Aug 2025 15:47:59 +0900 Subject: [PATCH 16/17] Update flake8 configuration --- hooks/run_ament_flake8.sh | 5 ++++- setup.cfg | 2 +- 2 files changed, 5 insertions(+), 2 deletions(-) diff --git a/hooks/run_ament_flake8.sh b/hooks/run_ament_flake8.sh index cc571e07..9163666d 100755 --- a/hooks/run_ament_flake8.sh +++ b/hooks/run_ament_flake8.sh @@ -4,6 +4,10 @@ set -e # プロジェクトルートへ移動（どこから実行されても .cfg が見えるように） cd "$(git rev-parse --show-toplevel)" +# 現在の設定を表示 +echo "Current configuration:" +cat setup.cfg + # ステージ済みの Python ファイルのみ取得 FILES=$(git diff --cached --name-only --diff-filter=ACM | grep '\.py$' || true) @@ -15,5 +19,4 @@ fi echo "[ament_flake8] Running on files:" echo "$FILES" -# 設定ファイル (setup.cfg or tox.ini) を自動検出し、flake8 を実行 ament_flake8 --config setup.cfg $FILES diff --git a/setup.cfg b/setup.cfg index cf3096d5..5974d70a 100644 --- a/setup.cfg +++ b/setup.cfg @@ -1,3 +1,3 @@ [flake8] max-line-length = 120 -ignore = D100, D400, D104, D202, D403, I100, Q000, W503 \ No newline at end of file +extend-ignore = D100, D400, D104, D202, D403, I100, Q000, W503, I201 \ No newline at end of file From b47d91b68af1f772f2a1b11babfaffef260b7564 Mon Sep 17 00:00:00 2001 From: ShotaAk Date: Sat, 9 Aug 2025 15:48:09 +0900 Subject: [PATCH 17/17] Fix dataclass --- .../consai_game/world_model/referee_model.py | 6 ++-- .../consai_game/world_model/world_model.py | 32 ++++++++++++------- 2 files changed, 23 insertions(+), 15 deletions(-) diff --git a/consai_game/consai_game/world_model/referee_model.py b/consai_game/consai_game/world_model/referee_model.py index 9b61668c..aa851567 100644 --- a/consai_game/consai_game/world_model/referee_model.py +++ b/consai_game/consai_game/world_model/referee_model.py @@ -17,7 +17,7 @@ """Refereeメッセージを解析し, ゲームの状態を抽象化したモデルに変換するモジュール.""" -from dataclasses import dataclass +from dataclasses import dataclass, field from robocup_ssl_msgs.msg import Referee from consai_game.utils.geometry import Point @@ -49,9 +49,9 @@ class RefereeModel: our_ball_placement: bool = False their_ball_placement: bool = False running: bool = False - placement_pos: Point = Point(0.0, 0.0) + placement_pos: Point = field(default_factory=lambda: Point(0.0, 0.0)) - stop_game_ball_pos: State2D = State2D(x=0.0, y=0.0) + stop_game_ball_pos: State2D = field(default_factory=lambda: State2D(x=0.0, y=0.0)) def is_position_on_placement_area(self, pos: State2D) -> bool: """指定座標がプレースメントエリアにあるかを判定する関数.""" diff --git a/consai_game/consai_game/world_model/world_model.py b/consai_game/consai_game/world_model/world_model.py index d98519f7..289be1be 100644 --- a/consai_game/consai_game/world_model/world_model.py +++ b/consai_game/consai_game/world_model/world_model.py @@ -18,6 +18,7 @@ """試合状況を統合的に表現するWorldModelの定義モジュール.""" from dataclasses import dataclass +from dataclasses import field as dataclass_field from consai_game.world_model.ball_activity_model import BallActivityModel from consai_game.world_model.ball_model import BallModel @@ -36,15 +37,22 @@ class WorldModel: """試合全体の状態を統合的に保持するデータクラス.""" - referee: RefereeModel = RefereeModel() - robots: RobotsModel = RobotsModel() - ball: BallModel = BallModel() - field: Field = Field() - field_points: FieldPoints = FieldPoints.create_field_points(field) - ball_position: BallPositionModel = BallPositionModel(field, field_points) - robot_activity: RobotActivityModel = RobotActivityModel() - ball_activity: BallActivityModel = BallActivityModel() - kick_target: KickTargetModel = KickTargetModel() - game_config: GameConfigModel = GameConfigModel() - threats: ThreatsModel = ThreatsModel(field, field_points) - meta: WorldMetaModel = WorldMetaModel() + referee: RefereeModel = dataclass_field(default_factory=RefereeModel) + robots: RobotsModel = dataclass_field(default_factory=RobotsModel) + ball: BallModel = dataclass_field(default_factory=BallModel) + field: Field = dataclass_field(default_factory=Field) + robot_activity: RobotActivityModel = dataclass_field(default_factory=RobotActivityModel) + ball_activity: BallActivityModel = dataclass_field(default_factory=BallActivityModel) + kick_target: KickTargetModel = dataclass_field(default_factory=KickTargetModel) + game_config: GameConfigModel = dataclass_field(default_factory=GameConfigModel) + meta: WorldMetaModel = dataclass_field(default_factory=WorldMetaModel) + + field_points: FieldPoints = dataclass_field(init=False) + ball_position: BallPositionModel = dataclass_field(init=False) + threats: ThreatsModel = dataclass_field(init=False) + + def __post_init__(self): + """クラスメンバを使って初期化を行う""" + self.field_points = FieldPoints.create_field_points(self.field) + self.ball_position = BallPositionModel(self.field, self.field_points) + self.threats = ThreatsModel(self.field, self.field_points)