From 40fafa1dc410be07164b851d01d327f08c0dc8a0 Mon Sep 17 00:00:00 2001 From: heyeuu <2829004293@qq.com> Date: Mon, 1 Sep 2025 14:37:02 +0800 Subject: [PATCH 01/53] refactor(utils): rename transform.hpp to coordinate.hpp --- src/tongji/solver/solver.cpp | 16 ++++++++-------- src/util/{transform.hpp => coordinate.hpp} | 2 +- 2 files changed, 9 insertions(+), 9 deletions(-) rename src/util/{transform.hpp => coordinate.hpp} (96%) diff --git a/src/tongji/solver/solver.cpp b/src/tongji/solver/solver.cpp index 48665c5..49496e5 100644 --- a/src/tongji/solver/solver.cpp +++ b/src/tongji/solver/solver.cpp @@ -16,9 +16,9 @@ #include "interfaces/time_stamped.hpp" #include "parameters/profile.hpp" #include "parameters/rm_parameters.hpp" +#include "util/coordinate.hpp" #include "util/index.hpp" #include "util/math.hpp" -#include "util/transform.hpp" namespace world_exe::tongji::solver { class ArmorInCameraImpl final : public interfaces::IArmorInCamera, public interfaces::ITimeStamped { @@ -66,7 +66,7 @@ class Solver::Impl { result->SetTimeStamp(armors_in_image->GetTimeStamped().GetTimeStamp()); for (int i = 0; i < static_cast(enumeration::ArmorIdFlag::Count); ++i) { - const auto armor_id = static_cast(1 << i); + const auto armor_id = static_cast(1 << i); const auto& image_armors = armors_in_image->GetArmors(armor_id); for (const auto& armor_image : image_armors) { @@ -103,10 +103,10 @@ class Solver::Impl { Eigen::Quaterniond orientation_in_camera(R_armor2camera); world_exe::data::ArmorCameraSpacing armor_in_camera; - auto orientation_ros = util::transform::opencv2ros_orientation(orientation_in_camera); + auto orientation_ros = util::coordinate::opencv2ros_orientation(orientation_in_camera); orientation_ros.normalize(); armor_in_camera.id = armor_in_image.id; - armor_in_camera.position = util::transform::opencv2ros_position(xyz_in_camera); + armor_in_camera.position = util::coordinate::opencv2ros_position(xyz_in_camera); armor_in_camera.orientation = orientation_ros; if (armor_in_camera.position.norm() > MaxArmorDistance) { @@ -120,13 +120,13 @@ class Solver::Impl { const world_exe::data::ArmorCameraSpacing& armor_in_camera) const { Eigen::Vector3d armor_in_camera_ocv = - util::transform::ros2opencv_position(armor_in_camera.position); + util::coordinate::ros2opencv_position(armor_in_camera.position); Eigen::Vector3d armor_xyz_in_gimbal = R_camera2gimbal_ * armor_in_camera_ocv + t_camera2gimbal_; Eigen::Vector3d armor_xyz_in_world = R_gimbal2world_ * armor_xyz_in_gimbal; // why no t? Eigen::Quaterniond armor_orientation_in_camera_ocv = - util::transform::ros2opencv_orientation(armor_in_camera.orientation); + util::coordinate::ros2opencv_orientation(armor_in_camera.orientation); Eigen::Matrix3d R_armor2camera_ocv = armor_orientation_in_camera_ocv.toRotationMatrix(); Eigen::Matrix3d R_armor2gimbal = R_camera2gimbal_ * R_armor2camera_ocv; Eigen::Matrix3d R_armor2world_initial = R_gimbal2world_ * R_armor2gimbal; @@ -216,8 +216,8 @@ class Solver::Impl { const Eigen::Vector3d& armor_xyz_in_world, const double& yaw, const double& inclined) const { - auto image_points = ReprojectArmor( - armor_xyz_in_world, yaw, armor_in_image.isLargeArmor, armor_in_image.id); + auto image_points = + ReprojectArmor(armor_xyz_in_world, yaw, armor_in_image.isLargeArmor, armor_in_image.id); auto error = 0.0; for (int i = 0; i < 4; i++) error += cv::norm(armor_in_image.image_points[i] - image_points[i]); diff --git a/src/util/transform.hpp b/src/util/coordinate.hpp similarity index 96% rename from src/util/transform.hpp rename to src/util/coordinate.hpp index 21c0dce..a0c6e96 100644 --- a/src/util/transform.hpp +++ b/src/util/coordinate.hpp @@ -2,7 +2,7 @@ #include -namespace world_exe::util::transform { +namespace world_exe::util::coordinate { static inline Eigen::Vector3d opencv2ros_position(const Eigen::Vector3d& position) { return Eigen::Vector3d(position.z(), -position.x(), -position.y()); From 83751c5657fb5221d9a21f88c984a965481151ac Mon Sep 17 00:00:00 2001 From: heyeuu <2829004293@qq.com> Date: Tue, 2 Sep 2025 19:02:04 +0800 Subject: [PATCH 02/53] feat(classifier): add classifier - Return armor_id from input armor_pattern --- src/tongji/identifier/classifier.cpp | 160 +++++++++++++++++++++++++++ src/tongji/identifier/classifier.hpp | 16 +++ src/tongji/solver/solver.cpp | 1 - 3 files changed, 176 insertions(+), 1 deletion(-) create mode 100644 src/tongji/identifier/classifier.cpp create mode 100644 src/tongji/identifier/classifier.hpp diff --git a/src/tongji/identifier/classifier.cpp b/src/tongji/identifier/classifier.cpp new file mode 100644 index 0000000..f538a0b --- /dev/null +++ b/src/tongji/identifier/classifier.cpp @@ -0,0 +1,160 @@ +#include "classifier.hpp" +#include "enum/armor_id.hpp" + +#include +#include +#include +#include +#include + +namespace world_exe::tongji::identifier { +class Classifier::Impl final { +public: + explicit Impl(const std::string& model_path, int model_image_width, int model_image_height) + : model_image_width_(model_image_width) + , model_image_height_(model_image_height) { + + net_ = cv::dnn::readNetFromONNX(model_path); + auto ovmodel = core_.read_model(model_path); + compiled_model_ = core_.compile_model( + ovmodel, "AUTO", ov::hint::performance_mode(ov::hint::PerformanceMode::LATENCY)); + } + + enumeration::ArmorIdFlag classify(const cv::Mat& armor_pattern) { + if (armor_pattern.empty()) { + return enumeration::ArmorIdFlag::Unknow; + } + + cv::Mat gray; + cv::cvtColor(armor_pattern, gray, cv::COLOR_BGR2GRAY); + + auto input = cv::Mat(model_image_height_, model_image_width_, CV_8UC1, cv::Scalar(0)); + auto x_scale = static_cast(model_image_width_) / gray.cols; + auto y_scale = static_cast(model_image_height_) / gray.rows; + auto scale = std::min(x_scale, y_scale); + auto h = static_cast(gray.rows * scale); + auto w = static_cast(gray.cols * scale); + + if (h == 0 || w == 0) { + return enumeration::ArmorIdFlag::Unknow; + } + auto roi = cv::Rect(0, 0, w, h); + cv::resize(gray, input(roi), { w, h }); + + auto blob = cv::dnn::blobFromImage(input, 1.0 / 255.0, cv::Size(), cv::Scalar()); + + net_.setInput(blob); + cv::Mat outputs = net_.forward(); + + // softmax + float max = *std::max_element(outputs.begin(), outputs.end()); + cv::exp(outputs - max, outputs); + float sum = cv::sum(outputs)[0]; + outputs /= sum; + + double confidence; + cv::Point label_point; + cv::minMaxLoc(outputs.reshape(1, 1), nullptr, &confidence, nullptr, &label_point); + int label_id = label_point.x; + + enumeration::ArmorIdFlag armor_id; + + if (label_id == 3) { + armor_id = enumeration::ArmorIdFlag::InfantryIII; + } else if (label_id == 4) { + armor_id = enumeration::ArmorIdFlag::InfantryIV; + } else if (label_id == 5) { + armor_id = enumeration::ArmorIdFlag::InfantryV; + } else if (label_id == 6) { + armor_id = enumeration::ArmorIdFlag::Outpost; + } else if (label_id == 1) { + armor_id = enumeration::ArmorIdFlag::Hero; + } else if (label_id == 2) { + armor_id = enumeration::ArmorIdFlag::Engineer; + } else if (label_id == 0) { + armor_id = enumeration::ArmorIdFlag::Sentry; + } else if (label_id == 7) { + armor_id = enumeration::ArmorIdFlag::Base; + }; + + return armor_id; + } + + enumeration::ArmorIdFlag ovclassify(const cv::Mat& armor_pattern) { + + if (armor_pattern.empty()) { + return enumeration::ArmorIdFlag::Unknow; + } + + cv::Mat gray; + cv::cvtColor(armor_pattern, gray, cv::COLOR_BGR2GRAY); + + // Resize image + auto input = cv::Mat(model_image_height_, model_image_width_, CV_8UC1, cv::Scalar(0)); + auto x_scale = static_cast(model_image_width_) / gray.cols; + auto y_scale = static_cast(model_image_height_) / gray.rows; + auto scale = std::min(x_scale, y_scale); + auto h = static_cast(gray.rows * scale); + auto w = static_cast(gray.cols * scale); + + if (h == 0 || w == 0) { + return enumeration::ArmorIdFlag::Unknow; + } + + auto roi = cv::Rect(0, 0, w, h); + cv::resize(gray, input(roi), { w, h }); + // Normalize the input image to [0, 1] range + input.convertTo(input, CV_32F, 1.0 / 255.0); + + ov::Tensor input_tensor(ov::element::f32, { 1, 1, 32, 32 }, input.data); + + ov::InferRequest infer_request = compiled_model_.create_infer_request(); + infer_request.set_input_tensor(input_tensor); + infer_request.infer(); + + auto output_tensor = infer_request.get_output_tensor(); + auto output_shape = output_tensor.get_shape(); + cv::Mat outputs(1, 9, CV_32F, output_tensor.data()); + + // Softmax + float max = *std::max_element(outputs.begin(), outputs.end()); + cv::exp(outputs - max, outputs); + float sum = cv::sum(outputs)[0]; + outputs /= sum; + + double confidence; + cv::Point label_point; + cv::minMaxLoc(outputs.reshape(1, 1), nullptr, &confidence, nullptr, &label_point); + int label_id = label_point.x; + enumeration::ArmorIdFlag armor_id; + + if (label_id == 3) { + armor_id = enumeration::ArmorIdFlag::InfantryIII; + } else if (label_id == 4) { + armor_id = enumeration::ArmorIdFlag::InfantryIV; + } else if (label_id == 5) { + armor_id = enumeration::ArmorIdFlag::InfantryV; + } else if (label_id == 6) { + armor_id = enumeration::ArmorIdFlag::Outpost; + } else if (label_id == 1) { + armor_id = enumeration::ArmorIdFlag::Hero; + } else if (label_id == 2) { + armor_id = enumeration::ArmorIdFlag::Engineer; + } else if (label_id == 0) { + armor_id = enumeration::ArmorIdFlag::Sentry; + } else if (label_id == 7) { + armor_id = enumeration::ArmorIdFlag::Base; + }; + + return armor_id; + }; + +private: + cv::dnn::Net net_; + ov::Core core_; + ov::CompiledModel compiled_model_; + + const int model_image_height_ = 640; + const int model_image_width_ = 640; +}; +} \ No newline at end of file diff --git a/src/tongji/identifier/classifier.hpp b/src/tongji/identifier/classifier.hpp new file mode 100644 index 0000000..6889475 --- /dev/null +++ b/src/tongji/identifier/classifier.hpp @@ -0,0 +1,16 @@ +#pragma once + +#include +namespace world_exe::tongji::identifier { + +class Classifier final { +public: + Classifier(); + ~Classifier(); + +private: + class Impl; + std::unique_ptr pimpl_; +}; + +} \ No newline at end of file diff --git a/src/tongji/solver/solver.cpp b/src/tongji/solver/solver.cpp index 49496e5..658841e 100644 --- a/src/tongji/solver/solver.cpp +++ b/src/tongji/solver/solver.cpp @@ -12,7 +12,6 @@ #include "data/armor_image_spaceing.hpp" #include "enum/armor_id.hpp" #include "interfaces/armor_in_camera.hpp" -#include "interfaces/armor_in_gimbal_control.hpp" #include "interfaces/time_stamped.hpp" #include "parameters/profile.hpp" #include "parameters/rm_parameters.hpp" From 477a35fd443bfc7894cc51baddc1ee92e79b1cab Mon Sep 17 00:00:00 2001 From: heyeuu <2829004293@qq.com> Date: Tue, 2 Sep 2025 19:42:37 +0800 Subject: [PATCH 03/53] fix(classifier): resolve issues in classifier class implementation --- src/tongji/identifier/classifier.cpp | 12 +++++++++++- src/tongji/identifier/classifier.hpp | 9 ++++++++- src/tongji/identifier/identifier.cpp | 0 src/tongji/identifier/identifier.hpp | 0 4 files changed, 19 insertions(+), 2 deletions(-) create mode 100644 src/tongji/identifier/identifier.cpp create mode 100644 src/tongji/identifier/identifier.hpp diff --git a/src/tongji/identifier/classifier.cpp b/src/tongji/identifier/classifier.cpp index f538a0b..c7e8139 100644 --- a/src/tongji/identifier/classifier.cpp +++ b/src/tongji/identifier/classifier.cpp @@ -81,7 +81,6 @@ class Classifier::Impl final { } enumeration::ArmorIdFlag ovclassify(const cv::Mat& armor_pattern) { - if (armor_pattern.empty()) { return enumeration::ArmorIdFlag::Unknow; } @@ -157,4 +156,15 @@ class Classifier::Impl final { const int model_image_height_ = 640; const int model_image_width_ = 640; }; + +Classifier::Classifier(const std::string& model_path, int model_image_width, int model_image_height) + : pimpl_(std::make_unique(model_path, model_image_width, model_image_height)) { } +Classifier::~Classifier() = default; + +enumeration::ArmorIdFlag Classifier::classify(const cv::Mat& armor_pattern) { + return pimpl_->classify(armor_pattern); +} +enumeration::ArmorIdFlag Classifier::ovclassify(const cv::Mat& armor_pattern){ + return pimpl_->ovclassify(armor_pattern); +} } \ No newline at end of file diff --git a/src/tongji/identifier/classifier.hpp b/src/tongji/identifier/classifier.hpp index 6889475..86080bc 100644 --- a/src/tongji/identifier/classifier.hpp +++ b/src/tongji/identifier/classifier.hpp @@ -1,13 +1,20 @@ #pragma once #include +#include + +#include "enum/armor_id.hpp" namespace world_exe::tongji::identifier { class Classifier final { public: - Classifier(); + Classifier(const std::string& model_path, int model_image_width, int model_image_height); ~Classifier(); + enumeration::ArmorIdFlag classify(const cv::Mat& armor_pattern); + + enumeration::ArmorIdFlag ovclassify(const cv::Mat& armor_pattern); + private: class Impl; std::unique_ptr pimpl_; diff --git a/src/tongji/identifier/identifier.cpp b/src/tongji/identifier/identifier.cpp new file mode 100644 index 0000000..e69de29 diff --git a/src/tongji/identifier/identifier.hpp b/src/tongji/identifier/identifier.hpp new file mode 100644 index 0000000..e69de29 From cd95986a98ea9d1dfb9e1dd1d24bd105760eb3b4 Mon Sep 17 00:00:00 2001 From: fan Date: Tue, 2 Sep 2025 22:55:08 +0800 Subject: [PATCH 04/53] =?UTF-8?q?=E6=B7=BB=E5=8A=A0=E6=B3=A8=E9=87=8A?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/util/scanline.hpp | 4 - src/v1/identifier/identifier.cpp | 152 +++++++++++++++++---- src/v1/predictor/car/car_predictor_ekf.hpp | 137 ++++++++++++------- src/v1/predictor/predictor_manager.cpp | 6 +- 4 files changed, 220 insertions(+), 79 deletions(-) diff --git a/src/util/scanline.hpp b/src/util/scanline.hpp index c09fefa..32185c3 100644 --- a/src/util/scanline.hpp +++ b/src/util/scanline.hpp @@ -11,7 +11,6 @@ class ScanLine { if (contour.empty()) return {}; std::vector points; - points.reserve(contour.size()); for (const auto& point : contour) points.emplace_back(point); @@ -163,11 +162,9 @@ class ScanLine { } static inline std::vector collect_poly_edges(const std::vector& points) { - int delta = ((1 << 0) >> 1); const auto points_size = points.size(); cv::Point2l pt0 = points[points_size - 1], pt1; pt0.x = (pt0.x) << XY_SHIFT; - pt0.y = pt0.y + delta; std::vector edges; edges.reserve(edges.size() + points_size); @@ -175,7 +172,6 @@ class ScanLine { cv::Point2l t0, t1; pt1 = points[i]; pt1.x = (pt1.x) << XY_SHIFT; - pt1.y = pt1.y + delta; t0.x = pt0.x; t1.x = pt1.x; diff --git a/src/v1/identifier/identifier.cpp b/src/v1/identifier/identifier.cpp index 9fb1bb6..be64c60 100644 --- a/src/v1/identifier/identifier.cpp +++ b/src/v1/identifier/identifier.cpp @@ -1,3 +1,12 @@ +/** + * @file identifier.cpp + * @brief 装甲板识别器实现文件 + * + * 该识别器使用深大开源模型进行识别获取到装甲板位置，但是模型是典型的四点模型， + * 我们只能把模型输出结果当做roi再去做精细的识别处理，得到装甲板的灯条的四个角点， + * 然后通过传统视觉方法进行灯条匹配，最后基于颜色信息进行敌我识别。 + */ + #include "identifier.hpp" #include "identifier_armor.hpp" #include "util/scanline.hpp" @@ -10,14 +19,23 @@ #include namespace world_exe::v1::identifier { + class Identifier::Impl { public: + /** + * @brief 构造函数，初始化 OpenVINO 模型和相关参数 + * @param model_path ONNX 模型文件路径 + * @param device 推理设备（如 "CPU", "GPU" 等） + * @param image_width 输入图像宽度（默认1440） + * @param image_height 输入图像高度（默认1080） + */ explicit Impl(const std::string& model_path, const std::string& device, const int& image_width = 1440, const int& image_height = 1080) : image_width_(image_width) , image_height_(image_height) , width_ratio_(static_cast(image_width_) / model_image_width_) , height_ratio_(static_cast(image_height_) / model_image_height_) { + // 此处主要是对模型推理的初始化 ov::Core core_; const auto adevice = core_.get_available_devices(); auto model_ = core_.read_model(model_path); @@ -25,6 +43,7 @@ class Identifier::Impl { std::unique_ptr pre_post_processor_ = std::make_unique(model_); ov::Shape input_shape_ { 1, model_image_height_, model_image_width_, 3 }; + pre_post_processor_->input() .tensor() .set_element_type(ov::element::u8) @@ -36,7 +55,6 @@ class Identifier::Impl { .convert_element_type(ov::element::f32) .convert_color(ov::preprocess::ColorFormat::RGB) .scale({ 255., 255., 255. }); - pre_post_processor_->input().model().set_layout("NCHW"); pre_post_processor_->output().tensor().set_element_type(ov::element::f32); model_ = pre_post_processor_->build(); @@ -44,26 +62,54 @@ class Identifier::Impl { compiled_model_ = core_.compile_model(model_, device); } + /** + * @brief 设置目标颜色（敌我识别） + * @param target_color false 为蓝色，true 为红色 + */ inline void SetTargetColor(bool target_color) { target_color_ = target_color; } + /** + * @brief 主识别接口，对输入图像进行装甲板识别 + * @param input_image 输入的 BGR 格式图像(默认就是) + */ std::tuple, enumeration::CarIDFlag> Identify( const cv::Mat& input_image) { + // 首先使用深度学习模型进行装甲板检测得到roi区域 const auto armor_infos = model_infer(input_image); + // 然后进行灯条匹配验证 return matchPlate(input_image, armor_infos); } + /** + * @brief 设置装甲板匹配时的放大倍数，这个放大倍数是对于模型输出的roi， + * 我们要以中心点为原点放大一定的比例来匹配灯条，这样主要是为了避免灯条再模型直接输出的roi内不完整 + * @param ratio 放大倍数，用于扩大装甲板区域进行灯条检测 + */ inline void set_match_magnification_ratio(const double& ratio) { match_magnification_ratio_ = ratio; } private: struct ArmorInfo { - cv::Rect rect_; - enumeration::ArmorIdFlag id_; + cv::Rect rect_; ///< 装甲板在图像中的边界框 + enumeration::ArmorIdFlag id_; ///< 装甲板类型（英雄、步兵等） }; + /** + * @brief 使用深度学习模型进行装甲板检测和分类 + * @param img 输入图像 + * @return 检测到的装甲板信息列表 + * + * 处理流程： + * 1. 图像预处理（缩放到模型输入尺寸） + * 2. 模型推理 + * 3. 后处理（解析输出、过滤、NMS） + * 4. 返回符合要求的装甲板信息 + */ std::vector model_infer(const cv::Mat& img) { + // 注意模型的输入为640x640，所以此处要resize，然后后面处理时要再变回去 cv::resize(img, resized_img_, cv::Size(model_image_width_, model_image_height_)); + const auto input_tensor = ov::Tensor { compiled_model_.input().get_element_type(), compiled_model_.input().get_shape(), resized_img_.data }; ov::InferRequest infer_request = compiled_model_.create_infer_request(); @@ -81,22 +127,30 @@ class Identifier::Impl { std::vector class_scores; std::vector confidences; std::vector tmp_objects_; + + // 以下就是模型推理获取结果的具体流程 for (int i = 0; i < output_buffer.rows; i++) { + // 获取置信度并应用 sigmoid 激活 float confidence = output_buffer.at(i, 8); confidence = static_cast(sigmoid(confidence)); - if (confidence < conf_threshold_) continue; + if (confidence < conf_threshold_) continue; // 过滤低置信度检测 + // 解析颜色分类结果（蓝、红、紫、无色） const auto color_scores = output_buffer.row(i).colRange(9, 13); // color + // 解析车辆类型分类结果（哨兵、英雄、工程等） const auto classes_scores = output_buffer.row(i).colRange(13, 22); // num cv::Point class_id, color_id; double score_color, score_num; cv::minMaxLoc(classes_scores, nullptr, &score_num, nullptr, &class_id); cv::minMaxLoc(color_scores, nullptr, &score_color, nullptr, &color_id); + // 颜色过滤：只保留目标颜色的装甲板 + // color_id.x: 0=蓝色, 1=红色, 2+=其他颜色 if (color_id.x >= 2 || (color_id.x == 1 && !target_color_) || (color_id.x == 0 && target_color_)) continue; + // 根据类别ID映射到装甲板类型枚举，这里尽量去吧高可能性的放前面，然后这个模型其实还支持识别其他的装甲板，但基本并不需要，如果有需求，可以去深大开源readme里找 ArmorInfo obj; if (class_id.x == 3) { obj.id_ = enumeration::ArmorIdFlag::InfantryIII; @@ -114,15 +168,18 @@ class Identifier::Impl { obj.id_ = enumeration::ArmorIdFlag::Sentry; } else if (class_id.x == 7) { obj.id_ = enumeration::ArmorIdFlag::Base; - } else continue; + } else continue; // 未知类型，跳过 tmp_objects_.emplace_back(obj); + // 解析装甲板的四个角点坐标 std::array points { cv::Point2f { output_buffer.at(i, 0), output_buffer.at(i, 1) }, cv::Point2f { output_buffer.at(i, 6), output_buffer.at(i, 7) }, cv::Point2f { output_buffer.at(i, 4), output_buffer.at(i, 5) }, cv::Point2f { output_buffer.at(i, 2), output_buffer.at(i, 3) } }; + + // 计算包围盒：找到四个点的最小和最大 x,y 坐标 float min_x = points[0].x; float max_x = points[0].x; float min_y = points[0].y; @@ -134,13 +191,17 @@ class Identifier::Impl { if (points[i].y > max_y) max_y = points[i].y; } + // 将坐标从模型尺寸映射回原图像尺寸 boxes.emplace_back(min_x * width_ratio_, min_y * height_ratio_, (max_x - min_x) * width_ratio_, (max_y - min_y) * height_ratio_); confidences.emplace_back(score_num); } + // 应用非最大抑制（NMS）去除重复检测 std::vector indices; cv::dnn::NMSBoxes(boxes, confidences, conf_threshold_, nms_threshold_, indices); + + // 构建最终的检测结果 std::vector objects_; for (const std::size_t valid_index : indices) if (valid_index <= boxes.size()) { @@ -158,8 +219,8 @@ class Identifier::Impl { } struct LightBar { - cv::Point2f top_, bottom_; - float angle_; + cv::Point2f top_, bottom_; ///< 灯条的上下端点 + float angle_; ///< 灯条的角度（这个实际并没有用，可以扔掉） LightBar(const cv::Point2f& top, const cv::Point2f& bottom, float angle) : top_(top) @@ -169,12 +230,19 @@ class Identifier::Impl { std::tuple, enumeration::CarIDFlag> matchPlate( const cv::Mat& img, const std::vector& armor_infos) { + if (armor_infos.empty()) return {}; + + // 图像预处理：转换为灰度图并二值化，为后续灯条检测做准备,由于我们前面通过模型拿到了灯条的roi区域，在区域内基本不会误识别，所以后面我们通过传统方式来匹配时各种阈值都可以给松一些，注意，这里的阈值可能要根据实际情况做出调整 cv::cvtColor(img, gray_img_, cv::COLOR_BGR2GRAY); cv::threshold(gray_img_, gray_img_, 30, 255, cv::THRESH_BINARY); + // 初始化结果容器 uint32_t all_car_id = static_cast(enumeration::ArmorIdFlag::None); std::vector armor_plates; + + // 对每个模型检测的装甲板进行验证 for (const auto& armor : armor_infos) { + // 这里是为了把模型直接输出的roi加个放大的因数放大，然后再进行匹配 const auto offset = cv::Point { std::clamp(static_cast(armor.rect_.x - armor.rect_.width / 2. * (match_magnification_ratio_ - 1.)), @@ -183,18 +251,20 @@ class Identifier::Impl { - armor.rect_.height / 2. * (match_magnification_ratio_ - 1.)), 0, image_width_) }; - cv::Size rect_size { std::clamp(static_cast( armor.rect_.width * match_magnification_ratio_), 0, image_width_ - offset.x), std::clamp(static_cast(armor.rect_.height * match_magnification_ratio_), 0, image_height_ - offset.y) }; - const auto armor_roi = gray_img_(cv::Rect { offset, rect_size }); + // 在 ROI 中寻找轮廓（潜在的灯条） std::vector> contours; cv::findContours(armor_roi, contours, cv::RETR_EXTERNAL, cv::CHAIN_APPROX_NONE); + + // 至少需要两个灯条才能构成装甲板 if (contours.size() >= 2) { + // 按面积降序排序轮廓，优先处理较大的轮廓 std::sort(contours.begin(), contours.end(), [](const std::vector& a, const std::vector& b) { return cv::contourArea(a, false) > cv::contourArea(b, false); @@ -202,55 +272,72 @@ class Identifier::Impl { double first_area { 0. }; std::vector lightbars_; + + // 遍历轮廓，寻找合适的灯条 for (const auto& contour : contours) { auto b_rect = cv::boundingRect(contour); + // 计算轮廓在原图中的位置 cv::Point roi_point { std::clamp(offset.x + b_rect.x, 0, image_width_), std::clamp(offset.y + b_rect.y, 0, image_height_) }; cv::Size roi_size { std::clamp(b_rect.width, 0, image_width_ - roi_point.x), std::clamp(b_rect.height, 0, image_height_ - roi_point.y) }; const auto light_bar_roi = img(cv::Rect { roi_point, roi_size }); + // 颜色过滤：基于 BGR 通道的差值判断灯条颜色 const auto channels = cv::mean(light_bar_roi); - const auto b_r_difference = channels[0] - channels[2]; + const auto b_r_difference = channels[0] - channels[2]; // B通道 - R通道 + // 如果目标是红色且B-R>0（偏蓝），或目标是蓝色且B-R<0（偏红），则跳过 if ((target_color_ && b_r_difference > 0) || (!target_color_ && b_r_difference < 0)) continue; + // 使用扫描线算法获取轮廓内的所有点，然后这个扫描线时把opencv中的源码掏出来精简得到的，然后里面有一些奇怪的操作，我个人也没看太明白 const auto points = util::ScanLine::get_points(armor_roi, contour); if (points.empty()) continue; + + // 使用 PCA 分析获取灯条的主轴端点 const auto [high_point, low_point] = perform_pca(points); + // 处理第一个有效的灯条 if (lightbars_.empty()) { first_area = cv::contourArea(contour); lightbars_.emplace_back(high_point + offset, low_point + offset, 0.); } else { + // 对于后续灯条，检查面积比例是否合理（避免尺寸差异过大的灯条配对，这里的目的是为了避免识别到特别侧着的装甲板，因为这个神经网络啊，实在太敏感了，即便是很偏的情况依然能够识别到，但是太偏的话，我们pca拿到的角点是不准的，所以我们不要） const auto area_ratio = first_area / cv::contourArea(contour); if (area_ratio < 10. && area_ratio > 1. / 10.) lightbars_.emplace_back(high_point + offset, low_point + offset, 0.); } + // 找到两个灯条就停止搜索 if (lightbars_.size() == 2) break; } const auto light_bar_size_ = lightbars_.size(); + // 如果找到了两个灯条，构建装甲板 if (light_bar_size_ == 2) { const auto& first = lightbars_[0]; const auto& second = lightbars_[1]; + + // 根据 x 坐标排序灯条，确保左灯条在前 if ((first.top_.x + first.bottom_.x) / 2. < (second.bottom_.x + second.top_.x) / 2.) { + // 构建装甲板的四个角点：左上、右上、右下、左下 armor_plates.emplace_back(data::ArmorImageSpacing { armor.id_, { first.top_, second.top_, second.bottom_, first.bottom_ } }); } else { armor_plates.emplace_back(data::ArmorImageSpacing { armor.id_, { second.top_, first.top_, first.bottom_, second.bottom_ } }); } + // 将装甲板类型添加到检测到的车辆类型标志位中 all_car_id |= static_cast(armor.id_); } } } + // 返回验证后的装甲板信息和检测到的车辆类型 return { std::make_shared(armor_plates), static_cast(all_car_id) }; } @@ -258,17 +345,23 @@ class Identifier::Impl { static inline std::tuple perform_pca( const std::vector& points) { const int points_num { static_cast(points.size()) }; + + // 将点坐标转换为 PCA 算法需要的矩阵格式 cv::Mat data(points_num, 2, CV_32F); for (int j = 0; j < points_num; ++j) { data.at(j, 0) = static_cast(points[j].x); data.at(j, 1) = static_cast(points[j].y); } + // 执行 PCA 分析 cv::PCA pca(data, cv::Mat(), cv::PCA::DATA_AS_ROW); + + // 获取第一主成分（主轴方向） cv::Vec2f principal_axis( pca.eigenvectors.at(0, 0), pca.eigenvectors.at(0, 1)); cv::Point2f center(pca.mean.at(0, 0), pca.mean.at(0, 1)); + // 将所有点投影到主轴上，找到最小和最大投影值 float min_proj = std::numeric_limits::max(); float max_proj = std::numeric_limits::lowest(); @@ -279,31 +372,37 @@ class Identifier::Impl { if (proj > max_proj) max_proj = proj; } + // 重构主轴的两个端点 const cv::Point reconstructed_min_point_local = center + cv::Point2f { principal_axis * min_proj }; const cv::Point reconstructed_max_point_local = center + cv::Point2f { principal_axis * max_proj }; + // 确保返回的第一个点是上端点（y坐标较小） if (reconstructed_min_point_local.y < reconstructed_max_point_local.y) return { reconstructed_min_point_local, reconstructed_max_point_local }; else return { reconstructed_max_point_local, reconstructed_min_point_local }; } - static constexpr int model_image_height_ = 640; - static constexpr int model_image_width_ = 640; - int image_height_ = 1080; - int image_width_ = 1440; - double width_ratio_ = static_cast(image_width_) / model_image_width_; - double height_ratio_ = static_cast(image_height_) / model_image_height_; - static constexpr double conf_threshold_ = 0.65; - static constexpr double nms_threshold_ = 0.45; - - double match_magnification_ratio_ = 1.5; - - cv::Mat resized_img_; - cv::Mat gray_img_; - bool target_color_ { false }; - ov::CompiledModel compiled_model_; + static constexpr int model_image_height_ = 640; ///< 模型输入图像高度 + static constexpr int model_image_width_ = 640; ///< 模型输入图像宽度 + + int image_height_ = 1080; ///< 实际输入图像高度 + int image_width_ = 1440; ///< 实际输入图像宽度 + double width_ratio_ = static_cast(image_width_) / model_image_width_; ///< 宽度缩放比例 + double height_ratio_ = + static_cast(image_height_) / model_image_height_; ///< 高度缩放比例 + + static constexpr double conf_threshold_ = 0.65; ///< 置信度阈值 + static constexpr double nms_threshold_ = 0.45; ///< NMS 阈值 + + double match_magnification_ratio_ = 1.5; ///< 装甲板匹配时的放大倍数 + + cv::Mat resized_img_; ///< 缩放后的图像缓存 + cv::Mat gray_img_; ///< 灰度图像缓存 + + bool target_color_ { false }; ///< 目标颜色：false=蓝色，true=红色 + ov::CompiledModel compiled_model_; ///< 编译后的 OpenVINO 模型 }; Identifier::Identifier(const std::string& model_path, const std::string& device, @@ -322,4 +421,5 @@ void Identifier::set_match_magnification_ratio(const double& ratio) { } Identifier::~Identifier() = default; -} \ No newline at end of file + +} // namespace world_exe::v1::identifier \ No newline at end of file diff --git a/src/v1/predictor/car/car_predictor_ekf.hpp b/src/v1/predictor/car/car_predictor_ekf.hpp index 9fbfd74..029551c 100644 --- a/src/v1/predictor/car/car_predictor_ekf.hpp +++ b/src/v1/predictor/car/car_predictor_ekf.hpp @@ -8,37 +8,63 @@ namespace world_exe::v1::predictor { +/** + * @class CarPredictEkf + * @brief 车辆预测EKF核心类 + * + * 状态向量维度: 11维 [x, vx, y, vy, z, z1, z2, r1, r2, θ, ω] + * 观测向量维度: 4维 [θ, yaw, pitch, distance] + * + * 这个是整个预测的核心，无论是逻辑的维护还是预测的模型都放在这里了，然后其他的主要是为了接口实现 + */ class CarPredictEkf : public world_exe::util::Ekf<11, 4, CarPredictEkf> { public: + /** + * @brief 构造函数，初始化状态向量和协方差矩阵 + * 状态: [x=2.0, vx=0, y=0, vy=0, z=-0.3, z1=-0.3, z2=-0.3, r1=0.2, r2=0.2, θ=π, ω=0] + */ CarPredictEkf() { X_k << 2.0, 0.0, 0.0, 0.0, -0.3, -0.3, -0.3, 0.2, 0.2, std::numbers::pi, 0.0; P_k.diagonal() << 1., 1., 1., 1., 1., 0., 0., 1., 1., 1., 1.; } + /** + * @brief 设置第二装甲板标志 + * 这里这个函数需要在同时识别到两个装甲板时调用，在update传入第一块装甲板的信息后，update传入第二块装甲板的信息前，这样做的目的是因为这个模型内维护有最近装甲板的角度，但又需要在识别到两块装甲板时能充分利用两块装甲板来更新状态，所以说需要一个标志位来动态地改变模型的一些参数，主要是 + + */ inline void set_second_armor() { second_armor_flag = true; } + /** + * @brief 获取预测的装甲板位置 + * @param id 装甲板类型 + * @param sec 预测时间（秒） + * @return 四个装甲板的预测位置和姿态 + */ inline std::vector get_predict_output_armor( const enumeration::ArmorIdFlag& id, const double& sec) const { std::vector target_armors; const auto model_output = X_k; - const auto vx = model_output(1); - const auto vy = model_output(3); + const auto vx = model_output(1); // x方向速度 + const auto vy = model_output(3); // y方向速度 + // 预测车辆中心位置 const double car_x = model_output(0) + vx * sec; const double car_y = model_output(2) + vy * sec; - const double car_z = model_output(4); - const double z1 = model_output(5); - const double z2 = model_output(6); - const double r1 = model_output(7); - const double r2 = model_output(8); - double model_yaw = model_output(9) + model_output(10) * sec; + const double car_z = model_output(4); // 车辆高度 + const double z1 = model_output(5); // 装甲板1高度 + const double z2 = model_output(6); // 装甲板2高度 + const double r1 = model_output(7); // 装甲板1到中心距离 + const double r2 = model_output(8); // 装甲板2到中心距离 + double model_yaw = model_output(9) + model_output(10) * sec; // 预测yaw角 - // TODO: 把 pitch 与 z 关联起来 + // TODO: 把 pitch 与 z 关联起来，这里可以通过z和z1、z2把pitch算出来 const double pitch1 = 15. / 180. * std::numbers::pi; const double pitch2 = 15. / 180. * std::numbers::pi; + // 根据side_flag_生成四个装甲板的位置 if (side_flag_) { const auto armor1 = data::ArmorGimbalControlSpacing { id, { car_x - r1 * std::cos(model_yaw), car_y - r1 * std::sin(model_yaw), z1 }, @@ -93,20 +119,24 @@ class CarPredictEkf : public world_exe::util::Ekf<11, 4, CarPredictEkf> { private: friend class world_exe::util::Ekf<11, 4, CarPredictEkf>; - // 0, 1, 2, 3, 4, 5, 6, 7, 8, 9,10 - // x分别代表[ x, v_x, y, v_y, z, z_1, z_2, r1, r2, θ, ω ] - // z分别代表[ θ, yaw, pitch, distance ] + /** + * @brief 观测预处理函数 + * 状态: x分别代表[ x, v_x, y, v_y, z, z_1, z_2, r1, r2, θ, ω ] + * 观测: z分别代表[ θ, yaw, pitch, distance ] + */ inline ZVec process_z(const ZVec& z_k) { ZVec processed_z { z_k }; if (z_k(0) < 0.) processed_z(0) += std::numbers::pi * 2.; + // 检测装甲板切换，switch_angle_difference_根据实际情况可能需要调整 double offset { processed_z(0) - last_theta_ }; if (std::abs(offset) >= switch_angle_difference_) { - side_flag_ = !side_flag_; + side_flag_ = !side_flag_; // 切换装甲板面 } else offset = 0.; last_theta_ = processed_z(0); + // 根据当前装甲板面更新高度 if (side_flag_) X_k(5) = z_k(3) * std::cos(z_k(1)) * -std::sin(z_k(2)); else X_k(6) = z_k(3) * std::cos(z_k(1)) * -std::sin(z_k(2)); @@ -116,9 +146,11 @@ class CarPredictEkf : public world_exe::util::Ekf<11, 4, CarPredictEkf> { return processed_z; }; + /** @brief 状态向量归一化 */ inline XVec normalize_x(const XVec& x_k) { normalized_x = x_k; auto camera_yaw = x_k(9); + // 角度归一化到 [0, 2π) while (camera_yaw < 0.) camera_yaw += 2 * std::numbers::pi; while (camera_yaw >= 2 * std::numbers::pi) @@ -128,37 +160,41 @@ class CarPredictEkf : public world_exe::util::Ekf<11, 4, CarPredictEkf> { return normalized_x; }; + /** @brief 状态转移函数 f(x,u,w,dt) */ inline XVec f(const XVec& X_k, const UVec&, const WVec&, const double& dt) { - const double x = X_k(0) + X_k(1) * dt; - const double y = X_k(2) + X_k(3) * dt; - const double z = (X_k(5) + X_k(6)) / 2.; - const double theta = X_k(9) + X_k(10) * dt; + const double x = X_k(0) + X_k(1) * dt; // 位置更新 + const double y = X_k(2) + X_k(3) * dt; // 位置更新 + const double z = (X_k(5) + X_k(6)) / 2.; // 平均高度 + const double theta = X_k(9) + X_k(10) * dt; // 角度更新 f_ << x, X_k(1), y, X_k(3), z, X_k(5), X_k(6), X_k(7), X_k(8), theta, X_k(10); return f_; }; + /** @brief 观测函数 h(x,v) */ inline ZVec h(const XVec& x_k_n, const VVec&) { double r, z; + // 根据当前装甲板面选择参数 if (side_flag_) { - r = x_k_n(7); - z = x_k_n(5); + r = x_k_n(7); // r1 + z = x_k_n(5); // z1 } else { - r = x_k_n(8); - z = x_k_n(6); + r = x_k_n(8); // r2 + z = x_k_n(6); // z2 } const double theta = x_k_n(9); - const double armor_x = x_k_n(0) + r * std::cos(theta); - const double armor_y = x_k_n(2) + r * std::sin(theta); - const double yaw = std::atan(armor_y / armor_x); - const double pitch = -std::atan(z / armor_x); - const double distance = std::sqrt(armor_x * armor_x + armor_y * armor_y + z * z); + const double armor_x = x_k_n(0) + r * std::cos(theta); // 装甲板x坐标 + const double armor_y = x_k_n(2) + r * std::sin(theta); // 装甲板y坐标 + const double yaw = std::atan(armor_y / armor_x); // yaw角 + const double pitch = -std::atan(z / armor_x); // pitch角 + const double distance = std::sqrt(armor_x * armor_x + armor_y * armor_y + z * z); // 距离 h_ << x_k_n(9), yaw, pitch, distance; return h_; }; + /** @brief 状态转移雅可比矩阵 A */ inline AMat A(const XVec&, const XVec&, const XVec&, const double& dt) { // clang-format off A_ << 1., dt, 0., 0., 0., 0., 0., 0., 0., 0., 0., @@ -176,8 +212,10 @@ class CarPredictEkf : public world_exe::util::Ekf<11, 4, CarPredictEkf> { return A_; }; + /** @brief 过程噪声雅可比矩阵 W */ inline WMat W(const XVec&, const XVec&, const XVec&) const { return W_; }; + /** @brief 观测雅可比矩阵 H */ inline HMat H(const XVec& x_k_n, const VVec&) { const double theta = x_k_n(9); @@ -260,8 +298,10 @@ class CarPredictEkf : public world_exe::util::Ekf<11, 4, CarPredictEkf> { return H_; }; + /** @brief 观测噪声雅可比矩阵 V */ inline VMat V(const XVec&, const VVec&) const { return V_; }; + /** @brief 过程噪声协方差矩阵 Q */ inline QMat Q(const double& dt) { double x; x = 100; @@ -303,6 +343,7 @@ class CarPredictEkf : public world_exe::util::Ekf<11, 4, CarPredictEkf> { const double yaw_center_difference_ = std::abs(std::sin(z(0))) + 1.; if (second_armor_flag) { second_armor_flag = false; + // 同时识别到两块装甲板时，第二装甲板时忽略θ观测，因为第一个输入装甲板是比较近的且较准 R_.diagonal() << 9999999999999., yaw_center_difference_ * r_theta_yaw_, yaw_center_difference_ * r_theta_pitch_, yaw_center_difference_ * r_theta_distance_; } else { @@ -313,25 +354,27 @@ class CarPredictEkf : public world_exe::util::Ekf<11, 4, CarPredictEkf> { return R_; }; - XVec f_ {}; - ZVec h_ {}; - AMat A_ {}; - WMat W_ { WMat::Identity() }; - HMat H_ {}; - VMat V_ { VMat::Identity() }; - QMat Q_ {}; - RMat R_ {}; - XVec normalized_x {}; - - bool debug_ { true }; - double last_theta_ { std::numbers::pi }; - static constexpr double switch_angle_difference_ = std::numbers::pi * 70. / 180.; - static constexpr double r_theta_theta_ = 0.1; - static constexpr double r_theta_yaw_ = 1e-3; - static constexpr double r_theta_pitch_ = 1e-3; - static constexpr double r_theta_distance_ = 1.; - - bool side_flag_ { true }; - bool second_armor_flag { false }; + XVec f_ {}; ///< 状态转移函数输出缓存 + ZVec h_ {}; ///< 观测函数输出缓存 + AMat A_ {}; ///< 状态转移雅可比矩阵 + WMat W_ { WMat::Identity() }; ///< 过程噪声雅可比矩阵 + HMat H_ {}; ///< 观测雅可比矩阵 + VMat V_ { VMat::Identity() }; ///< 观测噪声雅可比矩阵 + QMat Q_ {}; ///< 过程噪声协方差矩阵 + RMat R_ {}; ///< 观测噪声协方差矩阵 + XVec normalized_x {}; ///< 归一化状态向量缓存 + + bool debug_ { true }; ///< 调试标志 + double last_theta_ { std::numbers::pi }; ///< 上次观测角度 + static constexpr double switch_angle_difference_ = + std::numbers::pi * 70. / 180.; ///< 装甲板切换角度阈值 + static constexpr double r_theta_theta_ = 0.1; ///< θ观测噪声 + static constexpr double r_theta_yaw_ = 1e-3; ///< yaw观测噪声 + static constexpr double r_theta_pitch_ = 1e-3; ///< pitch观测噪声 + static constexpr double r_theta_distance_ = 1.; ///< 距离观测噪声 + + bool side_flag_ { true }; ///< 装甲板面标志位 + bool second_armor_flag { false }; ///< 第二装甲板标志位 }; -} \ No newline at end of file + +} // namespace world_exe::v1::predictor \ No newline at end of file diff --git a/src/v1/predictor/predictor_manager.cpp b/src/v1/predictor/predictor_manager.cpp index 37f0c61..a685f94 100644 --- a/src/v1/predictor/predictor_manager.cpp +++ b/src/v1/predictor/predictor_manager.cpp @@ -11,9 +11,9 @@ namespace world_exe::v1::predictor { class PredictorManager::Impl { public: - inline void Update(std::shared_ptr data) { + inline void Update(const std::shared_ptr& data) { const auto time_stamp = data->GetTimeStamped().GetTimeStamp(); - const auto dt = (time_stamp - last_update_time_stamp_.GetTimeStamp()) / 1.e9; + const auto dt = (time_stamp - last_update_time_stamp_.GetTimeStamp()); const auto transform = data->GetTransform(); const auto rotation_transform = Eigen::Quaterniond { transform.linear() }; @@ -46,6 +46,7 @@ class PredictorManager::Impl { -std::atan(tmp_armor0.position.z() / tmp_armor0.position.x()), tmp_armor0.position.norm(); predictors_[i].Update(input, {}, dt); + // 同时识别到一辆车的两块装甲板时要调这个函数 predictors_[i].set_second_armor(); input << util::math::get_yaw_from_quaternion(tmp_armor1.orientation), std::atan(tmp_armor1.position.y() / tmp_armor1.position.x()), @@ -58,6 +59,7 @@ class PredictorManager::Impl { -std::atan(tmp_armor1.position.z() / tmp_armor1.position.x()), tmp_armor1.position.norm(); predictors_[i].Update(input, {}, dt); + // 同时识别到一辆车的两块装甲板时要调这个函数 predictors_[i].set_second_armor(); input << util::math::get_yaw_from_quaternion(tmp_armor0.orientation), std::atan(tmp_armor0.position.y() / tmp_armor0.position.x()), From 879682b8abfd9eaa68af01cfc726832450a6a651 Mon Sep 17 00:00:00 2001 From: heyeuu <2829004293@qq.com> Date: Wed, 3 Sep 2025 02:57:23 +0800 Subject: [PATCH 05/53] feat(logging): add logger module --- CMakeLists.txt | 12 ++++++++++++ src/util/logger.cpp | 32 ++++++++++++++++++++++++++++++++ 2 files changed, 44 insertions(+) create mode 100644 src/util/logger.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index 1716dd6..f3e5b7e 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -53,6 +53,18 @@ include_directories(${CERES_INCLUDE_DIRS}) target_link_libraries(${PROJECT_NAME} ${CERES_LIBRARIES}) ### ceres End === +### libspdlog-dev === +find_package(spdlog REQUIRED) +target_include_directories(${fmt_INCLUDE_DIRS}) +target_link_libraries(${PROJECT_NAME} ${fmt_LIBRARIES}) +### libspdlog-dev End === + +### libfmt-dev === +find_package(fmt REQUIRED) +target_include_directories(${fmt_INCLUDE_DIRS}) +target_link_libraries(${PROJECT_NAME} ${fmt_LIBRARIES}) +### libfmt-dev End === + # 安装库文件 target_include_directories(${PROJECT_NAME} PUBLIC $ diff --git a/src/util/logger.cpp b/src/util/logger.cpp new file mode 100644 index 0000000..8b12095 --- /dev/null +++ b/src/util/logger.cpp @@ -0,0 +1,32 @@ +#pragma once + +#include +#include +#include +#include + +#include + +namespace world_exe::util::logger { +std::shared_ptr logger_ = nullptr; + +void set_logger() { + auto file_name = fmt::format("logs/{:%Y-%m-%d_%H-%M-%S}.log", std::chrono::system_clock::now()); + auto file_sink = std::make_shared(file_name, true); + file_sink->set_level(spdlog::level::debug); + + auto console_sink = std::make_shared(); + console_sink->set_level(spdlog::level::debug); + + logger_ = + std::make_shared("", spdlog::sinks_init_list { file_sink, console_sink }); + logger_->set_level(spdlog::level::debug); + logger_->flush_on(spdlog::level::info); +} + +std::shared_ptr logger() { + if (!logger_) set_logger(); + return logger_; +} + +} // namespace logger \ No newline at end of file From 0e7e50559b21e2bf6eac431d0ebacb8ea956ff85 Mon Sep 17 00:00:00 2001 From: heyeuu <2829004293@qq.com> Date: Thu, 4 Sep 2025 00:30:04 +0800 Subject: [PATCH 06/53] fix(build): correct CMakeLists.txt include directory configuration --- CMakeLists.txt | 2 -- 1 file changed, 2 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index f3e5b7e..1780a17 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -55,13 +55,11 @@ target_link_libraries(${PROJECT_NAME} ${CERES_LIBRARIES}) ### libspdlog-dev === find_package(spdlog REQUIRED) -target_include_directories(${fmt_INCLUDE_DIRS}) target_link_libraries(${PROJECT_NAME} ${fmt_LIBRARIES}) ### libspdlog-dev End === ### libfmt-dev === find_package(fmt REQUIRED) -target_include_directories(${fmt_INCLUDE_DIRS}) target_link_libraries(${PROJECT_NAME} ${fmt_LIBRARIES}) ### libfmt-dev End === From 78f16f9b5104444207232585d065437be43a21ee Mon Sep 17 00:00:00 2001 From: heyeuu <2829004293@qq.com> Date: Thu, 4 Sep 2025 00:32:50 +0800 Subject: [PATCH 07/53] feat(logging): add logger module --- src/util/logger.cpp | 6 +++--- src/util/logger.hpp | 8 ++++++++ 2 files changed, 11 insertions(+), 3 deletions(-) create mode 100644 src/util/logger.hpp diff --git a/src/util/logger.cpp b/src/util/logger.cpp index 8b12095..43e45e2 100644 --- a/src/util/logger.cpp +++ b/src/util/logger.cpp @@ -1,12 +1,12 @@ -#pragma once +#include "logger.hpp" + +#include #include #include #include #include -#include - namespace world_exe::util::logger { std::shared_ptr logger_ = nullptr; diff --git a/src/util/logger.hpp b/src/util/logger.hpp new file mode 100644 index 0000000..801db97 --- /dev/null +++ b/src/util/logger.hpp @@ -0,0 +1,8 @@ +#pragma once + +#include + +namespace world_exe::util::logger { +std::shared_ptr logger(); + +} // namespace logger From 7e477e68635645bcf16a41b20e90721382f30fd2 Mon Sep 17 00:00:00 2001 From: heyeuu <2829004293@qq.com> Date: Thu, 4 Sep 2025 00:35:49 +0800 Subject: [PATCH 08/53] feat(debug): add stringifier utility for ID string conversion --- src/util/stringifier.hpp | 35 +++++++++++++++++++++++++++++++++++ 1 file changed, 35 insertions(+) create mode 100644 src/util/stringifier.hpp diff --git a/src/util/stringifier.hpp b/src/util/stringifier.hpp new file mode 100644 index 0000000..0953582 --- /dev/null +++ b/src/util/stringifier.hpp @@ -0,0 +1,35 @@ +#pragma once + +#include + +#include "enum/armor_id.hpp" + +namespace world_exe::util::stringifier { +static inline std::string ToString(const world_exe::enumeration::ArmorIdFlag& id) { + + switch (id) { + case enumeration::ArmorIdFlag::Hero: + return "Hero"; + case enumeration::ArmorIdFlag::Engineer: + return "Engineer"; + case enumeration::ArmorIdFlag::InfantryIII: + return "InfantryIII"; + case enumeration::ArmorIdFlag::InfantryIV: + return "InfantryIV"; + case enumeration::ArmorIdFlag::InfantryV: + return "InfantryV"; + case enumeration::ArmorIdFlag::Sentry: + return "Sentry"; + case enumeration::ArmorIdFlag::Base: + return "Base"; + case enumeration::ArmorIdFlag::Outpost: + return "Outpost"; + case enumeration::ArmorIdFlag::Unknow: + return "Unknow"; + case enumeration::ArmorIdFlag::None: + return "None"; + default: + return "UnknownID"; + } +} +} \ No newline at end of file From 1586caf13035921a649b4ac1c1bfa9f41028dba9 Mon Sep 17 00:00:00 2001 From: heyeuu <2829004293@qq.com> Date: Thu, 4 Sep 2025 04:24:10 +0800 Subject: [PATCH 09/53] feat(identifier): add identifier module for armor identification --- src/tongji/identifier/classifier.cpp | 170 ---------- src/tongji/identifier/classifier.hpp | 148 ++++++++- src/tongji/identifier/identifier.cpp | 345 +++++++++++++++++++++ src/tongji/identifier/identifier.hpp | 55 ++++ src/tongji/identifier/identifier_armor.hpp | 36 +++ 5 files changed, 575 insertions(+), 179 deletions(-) delete mode 100644 src/tongji/identifier/classifier.cpp create mode 100644 src/tongji/identifier/identifier_armor.hpp diff --git a/src/tongji/identifier/classifier.cpp b/src/tongji/identifier/classifier.cpp deleted file mode 100644 index c7e8139..0000000 --- a/src/tongji/identifier/classifier.cpp +++ /dev/null @@ -1,170 +0,0 @@ -#include "classifier.hpp" -#include "enum/armor_id.hpp" - -#include -#include -#include -#include -#include - -namespace world_exe::tongji::identifier { -class Classifier::Impl final { -public: - explicit Impl(const std::string& model_path, int model_image_width, int model_image_height) - : model_image_width_(model_image_width) - , model_image_height_(model_image_height) { - - net_ = cv::dnn::readNetFromONNX(model_path); - auto ovmodel = core_.read_model(model_path); - compiled_model_ = core_.compile_model( - ovmodel, "AUTO", ov::hint::performance_mode(ov::hint::PerformanceMode::LATENCY)); - } - - enumeration::ArmorIdFlag classify(const cv::Mat& armor_pattern) { - if (armor_pattern.empty()) { - return enumeration::ArmorIdFlag::Unknow; - } - - cv::Mat gray; - cv::cvtColor(armor_pattern, gray, cv::COLOR_BGR2GRAY); - - auto input = cv::Mat(model_image_height_, model_image_width_, CV_8UC1, cv::Scalar(0)); - auto x_scale = static_cast(model_image_width_) / gray.cols; - auto y_scale = static_cast(model_image_height_) / gray.rows; - auto scale = std::min(x_scale, y_scale); - auto h = static_cast(gray.rows * scale); - auto w = static_cast(gray.cols * scale); - - if (h == 0 || w == 0) { - return enumeration::ArmorIdFlag::Unknow; - } - auto roi = cv::Rect(0, 0, w, h); - cv::resize(gray, input(roi), { w, h }); - - auto blob = cv::dnn::blobFromImage(input, 1.0 / 255.0, cv::Size(), cv::Scalar()); - - net_.setInput(blob); - cv::Mat outputs = net_.forward(); - - // softmax - float max = *std::max_element(outputs.begin(), outputs.end()); - cv::exp(outputs - max, outputs); - float sum = cv::sum(outputs)[0]; - outputs /= sum; - - double confidence; - cv::Point label_point; - cv::minMaxLoc(outputs.reshape(1, 1), nullptr, &confidence, nullptr, &label_point); - int label_id = label_point.x; - - enumeration::ArmorIdFlag armor_id; - - if (label_id == 3) { - armor_id = enumeration::ArmorIdFlag::InfantryIII; - } else if (label_id == 4) { - armor_id = enumeration::ArmorIdFlag::InfantryIV; - } else if (label_id == 5) { - armor_id = enumeration::ArmorIdFlag::InfantryV; - } else if (label_id == 6) { - armor_id = enumeration::ArmorIdFlag::Outpost; - } else if (label_id == 1) { - armor_id = enumeration::ArmorIdFlag::Hero; - } else if (label_id == 2) { - armor_id = enumeration::ArmorIdFlag::Engineer; - } else if (label_id == 0) { - armor_id = enumeration::ArmorIdFlag::Sentry; - } else if (label_id == 7) { - armor_id = enumeration::ArmorIdFlag::Base; - }; - - return armor_id; - } - - enumeration::ArmorIdFlag ovclassify(const cv::Mat& armor_pattern) { - if (armor_pattern.empty()) { - return enumeration::ArmorIdFlag::Unknow; - } - - cv::Mat gray; - cv::cvtColor(armor_pattern, gray, cv::COLOR_BGR2GRAY); - - // Resize image - auto input = cv::Mat(model_image_height_, model_image_width_, CV_8UC1, cv::Scalar(0)); - auto x_scale = static_cast(model_image_width_) / gray.cols; - auto y_scale = static_cast(model_image_height_) / gray.rows; - auto scale = std::min(x_scale, y_scale); - auto h = static_cast(gray.rows * scale); - auto w = static_cast(gray.cols * scale); - - if (h == 0 || w == 0) { - return enumeration::ArmorIdFlag::Unknow; - } - - auto roi = cv::Rect(0, 0, w, h); - cv::resize(gray, input(roi), { w, h }); - // Normalize the input image to [0, 1] range - input.convertTo(input, CV_32F, 1.0 / 255.0); - - ov::Tensor input_tensor(ov::element::f32, { 1, 1, 32, 32 }, input.data); - - ov::InferRequest infer_request = compiled_model_.create_infer_request(); - infer_request.set_input_tensor(input_tensor); - infer_request.infer(); - - auto output_tensor = infer_request.get_output_tensor(); - auto output_shape = output_tensor.get_shape(); - cv::Mat outputs(1, 9, CV_32F, output_tensor.data()); - - // Softmax - float max = *std::max_element(outputs.begin(), outputs.end()); - cv::exp(outputs - max, outputs); - float sum = cv::sum(outputs)[0]; - outputs /= sum; - - double confidence; - cv::Point label_point; - cv::minMaxLoc(outputs.reshape(1, 1), nullptr, &confidence, nullptr, &label_point); - int label_id = label_point.x; - enumeration::ArmorIdFlag armor_id; - - if (label_id == 3) { - armor_id = enumeration::ArmorIdFlag::InfantryIII; - } else if (label_id == 4) { - armor_id = enumeration::ArmorIdFlag::InfantryIV; - } else if (label_id == 5) { - armor_id = enumeration::ArmorIdFlag::InfantryV; - } else if (label_id == 6) { - armor_id = enumeration::ArmorIdFlag::Outpost; - } else if (label_id == 1) { - armor_id = enumeration::ArmorIdFlag::Hero; - } else if (label_id == 2) { - armor_id = enumeration::ArmorIdFlag::Engineer; - } else if (label_id == 0) { - armor_id = enumeration::ArmorIdFlag::Sentry; - } else if (label_id == 7) { - armor_id = enumeration::ArmorIdFlag::Base; - }; - - return armor_id; - }; - -private: - cv::dnn::Net net_; - ov::Core core_; - ov::CompiledModel compiled_model_; - - const int model_image_height_ = 640; - const int model_image_width_ = 640; -}; - -Classifier::Classifier(const std::string& model_path, int model_image_width, int model_image_height) - : pimpl_(std::make_unique(model_path, model_image_width, model_image_height)) { } -Classifier::~Classifier() = default; - -enumeration::ArmorIdFlag Classifier::classify(const cv::Mat& armor_pattern) { - return pimpl_->classify(armor_pattern); -} -enumeration::ArmorIdFlag Classifier::ovclassify(const cv::Mat& armor_pattern){ - return pimpl_->ovclassify(armor_pattern); -} -} \ No newline at end of file diff --git a/src/tongji/identifier/classifier.hpp b/src/tongji/identifier/classifier.hpp index 86080bc..47924f3 100644 --- a/src/tongji/identifier/classifier.hpp +++ b/src/tongji/identifier/classifier.hpp @@ -1,23 +1,153 @@ #pragma once -#include #include +#include +#include +#include +#include #include "enum/armor_id.hpp" -namespace world_exe::tongji::identifier { +namespace world_exe::tongji::identifier { class Classifier final { public: - Classifier(const std::string& model_path, int model_image_width, int model_image_height); - ~Classifier(); + explicit Classifier( + const std::string& model_path, int model_image_width, int model_image_height) + : model_image_width_(model_image_width) + , model_image_height_(model_image_height) { + net_ = cv::dnn::readNetFromONNX(model_path); + auto ovmodel = core_.read_model(model_path); + compiled_model_ = core_.compile_model( + ovmodel, "AUTO", ov::hint::performance_mode(ov::hint::PerformanceMode::LATENCY)); + } + + void Classify(const cv::Mat& armor_pattern, enumeration::ArmorIdFlag& armor_id, + double& armor_confidence) { + if (armor_pattern.empty()) { + armor_id = enumeration::ArmorIdFlag::Unknow; + } + + cv::Mat gray; + cv::cvtColor(armor_pattern, gray, cv::COLOR_BGR2GRAY); + + auto input = cv::Mat(model_image_height_, model_image_width_, CV_8UC1, cv::Scalar(0)); + auto x_scale = static_cast(model_image_width_) / gray.cols; + auto y_scale = static_cast(model_image_height_) / gray.rows; + auto scale = std::min(x_scale, y_scale); + auto h = static_cast(gray.rows * scale); + auto w = static_cast(gray.cols * scale); + + if (h == 0 || w == 0) { + armor_id = enumeration::ArmorIdFlag::Unknow; + } + auto roi = cv::Rect(0, 0, w, h); + cv::resize(gray, input(roi), { w, h }); + + auto blob = cv::dnn::blobFromImage(input, 1.0 / 255.0, cv::Size(), cv::Scalar()); + + net_.setInput(blob); + cv::Mat outputs = net_.forward(); + + // softmax + float max = *std::max_element(outputs.begin(), outputs.end()); + cv::exp(outputs - max, outputs); + float sum = cv::sum(outputs)[0]; + outputs /= sum; + + cv::Point label_point; + cv::minMaxLoc(outputs.reshape(1, 1), nullptr, &armor_confidence, nullptr, &label_point); + int label_id = label_point.x; + + if (label_id == 3) { + armor_id = enumeration::ArmorIdFlag::InfantryIII; + } else if (label_id == 4) { + armor_id = enumeration::ArmorIdFlag::InfantryIV; + } else if (label_id == 5) { + armor_id = enumeration::ArmorIdFlag::InfantryV; + } else if (label_id == 6) { + armor_id = enumeration::ArmorIdFlag::Outpost; + } else if (label_id == 1) { + armor_id = enumeration::ArmorIdFlag::Hero; + } else if (label_id == 2) { + armor_id = enumeration::ArmorIdFlag::Engineer; + } else if (label_id == 0) { + armor_id = enumeration::ArmorIdFlag::Sentry; + } else if (label_id == 7) { + armor_id = enumeration::ArmorIdFlag::Base; + }; + } - enumeration::ArmorIdFlag classify(const cv::Mat& armor_pattern); + void OvClassify(const cv::Mat& armor_pattern, enumeration::ArmorIdFlag& armor_id, + double& armor_confidence) { + if (armor_pattern.empty()) { + armor_id = enumeration::ArmorIdFlag::Unknow; + } - enumeration::ArmorIdFlag ovclassify(const cv::Mat& armor_pattern); + cv::Mat gray; + cv::cvtColor(armor_pattern, gray, cv::COLOR_BGR2GRAY); + + // Resize image + auto input = cv::Mat(model_image_height_, model_image_width_, CV_8UC1, cv::Scalar(0)); + auto x_scale = static_cast(model_image_width_) / gray.cols; + auto y_scale = static_cast(model_image_height_) / gray.rows; + auto scale = std::min(x_scale, y_scale); + auto h = static_cast(gray.rows * scale); + auto w = static_cast(gray.cols * scale); + + if (h == 0 || w == 0) { + armor_id = enumeration::ArmorIdFlag::Unknow; + } + + auto roi = cv::Rect(0, 0, w, h); + cv::resize(gray, input(roi), { w, h }); + // Normalize the input image to [0, 1] range + input.convertTo(input, CV_32F, 1.0 / 255.0); + + ov::Tensor input_tensor(ov::element::f32, { 1, 1, 32, 32 }, input.data); + + ov::InferRequest infer_request = compiled_model_.create_infer_request(); + infer_request.set_input_tensor(input_tensor); + infer_request.infer(); + + auto output_tensor = infer_request.get_output_tensor(); + auto output_shape = output_tensor.get_shape(); + cv::Mat outputs(1, 9, CV_32F, output_tensor.data()); + + // Softmax + float max = *std::max_element(outputs.begin(), outputs.end()); + cv::exp(outputs - max, outputs); + float sum = cv::sum(outputs)[0]; + outputs /= sum; + + cv::Point label_point; + cv::minMaxLoc(outputs.reshape(1, 1), nullptr, &armor_confidence, nullptr, &label_point); + int label_id = label_point.x; + + if (label_id == 3) { + armor_id = enumeration::ArmorIdFlag::InfantryIII; + } else if (label_id == 4) { + armor_id = enumeration::ArmorIdFlag::InfantryIV; + } else if (label_id == 5) { + armor_id = enumeration::ArmorIdFlag::InfantryV; + } else if (label_id == 6) { + armor_id = enumeration::ArmorIdFlag::Outpost; + } else if (label_id == 1) { + armor_id = enumeration::ArmorIdFlag::Hero; + } else if (label_id == 2) { + armor_id = enumeration::ArmorIdFlag::Engineer; + } else if (label_id == 0) { + armor_id = enumeration::ArmorIdFlag::Sentry; + } else if (label_id == 7) { + armor_id = enumeration::ArmorIdFlag::Base; + }; + }; private: - class Impl; - std::unique_ptr pimpl_; -}; + cv::dnn::Net net_; + ov::Core core_; + ov::CompiledModel compiled_model_; + const int model_image_height_ = 640; + const int model_image_width_ = 640; +}; } \ No newline at end of file diff --git a/src/tongji/identifier/identifier.cpp b/src/tongji/identifier/identifier.cpp index e69de29..2705d6f 100644 --- a/src/tongji/identifier/identifier.cpp +++ b/src/tongji/identifier/identifier.cpp @@ -0,0 +1,345 @@ +#include "identifier.hpp" + +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +#include "data/armor_image_spaceing.hpp" +#include "enum/armor_id.hpp" +#include "identifier_armor.hpp" +#include "interfaces/armor_in_image.hpp" +#include "tongji/identifier/classifier.hpp" +#include "util/logger.hpp" +#include "util/stringifier.hpp" + +namespace world_exe::tongji::identifier { + +class Identifier::Impl { +public: + explicit Impl(const std::string& model_path, const int& model_image_width, + const int& model_image_height, const double& threshold, const double& max_angle_error, + const double& min_lightbar_ratio, const double& max_lightbar_ratio, + const double& min_lightbar_length, const double& max_armor_ratio, + const double& min_armor_ratio, const double& max_side_ratio, const double& min_confidence, + const double& max_rectangular_error, const std::string& save_path, const bool& debug = true) + : classifier_(model_path, model_image_width, model_image_height) + , threshold_(threshold) + , max_angle_error_(max_angle_error / 57.3) // degree to rad + , min_lightbar_ratio_(min_lightbar_ratio) + , max_lightbar_ratio_(max_lightbar_ratio) + , min_lightbar_length_(min_lightbar_length) + , max_armor_ratio_(max_armor_ratio) + , min_armor_ratio_(min_armor_ratio) + , max_side_ratio_(max_side_ratio) + , min_confidence_(min_confidence) + , max_rectangular_error_(max_rectangular_error / 57.3) // degree to rad + , save_path_(std::move(save_path)) + , debug_(debug) { + + if (!std::filesystem::exists(save_path_)) std::filesystem::create_directories(save_path_); + } + + void SetTargetColor(bool target_color) { target_color_ = target_color; } + + const std::tuple, enumeration::CarIDFlag> + Identify(const cv::Mat& bgr_img) { + // 彩色图转灰度图 + cv::Mat gray_img; + cv::cvtColor(bgr_img, gray_img, cv::COLOR_BGR2GRAY); + + // 进行二值化 + cv::Mat binary_img; + cv::threshold(gray_img, binary_img, threshold_, 255, cv::THRESH_BINARY); + cv::imshow("binary_img", binary_img); + + // 获取轮廓点 + std::vector> contours; + cv::findContours(binary_img, contours, cv::RETR_EXTERNAL, cv::CHAIN_APPROX_NONE); + + // 获取灯条 + std::size_t lightbar_id = 0; + std::list lightbars; + + for (const auto& contour : contours) { + auto rotated_rect = cv::minAreaRect(contour); + auto lightbar = Lightbar(rotated_rect, lightbar_id); + + if (!CheckGeometry(lightbar)) continue; + + lightbar.color = GetColor(bgr_img, contour); + lightbars.emplace_back(lightbar); + lightbar_id += 1; + } + + // 将灯条从左到右排序 + lightbars.sort( + [](const Lightbar& a, const Lightbar& b) { return a.center.x < b.center.x; }); + + struct ArmorCandidate { + Lightbar left, right; + cv::Mat pattern; + enumeration::ArmorIdFlag id = enumeration::ArmorIdFlag::Unknow; + double confidence; + bool duplicated = false; + + explicit ArmorCandidate(const Lightbar& left, const Lightbar& right) + : left(left) + , right(right) { } + }; + + // 获取装甲板 + std::list armor_candidates; + for (auto left = lightbars.begin(); left != lightbars.end(); left++) { + for (auto right = std::next(left); right != lightbars.end(); right++) { + + if (left->color != right->color) continue; + + auto armor_candidate = ArmorCandidate(*left, *right); + + auto left2right = right->center - left->center; + auto width = cv::norm(left2right); + auto max_lightbar_length = std::max(left->length, right->length); + auto min_lightbar_length = std::min(left->length, right->length); + + auto armor_ratio = + width / max_lightbar_length; // 两灯条的中点连线与较长灯条的长度之比 + auto armor_side_ratio = + max_lightbar_length / min_lightbar_length; // 长灯条与短灯条的长度之比 + + auto roll = std::atan2(left2right.y, left2right.x); + auto left_rectangular_error = std::abs(left->angle - roll - M_PI / 2); + auto right_rectangular_error = std::abs(right->angle - roll - CV_PI / 2); + auto armor_rectangular_error = std::max(left_rectangular_error, + right_rectangular_error); // 灯条和中点连线所成夹角与π/2的差值 + + if (!CheckGeometry(armor_ratio, armor_side_ratio, armor_rectangular_error)) + continue; + + armor_candidate.pattern = GetPattern(bgr_img, *left, *right); + + classifier_.Classify( + armor_candidate.pattern, armor_candidate.id, armor_candidate.confidence); + + if (!CheckName( + armor_candidate.id, armor_candidate.confidence, armor_candidate.pattern)) + continue; + + auto is_large = IsLargeArmor(armor_ratio, armor_candidate.id); + if (!CheckType(armor_candidate.id, !is_large, armor_candidate.pattern)) continue; + + auto armor_center = + (left->center + right->center) / 2.; // 不是对角线交点，不能作为实际中心！ + auto armor_center_norm = GetCenterNorm(bgr_img, armor_center); + armor_candidates.emplace_back(armor_candidate); + } + } + + // 检查装甲板是否存在共用灯条的情况 + for (auto armor1 = armor_candidates.begin(); armor1 != armor_candidates.end(); armor1++) { + for (auto armor2 = std::next(armor1); armor2 != armor_candidates.end(); armor2++) { + if (armor1->left.id != armor2->left.id && armor1->left.id != armor2->right.id + && armor1->right.id != armor2->left.id + && armor1->right.id != armor2->right.id) { + continue; + } + + // 装甲板重叠, 保留roi小的 + if (armor1->left.id == armor2->left.id || armor1->right.id == armor2->right.id) { + auto area1 = armor1->pattern.cols * armor1->pattern.rows; + auto area2 = armor2->pattern.cols * armor2->pattern.rows; + if (area1 < area2) armor2->duplicated = true; + else armor1->duplicated = true; + } + + // 装甲板相连，保留置信度大的 + if (armor1->left.id == armor2->right.id || armor1->right.id == armor2->left.id) { + if (armor1->confidence < armor2->confidence) armor1->duplicated = true; + else armor2->duplicated = true; + } + } + } + + armor_candidates.remove_if([&](const ArmorCandidate& a) { return a.duplicated; }); + armor_candidates.remove_if( + [&](const ArmorCandidate& a) { return a.left.color != target_color_; }); + + std::vector armor_plates; + uint32_t all_car_id = static_cast(enumeration::ArmorIdFlag::None); + for (const auto& armor : armor_candidates) { + armor_plates.emplace_back(data::ArmorImageSpacing { armor.id, + { + armor.left.top, + armor.right.top, + armor.right.bottom, + armor.left.bottom, + } }); + + all_car_id |= static_cast(armor.id); + } + + return { std::make_shared(armor_plates), + static_cast(all_car_id) }; + } + +private: + void Save(const cv::Mat& armor_pattern, enumeration::ArmorIdFlag armor_id) const { + auto file_name = fmt::format("{:%Y-%m-%d_%H-%M-%S}", std::chrono::system_clock::now()); + auto img_path = fmt::format( + "{}/{}_{}.jpg", save_path_, util::stringifier::ToString(armor_id), file_name); + cv::imwrite(img_path, armor_pattern); + } + + Color GetColor(const cv::Mat& bgr_img, const std::vector& contour) const { + int red_sum = 0, blue_sum = 0; + + for (const auto& point : contour) { + red_sum += bgr_img.at(point)[2]; + blue_sum += bgr_img.at(point)[0]; + } + + return blue_sum > red_sum ? Color::blue : Color::red; + } + + cv::Mat GetPattern(const cv::Mat& bgr_img, const Lightbar& left_lightbar, + const Lightbar& right_lightbar) const { + // 延长灯条获得装甲板角点 + // 1.125 = 0.5 * armor_height / lightbar_length = 0.5 * 126mm / 56mm + auto tl = left_lightbar.center - left_lightbar.top2bottom * 1.125; + auto bl = left_lightbar.center + left_lightbar.top2bottom * 1.125; + auto tr = right_lightbar.center - right_lightbar.top2bottom * 1.125; + auto br = right_lightbar.center + right_lightbar.top2bottom * 1.125; + + auto roi_left = std::max(std::min(tl.x, bl.x), 0); + auto roi_top = std::max(std::min(tl.y, tr.y), 0); + auto roi_right = std::min(std::max(tr.x, br.x), bgr_img.cols); + auto roi_bottom = std::min(std::max(bl.y, br.y), bgr_img.rows); + auto roi_tl = cv::Point(roi_left, roi_top); + auto roi_br = cv::Point(roi_right, roi_bottom); + auto roi = cv::Rect(roi_tl, roi_br); + + return bgr_img(roi); + } + + cv::Point2f GetCenterNorm(const cv::Mat& bgr_img, const cv::Point2f& center) const { + auto h = bgr_img.rows; + auto w = bgr_img.cols; + return { center.x / w, center.y / h }; + } + + bool IsLargeArmor(double armor_ratio, enumeration::ArmorIdFlag armor_id) const { + /// 优先根据当前armor.ratio判断 + /// TODO: 26赛季是否还需要根据比例判断大小装甲？能否根据图案直接判断？ + + if (armor_ratio > 3.0) { + // util::logger::logger()->debug("[Identifier] get armor type by ratio: LARGE {} with " + // "ratio :{:.2f}", + // util::stringifier::ToString(armor_id), armor_ratio); + return true; + } + + if (armor_ratio < 2.5) { + // util::logger::logger()->debug("[Identifier] get armor type by ratio: NORMAL {} with " + // "ratio: {:.2f}", + // util::stringifier::ToString(armor_id), armor_ratio); + return false; + } + + // util::logger::logger()->debug( + // "[Detector] get armor type by name: {}", util::stringifier::ToString(armor_id)); + + // 英雄、基地只能是大装甲板 + if (armor_id == enumeration::ArmorIdFlag::Hero + || armor_id == enumeration::ArmorIdFlag::Base) { + return true; + } + + // 其他所有（工程、哨兵、前哨站、步兵）都是小装甲板 + /// TODO: 基地顶装甲是小装甲板 + return false; + } + + bool CheckGeometry(const Lightbar& lightbar) const { + auto angle_ok = lightbar.angle_error < max_angle_error_; + auto ratio_ok = + lightbar.ratio > min_lightbar_ratio_ && lightbar.ratio < max_lightbar_ratio_; + auto length_ok = lightbar.length > min_lightbar_length_; + return angle_ok && ratio_ok && length_ok; + } + + bool CheckGeometry(const double& armor_ratio, const double& armor_side_ratio, + const double& armor_rectangular_error) const { + + auto ratio_ok = armor_ratio > min_armor_ratio_ && armor_ratio < max_armor_ratio_; + auto side_ratio_ok = armor_side_ratio < max_side_ratio_; + auto rectangular_error_ok = armor_rectangular_error < max_rectangular_error_; + + return ratio_ok && side_ratio_ok && rectangular_error_ok; + } + + bool CheckName(const enumeration::ArmorIdFlag& armor_id, const double& armor_confidence, + const cv::Mat& armor_pattern) const { + auto name_ok = (armor_id != enumeration::ArmorIdFlag::Unknow); + auto confidence_ok = armor_confidence > min_confidence_; + + // 保存不确定的图案，用于分类器的迭代 + if (name_ok && !confidence_ok) Save(armor_pattern, armor_id); + + // 出现 5号 则显示 debug 信息。但不过滤。 + if (armor_id == enumeration::ArmorIdFlag::InfantryV) + world_exe::util::logger::logger()->debug("See pattern 5 : InfantryV "); + + return name_ok && confidence_ok; + } + + bool CheckType(const enumeration::ArmorIdFlag& armor_id, bool is_normal_armor, + const cv::Mat& armor_pattern) const { + auto name_ok = is_normal_armor ? (armor_id != enumeration::ArmorIdFlag::Hero + && armor_id != enumeration::ArmorIdFlag::Base) + : (armor_id == enumeration::ArmorIdFlag::Hero + || armor_id == enumeration::ArmorIdFlag::Base); + + // 保存异常的图案，用于分类器的迭代 + if (!name_ok) { + util::logger::logger()->debug( + "see strange armor: {}", util::stringifier::ToString(armor_id)); + Save(armor_pattern, armor_id); + } + return name_ok; + } + +private: + Classifier classifier_; + + double threshold_; + double max_angle_error_; + double min_lightbar_ratio_, max_lightbar_ratio_; + double min_lightbar_length_; + double min_armor_ratio_, max_armor_ratio_; + double max_side_ratio_; + double min_confidence_; + double max_rectangular_error_; + + bool target_color_ = true; // true: red, false: blue + + bool debug_; + std::string save_path_; +}; + +const std::tuple, enumeration::CarIDFlag> +Identifier::identify(const cv::Mat& input_image) { + return pimpl_->Identify(input_image); +} + +void Identifier::SetTargetColor(Color target_color) { return pimpl_->SetTargetColor(target_color); } +} \ No newline at end of file diff --git a/src/tongji/identifier/identifier.hpp b/src/tongji/identifier/identifier.hpp index e69de29..01e73a4 100644 --- a/src/tongji/identifier/identifier.hpp +++ b/src/tongji/identifier/identifier.hpp @@ -0,0 +1,55 @@ +#pragma once + +#include "interfaces/identifier.hpp" + +namespace world_exe::tongji::identifier { + +enum Color { red, blue, extinguish, purple }; + +struct Lightbar { + std::size_t id; + Color color; + cv::Point2f center, top, bottom, top2bottom; + std::vector points; + double angle, angle_error, length, width, ratio; + cv::RotatedRect rotated_rect; + + Lightbar(const cv::RotatedRect& rotated_rect, std::size_t id) + : id(id) + , rotated_rect(rotated_rect) { + std::vector corners(4); + rotated_rect.points(&corners[0]); + std::sort(corners.begin(), corners.end(), + [](const cv::Point2f& a, const cv::Point2f& b) { return a.y < b.y; }); + + this->center = rotated_rect.center; + this->top = (corners[0] + corners[1]) / 2.; + this->bottom = (corners[2] + corners[3]) / 2.; + this->top2bottom = bottom - top; + + this->points.emplace_back(top); + this->points.emplace_back(bottom); + + this->width = cv::norm(corners[0] - corners[1]); + this->angle = std::atan2(top2bottom.y, top2bottom.x); + this->angle_error = std::abs(angle - M_PI / 2); + this->length = cv::norm(top2bottom); + this->ratio = length / width; + } +}; +class Identifier final : public interfaces::IIdentifier { +public: + Identifier(const std::string& model_path, int model_image_width, int model_image_height); + ~Identifier(); + + const std::tuple, enumeration::CarIDFlag> + identify(const cv::Mat& input_image) override; + + void SetTargetColor(Color target_color); + +private: + class Impl; + std::unique_ptr pimpl_; +}; + +} \ No newline at end of file diff --git a/src/tongji/identifier/identifier_armor.hpp b/src/tongji/identifier/identifier_armor.hpp new file mode 100644 index 0000000..ec94241 --- /dev/null +++ b/src/tongji/identifier/identifier_armor.hpp @@ -0,0 +1,36 @@ +#pragma once + +#include "enum/armor_id.hpp" +#include "interfaces/armor_in_image.hpp" +#include "interfaces/time_stamped.hpp" +#include "util/index.hpp" + +namespace world_exe::tongji::identifier { +class IdentifierArmor final : public interfaces::IArmorInImage, public interfaces::ITimeStamped { +public: + explicit IdentifierArmor(const std::vector& armors) { + SetArmors(armors); + } + + const interfaces::ITimeStamped& GetTimeStamped() const override { return *this; } + + const std::time_t& GetTimeStamp() const override { return time_stamp_; }; + + void SetArmors(const std::vector& armors) { + std::array, 8> temp_armors; + for (const auto& armor : armors) { + temp_armors[util::enumeration::GetIndex(armor.id)].emplace_back(armor); + } + armors_ = std::move(temp_armors); + } + + const std::vector& GetArmors( + const enumeration::ArmorIdFlag& armor_id) const override { + return armors_[util::enumeration::GetIndex(armor_id)]; + } + +private: + std::time_t time_stamp_ { 0 }; + std::array, 8> armors_; +}; +} \ No newline at end of file From 541f531b9ebe26544cb84ea8d0a5937e1cf9c679 Mon Sep 17 00:00:00 2001 From: heyeuu <2829004293@qq.com> Date: Thu, 4 Sep 2025 04:26:25 +0800 Subject: [PATCH 10/53] fix(utils): correct incomplete namespace in stringifier module --- src/util/stringifier.hpp | 20 ++++++++++---------- 1 file changed, 10 insertions(+), 10 deletions(-) diff --git a/src/util/stringifier.hpp b/src/util/stringifier.hpp index 0953582..0fe4974 100644 --- a/src/util/stringifier.hpp +++ b/src/util/stringifier.hpp @@ -8,25 +8,25 @@ namespace world_exe::util::stringifier { static inline std::string ToString(const world_exe::enumeration::ArmorIdFlag& id) { switch (id) { - case enumeration::ArmorIdFlag::Hero: + case world_exe::enumeration::ArmorIdFlag::Hero: return "Hero"; - case enumeration::ArmorIdFlag::Engineer: + case world_exe::enumeration::ArmorIdFlag::Engineer: return "Engineer"; - case enumeration::ArmorIdFlag::InfantryIII: + case world_exe::enumeration::ArmorIdFlag::InfantryIII: return "InfantryIII"; - case enumeration::ArmorIdFlag::InfantryIV: + case world_exe::enumeration::ArmorIdFlag::InfantryIV: return "InfantryIV"; - case enumeration::ArmorIdFlag::InfantryV: + case world_exe::enumeration::ArmorIdFlag::InfantryV: return "InfantryV"; - case enumeration::ArmorIdFlag::Sentry: + case world_exe::enumeration::ArmorIdFlag::Sentry: return "Sentry"; - case enumeration::ArmorIdFlag::Base: + case world_exe::enumeration::ArmorIdFlag::Base: return "Base"; - case enumeration::ArmorIdFlag::Outpost: + case world_exe::enumeration::ArmorIdFlag::Outpost: return "Outpost"; - case enumeration::ArmorIdFlag::Unknow: + case world_exe::enumeration::ArmorIdFlag::Unknow: return "Unknow"; - case enumeration::ArmorIdFlag::None: + case world_exe::enumeration::ArmorIdFlag::None: return "None"; default: return "UnknownID"; From 4304b8c758c3f697785a0f3a87ab1e4095db3857 Mon Sep 17 00:00:00 2001 From: heyeuu <2829004293@qq.com> Date: Thu, 4 Sep 2025 08:25:49 +0800 Subject: [PATCH 11/53] refactor(solver): rewrite interface implementation and add index-to-id function --- src/tongji/solver/solver.cpp | 78 ++++++++---------------------- src/tongji/solver/solver.hpp | 1 - src/tongji/solver/solver_armor.hpp | 39 +++++++++++++++ src/util/index.hpp | 9 ++++ 4 files changed, 67 insertions(+), 60 deletions(-) create mode 100644 src/tongji/solver/solver_armor.hpp diff --git a/src/tongji/solver/solver.cpp b/src/tongji/solver/solver.cpp index 658841e..2213d5a 100644 --- a/src/tongji/solver/solver.cpp +++ b/src/tongji/solver/solver.cpp @@ -1,55 +1,19 @@ #include "solver.hpp" -#include -#include +#include #include #include -#include -#include #include "data/armor_camera_spacing.hpp" #include "data/armor_gimbal_control_spacing.hpp" -#include "data/armor_image_spaceing.hpp" -#include "enum/armor_id.hpp" -#include "interfaces/armor_in_camera.hpp" -#include "interfaces/time_stamped.hpp" #include "parameters/profile.hpp" #include "parameters/rm_parameters.hpp" +#include "solver_armor.hpp" #include "util/coordinate.hpp" #include "util/index.hpp" #include "util/math.hpp" namespace world_exe::tongji::solver { -class ArmorInCameraImpl final : public interfaces::IArmorInCamera, public interfaces::ITimeStamped { -public: - void AddArmor(const data::ArmorCameraSpacing& armor) { - int index = util::enumeration::GetIndex(armor.id); - armors_.at(index).push_back(armor); - } - - void SetTimeStamp(const std::time_t& timestamp) { timestamp_ = timestamp; } - - const std::vector& GetArmors( - const enumeration::ArmorIdFlag& armor_id) const override { - static const std::vector empty_vector; - try { - int index = util::enumeration::GetIndex(armor_id); - return armors_.at(index); - } catch (const std::exception& e) { - return empty_vector; - } - } - - const std::time_t& GetTimeStamp() const override { return timestamp_; } - - const interfaces::ITimeStamped& GetTimeStamped() const override { return *this; } - -private: - std::vector> armors_ { static_cast( - enumeration::ArmorIdFlag::Count) }; - std::time_t timestamp_ = 0; -}; - class Solver::Impl { public: Impl(Eigen::Matrix3d R_camera2gimbal, Eigen::Matrix3d R_gimbal2world, @@ -59,24 +23,20 @@ class Solver::Impl { , t_camera2gimbal_(t_camera2gimbal) { } std::shared_ptr SolvePnp( - std::shared_ptr armors_in_image) const { - - auto result = std::make_shared(); - result->SetTimeStamp(armors_in_image->GetTimeStamped().GetTimeStamp()); - - for (int i = 0; i < static_cast(enumeration::ArmorIdFlag::Count); ++i) { - const auto armor_id = static_cast(1 << i); - const auto& image_armors = armors_in_image->GetArmors(armor_id); - - for (const auto& armor_image : image_armors) { - const auto armor_in_camera_optional = Solve(armor_image); - if (armor_in_camera_optional.has_value()) { - result->AddArmor(armor_in_camera_optional.value()); + std::shared_ptr armors_in_image) { + std::vector armor_plates; + + for (int i = 0; i < static_cast(enumeration::ArmorIdFlag::Count); i++) { + const auto& armor_id = util::enumeration::GetArmorIdFlag(i); + const auto& armors = armors_in_image->GetArmors(armor_id); + for (const auto& armor : armors) { + auto solved_armor = Solve(armor); + if (solved_armor.has_value()) { + armor_plates.emplace_back(solved_armor.value()); } } } - - return result; + return std::make_shared(armor_plates); } std::optional Solve( @@ -167,6 +127,8 @@ class Solver::Impl { return result; } + const std::time_t& GetTimeStamp() const { return time_stamp_; } + private: std::vector ReprojectArmor(const Eigen::Vector3d& xyz_in_world, const double& yaw, const bool& is_large, const enumeration::ArmorIdFlag& id) const { @@ -332,21 +294,19 @@ class Solver::Impl { Eigen::Matrix3d R_gimbal2world_; Eigen::Vector3d t_camera2gimbal_; inline constexpr static const double MaxArmorDistance = 15.0; + + std::time_t time_stamp_ { 0 }; }; Solver::Solver(Eigen::Matrix3d R_camera2gimbal, Eigen::Matrix3d R_gimbal2world, Eigen::Vector3d t_camera2gimbal) - : pimpl_(std::make_unique(R_camera2gimbal, R_gimbal2world, t_camera2gimbal)) - , last_processed_time_(0) { } + : pimpl_(std::make_unique(R_camera2gimbal, R_gimbal2world, t_camera2gimbal)) { } Solver::~Solver() = default; -const std::time_t& Solver::GetTimeStamp() const { return last_processed_time_; } - -class ArmorInCameraImpl; +const std::time_t& Solver::GetTimeStamp() const { return pimpl_->GetTimeStamp(); } std::shared_ptr Solver::SolvePnp( std::shared_ptr armors_in_image) { - last_processed_time_ = armors_in_image->GetTimeStamped().GetTimeStamp(); return pimpl_->SolvePnp(armors_in_image); } diff --git a/src/tongji/solver/solver.hpp b/src/tongji/solver/solver.hpp index b3a1177..5f6c2f4 100644 --- a/src/tongji/solver/solver.hpp +++ b/src/tongji/solver/solver.hpp @@ -19,7 +19,6 @@ class Solver final : public interfaces::IPnpSolver, public interfaces::ITimeStam private: class Impl; std::unique_ptr pimpl_; - std::time_t last_processed_time_; }; } \ No newline at end of file diff --git a/src/tongji/solver/solver_armor.hpp b/src/tongji/solver/solver_armor.hpp new file mode 100644 index 0000000..1e8145b --- /dev/null +++ b/src/tongji/solver/solver_armor.hpp @@ -0,0 +1,39 @@ +#pragma once + +#include +#include + +#include "interfaces/armor_in_camera.hpp" +#include "interfaces/time_stamped.hpp" +#include "util/index.hpp" + +namespace world_exe::tongji::solver { +class SolverArmor final : public interfaces::IArmorInCamera, public interfaces::ITimeStamped { +public: + explicit SolverArmor(const std::vector& armors) { + SetArmors(armors); + time_stamp_ = 0; + } + + const interfaces::ITimeStamped& GetTimeStamped() const override { return *this; } + + const std::time_t& GetTimeStamp() const override { return time_stamp_; }; + + void SetArmors(const std::vector& armors) { + std::array, 8> temp_armors; + for (const auto& armor : armors) { + temp_armors[util::enumeration::GetIndex(armor.id)].emplace_back(armor); + } + armors_ = std::move(temp_armors); + } + + const std::vector& GetArmors( + const enumeration::ArmorIdFlag& armor_id) const override { + return armors_[util::enumeration::GetIndex(armor_id)]; + } + +private: + std::time_t time_stamp_ { 0 }; + std::array, 8> armors_; +}; +} \ No newline at end of file diff --git a/src/util/index.hpp b/src/util/index.hpp index 483cb93..350fe26 100644 --- a/src/util/index.hpp +++ b/src/util/index.hpp @@ -22,4 +22,13 @@ static inline int GetIndex(const world_exe::enumeration::ArmorIdFlag& flag) { throw std::runtime_error("Invalid ArmorIdFlag value: " + std::to_string(value)); } } + +static const world_exe::enumeration::ArmorIdFlag GetArmorIdFlag(int index) { + if (index >= 0 && index < static_cast(world_exe::enumeration::ArmorIdFlag::Count)) { + uint32_t value = 1U << index; + return static_cast(value); + } else { + throw std::out_of_range("Invalid index for ArmorIdFlag conversion."); + } +} } \ No newline at end of file From 43109dcf873681c16529b9b35f2f4a820c7e9c82 Mon Sep 17 00:00:00 2001 From: fan Date: Fri, 5 Sep 2025 01:20:35 +0800 Subject: [PATCH 12/53] add comment --- src/util/scanline.hpp | 88 +++++++++++++++------- src/v1/predictor/car/car_predictor_ekf.hpp | 10 +-- src/v1/predictor/predictor_manager.cpp | 2 + 3 files changed, 67 insertions(+), 33 deletions(-) diff --git a/src/util/scanline.hpp b/src/util/scanline.hpp index 32185c3..2cd5fa3 100644 --- a/src/util/scanline.hpp +++ b/src/util/scanline.hpp @@ -6,6 +6,17 @@ namespace world_exe::util { class ScanLine { public: + /** + * @brief 获取多边形轮廓内部的所有像素点 + * + * 该函数使用扫描线算法填充多边形，返回轮廓内部的所有像素点坐标。 + * + * @param image 输入图像，用于确定图像边界 + * @param contour 多边形轮廓点的集合 + * @return std::vector 返回多边形内部所有像素点的坐标集合 + * + * @note 如果轮廓为空，则返回空向量 + */ static inline std::vector get_points( const cv::Mat& image, const std::vector& contour) { if (contour.empty()) return {}; @@ -15,27 +26,43 @@ class ScanLine { for (const auto& point : contour) points.emplace_back(point); + // 收集多边形的所有边 auto edges = collect_poly_edges(points); + // 使用边表填充算法获取内部点 return fill_edge_collection(image, edges); } private: struct PolyEdge { PolyEdge() - : y0(0) - , y1(0) - , x(0) - , dx(0) - , next(nullptr) { } - - int y0, y1; - int64 x, dx; - PolyEdge* next; + : y0(0) // 边的起始Y坐标 + , y1(0) // 边的结束Y坐标 + , x(0) // 当前X坐标（定点数） + , dx(0) // X方向的增量（定点数） + , next(nullptr) { } // 指向下一条边的指针 + + int y0, y1; // 边的Y坐标范围 + int64 x, dx; // X坐标和X增量（使用定点数提高精度） + PolyEdge* next; // 链表指针 }; + /** + * @brief 定点数相关常量定义 + * + * XY_SHIFT: 定点数位移量，用于提高坐标计算精度 + * XY_ONE: 定点数的1.0表示 + * DRAWING_STORAGE_BLOCK: 绘制存储块大小 + */ enum { XY_SHIFT = 16, XY_ONE = 1 << XY_SHIFT, DRAWING_STORAGE_BLOCK = (1 << 12) - 256 }; + /** + * @details 算法流程： + * 1. 计算边界框并进行边界检查 + * 2. 按Y坐标对边进行排序 + * 3. 逐行扫描，维护活动边表 + * 4. 在每条扫描线上，计算交点并填充像素 + */ static inline std::vector fill_edge_collection( const cv::Mat& img, std::vector& edges) { PolyEdge tmp; @@ -47,11 +74,11 @@ class ScanLine { if (total < 2) return {}; + // 计算边界框 for (i = 0; i < total; i++) { PolyEdge& e1 = edges[i]; assert(e1.y0 < e1.y1); - // Determine x-coordinate of the end of the edge. - // (This is not necessary x-coordinate of any vertex in the array.) + // 计算边末端的X坐标（不一定是顶点的X坐标） int64 x1 = e1.x + (e1.y1 - e1.y0) * e1.dx; y_min = std::min(y_min, e1.y0); y_max = std::max(y_max, e1.y1); @@ -61,45 +88,48 @@ class ScanLine { x_max = std::max(x_max, x1); } + // 边界检查：如果多边形完全在图像外部，直接返回 if (y_max < 0 || y_min >= size.height || x_max < 0 || x_min >= ((int64)size.width << XY_SHIFT)) return {}; + // 按Y坐标排序边，Y相同时按X坐标排序，X也相同时按斜率排序 std::sort(edges.begin(), edges.end(), [](const PolyEdge& e1, const PolyEdge& e2) { return e1.y0 - e2.y0 ? e1.y0 < e2.y0 : e1.x - e2.x ? e1.x < e2.x : e1.dx < e2.dx; }); tmp.y0 = std::numeric_limits::max(); - edges.push_back(tmp); // after this point we do not add - // any elements to edges, thus we can use pointers + edges.push_back(tmp); // 添加哨兵边，此后不再向edges添加元素，可以安全使用指针 i = 0; tmp.next = nullptr; e = &edges[i]; y_max = MIN(y_max, size.height); std::vector points; + // 逐行扫描 for (y = e->y0; y < y_max; y++) { PolyEdge *last, *prelast, *keep_prelast; int sort_flag = 0; - int draw = 0; - int clipline = y < 0; + int draw = 0; // 绘制标志，奇偶规则 + int clipline = y < 0; // 当前行是否在图像外部 + // 维护活动边表 prelast = &tmp; last = tmp.next; while (last || e->y0 == y) { if (last && last->y1 == y) { - // exclude edge if y reaches its lower point + // 如果边到达下端点，从活动边表中移除 prelast->next = last->next; last = last->next; continue; } keep_prelast = prelast; if (last && (e->y0 > y || last->x < e->x)) { - // go to the next edge in active list + // 移动到活动边表中的下一条边 prelast = last; last = last->next; } else if (i < total) { - // insert new edge into active list if y reaches its upper point + // 如果到达边的上端点，将新边插入活动边表 prelast->next = e; e->next = last; prelast = e; @@ -108,7 +138,7 @@ class ScanLine { if (draw) { if (!clipline) { - // convert x's from fixed-point to image coordinates + // 将定点数X坐标转换为图像坐标 int x1, x2; if (keep_prelast->x > prelast->x) { @@ -119,7 +149,7 @@ class ScanLine { x2 = (int)(prelast->x >> XY_SHIFT); } - // clip and draw the line + // 裁剪并绘制线段 if (x1 < size.width && x2 >= 0) { if (x1 < 0) x1 = 0; if (x2 >= size.width) x2 = size.width - 1; @@ -127,13 +157,14 @@ class ScanLine { points.emplace_back(i, y); } } + // 更新边的X坐标 keep_prelast->x += keep_prelast->dx; prelast->x += prelast->dx; } - draw ^= 1; + draw ^= 1; // 切换绘制状态（奇偶规则） } - // sort edges (using bubble sort) + // 对活动边表中的边按X坐标排序（使用冒泡排序） keep_prelast = 0; do { @@ -143,7 +174,7 @@ class ScanLine { while (last != keep_prelast && last->next != 0) { PolyEdge* te = last->next; - // swap edges + // 交换边的位置 if (last->x > te->x) { prelast->next = te; last->next = te->next; @@ -164,23 +195,27 @@ class ScanLine { static inline std::vector collect_poly_edges(const std::vector& points) { const auto points_size = points.size(); cv::Point2l pt0 = points[points_size - 1], pt1; - pt0.x = (pt0.x) << XY_SHIFT; + pt0.x = (pt0.x) << XY_SHIFT; // 转换为定点数 std::vector edges; edges.reserve(edges.size() + points_size); + + // 遍历所有相邻顶点对，构建边 for (std::size_t i = 0; i < points_size; i++, pt0 = pt1) { cv::Point2l t0, t1; pt1 = points[i]; - pt1.x = (pt1.x) << XY_SHIFT; + pt1.x = (pt1.x) << XY_SHIFT; // 转换为定点数 t0.x = pt0.x; t1.x = pt1.x; t0.y = pt0.y << XY_SHIFT; t1.y = pt1.y << XY_SHIFT; + // 跳过水平边 if (pt0.y == pt1.y) continue; PolyEdge edge; + // 确保边的方向统一：y0 < y1 if (pt0.y < pt1.y) { edge.y0 = static_cast(pt0.y); edge.y1 = static_cast(pt1.y); @@ -190,10 +225,11 @@ class ScanLine { edge.y1 = static_cast(pt0.y); edge.x = pt1.x; } + // 计算X方向的增量（斜率的倒数） edge.dx = (pt1.x - pt0.x) / (pt1.y - pt0.y); edges.emplace_back(edge); } return edges; } }; -} +} // namespace world_exe::util diff --git a/src/v1/predictor/car/car_predictor_ekf.hpp b/src/v1/predictor/car/car_predictor_ekf.hpp index 029551c..bf82f85 100644 --- a/src/v1/predictor/car/car_predictor_ekf.hpp +++ b/src/v1/predictor/car/car_predictor_ekf.hpp @@ -12,17 +12,13 @@ namespace world_exe::v1::predictor { * @class CarPredictEkf * @brief 车辆预测EKF核心类 * - * 状态向量维度: 11维 [x, vx, y, vy, z, z1, z2, r1, r2, θ, ω] + * 状态向量维度: 11维 [x, vx, y, vy, z, z1, z2, r1, r2, θ, ω]，其中 θ为最近装甲板与车辆中心连线与选定系的x轴的夹角 * 观测向量维度: 4维 [θ, yaw, pitch, distance] * * 这个是整个预测的核心，无论是逻辑的维护还是预测的模型都放在这里了，然后其他的主要是为了接口实现 */ class CarPredictEkf : public world_exe::util::Ekf<11, 4, CarPredictEkf> { public: - /** - * @brief 构造函数，初始化状态向量和协方差矩阵 - * 状态: [x=2.0, vx=0, y=0, vy=0, z=-0.3, z1=-0.3, z2=-0.3, r1=0.2, r2=0.2, θ=π, ω=0] - */ CarPredictEkf() { X_k << 2.0, 0.0, 0.0, 0.0, -0.3, -0.3, -0.3, 0.2, 0.2, std::numbers::pi, 0.0; @@ -37,7 +33,7 @@ class CarPredictEkf : public world_exe::util::Ekf<11, 4, CarPredictEkf> { inline void set_second_armor() { second_armor_flag = true; } /** - * @brief 获取预测的装甲板位置 + * @brief 获取预测的装甲板位置，这里的预测尚未加入速度擦差分得加速度 * @param id 装甲板类型 * @param sec 预测时间（秒） * @return 四个装甲板的预测位置和姿态 @@ -60,7 +56,7 @@ class CarPredictEkf : public world_exe::util::Ekf<11, 4, CarPredictEkf> { const double r2 = model_output(8); // 装甲板2到中心距离 double model_yaw = model_output(9) + model_output(10) * sec; // 预测yaw角 - // TODO: 把 pitch 与 z 关联起来，这里可以通过z和z1、z2把pitch算出来 + // 这里也可以通过z和z1、z2把pitch算出来 const double pitch1 = 15. / 180. * std::numbers::pi; const double pitch2 = 15. / 180. * std::numbers::pi; diff --git a/src/v1/predictor/predictor_manager.cpp b/src/v1/predictor/predictor_manager.cpp index a685f94..c2a004b 100644 --- a/src/v1/predictor/predictor_manager.cpp +++ b/src/v1/predictor/predictor_manager.cpp @@ -23,6 +23,7 @@ class PredictorManager::Impl { static_cast(0b00000001 << i)); if (armors.empty()) continue; CarPredictEkf::ZVec input; + // 此处只对识别到一块或两块装甲板时做处理，因为对同一辆车不可能有更多，即便有此处也不应处理，当作异常 if (armors.size() == 1) { const auto tmp_armor = data::ArmorGimbalControlSpacing { armors[0].id, transform * armors[0].position, rotation_transform * armors[0].orientation }; @@ -32,6 +33,7 @@ class PredictorManager::Impl { tmp_armor.position.norm(); predictors_[i].Update(input, {}, dt); } else if (armors.size() == 2) { + // 当同时识别到两块装甲板时，优先更新近的那块，再更新远的 const auto armor0_yaw = util::math::get_yaw_from_quaternion(armors[0].orientation); const auto armor1_yaw = util::math::get_yaw_from_quaternion(armors[1].orientation); From 8b24d9808cca1f88bec4c33b8527587a7377bb05 Mon Sep 17 00:00:00 2001 From: heyeuu <2829004293@qq.com> Date: Fri, 5 Sep 2025 05:14:38 +0800 Subject: [PATCH 13/53] fix(identifier):enhance identifier debug output and change target color type --- src/tongji/identifier/classifier.hpp | 2 +- src/tongji/identifier/identifier.cpp | 11 ++++++----- 2 files changed, 7 insertions(+), 6 deletions(-) diff --git a/src/tongji/identifier/classifier.hpp b/src/tongji/identifier/classifier.hpp index 47924f3..175c534 100644 --- a/src/tongji/identifier/classifier.hpp +++ b/src/tongji/identifier/classifier.hpp @@ -20,7 +20,7 @@ class Classifier final { compiled_model_ = core_.compile_model( ovmodel, "AUTO", ov::hint::performance_mode(ov::hint::PerformanceMode::LATENCY)); } - +// void Classify(const cv::Mat& armor_pattern, enumeration::ArmorIdFlag& armor_id, double& armor_confidence) { if (armor_pattern.empty()) { diff --git a/src/tongji/identifier/identifier.cpp b/src/tongji/identifier/identifier.cpp index 2705d6f..b79b572 100644 --- a/src/tongji/identifier/identifier.cpp +++ b/src/tongji/identifier/identifier.cpp @@ -45,12 +45,13 @@ class Identifier::Impl { , min_confidence_(min_confidence) , max_rectangular_error_(max_rectangular_error / 57.3) // degree to rad , save_path_(std::move(save_path)) - , debug_(debug) { + , debug_(debug) + , target_color_(blue) { if (!std::filesystem::exists(save_path_)) std::filesystem::create_directories(save_path_); } - void SetTargetColor(bool target_color) { target_color_ = target_color; } + void SetTargetColor(Color target_color) { target_color_ = target_color; } const std::tuple, enumeration::CarIDFlag> Identify(const cv::Mat& bgr_img) { @@ -296,7 +297,7 @@ class Identifier::Impl { if (name_ok && !confidence_ok) Save(armor_pattern, armor_id); // 出现 5号 则显示 debug 信息。但不过滤。 - if (armor_id == enumeration::ArmorIdFlag::InfantryV) + if (armor_id == enumeration::ArmorIdFlag::InfantryV && debug_) world_exe::util::logger::logger()->debug("See pattern 5 : InfantryV "); return name_ok && confidence_ok; @@ -310,7 +311,7 @@ class Identifier::Impl { || armor_id == enumeration::ArmorIdFlag::Base); // 保存异常的图案，用于分类器的迭代 - if (!name_ok) { + if (!name_ok && debug_) { util::logger::logger()->debug( "see strange armor: {}", util::stringifier::ToString(armor_id)); Save(armor_pattern, armor_id); @@ -330,7 +331,7 @@ class Identifier::Impl { double min_confidence_; double max_rectangular_error_; - bool target_color_ = true; // true: red, false: blue + Color target_color_; bool debug_; std::string save_path_; From 2bc4e9d45c94d54f3632187c34804721f8db0a1a Mon Sep 17 00:00:00 2001 From: heyeuu <2829004293@qq.com> Date: Sat, 6 Sep 2025 05:08:28 +0800 Subject: [PATCH 14/53] fix(identifier):enhance identifier debug output --- src/tongji/identifier/identifier.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/tongji/identifier/identifier.cpp b/src/tongji/identifier/identifier.cpp index b79b572..e7457fb 100644 --- a/src/tongji/identifier/identifier.cpp +++ b/src/tongji/identifier/identifier.cpp @@ -294,7 +294,7 @@ class Identifier::Impl { auto confidence_ok = armor_confidence > min_confidence_; // 保存不确定的图案，用于分类器的迭代 - if (name_ok && !confidence_ok) Save(armor_pattern, armor_id); + if (name_ok && !confidence_ok && debug_) Save(armor_pattern, armor_id); // 出现 5号 则显示 debug 信息。但不过滤。 if (armor_id == enumeration::ArmorIdFlag::InfantryV && debug_) From 99a4bc7a53cc575a3f6880e02697090a239b2c70 Mon Sep 17 00:00:00 2001 From: heyeuu <2829004293@qq.com> Date: Sat, 6 Sep 2025 06:00:21 +0800 Subject: [PATCH 15/53] fix(identifier):add internal recording flag --- src/tongji/identifier/identifier.cpp | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/src/tongji/identifier/identifier.cpp b/src/tongji/identifier/identifier.cpp index e7457fb..0b2c382 100644 --- a/src/tongji/identifier/identifier.cpp +++ b/src/tongji/identifier/identifier.cpp @@ -32,7 +32,8 @@ class Identifier::Impl { const double& min_lightbar_ratio, const double& max_lightbar_ratio, const double& min_lightbar_length, const double& max_armor_ratio, const double& min_armor_ratio, const double& max_side_ratio, const double& min_confidence, - const double& max_rectangular_error, const std::string& save_path, const bool& debug = true) + const double& max_rectangular_error, const std::string& save_path, const bool& debug = true, + const bool& record = true) : classifier_(model_path, model_image_width, model_image_height) , threshold_(threshold) , max_angle_error_(max_angle_error / 57.3) // degree to rad @@ -46,6 +47,7 @@ class Identifier::Impl { , max_rectangular_error_(max_rectangular_error / 57.3) // degree to rad , save_path_(std::move(save_path)) , debug_(debug) + , record_(record) , target_color_(blue) { if (!std::filesystem::exists(save_path_)) std::filesystem::create_directories(save_path_); @@ -294,7 +296,7 @@ class Identifier::Impl { auto confidence_ok = armor_confidence > min_confidence_; // 保存不确定的图案，用于分类器的迭代 - if (name_ok && !confidence_ok && debug_) Save(armor_pattern, armor_id); + if (name_ok && !confidence_ok && record_) Save(armor_pattern, armor_id); // 出现 5号 则显示 debug 信息。但不过滤。 if (armor_id == enumeration::ArmorIdFlag::InfantryV && debug_) @@ -334,6 +336,7 @@ class Identifier::Impl { Color target_color_; bool debug_; + bool record_; std::string save_path_; }; From 53fd55affaf5e548239ea6218a3470193b69b9b8 Mon Sep 17 00:00:00 2001 From: alray <1780284652@qq.com> Date: Sun, 7 Sep 2025 09:49:17 +0800 Subject: [PATCH 16/53] =?UTF-8?q?=E6=B7=BB=E5=8A=A0=E4=BA=86=E6=8E=A5?= =?UTF-8?q?=E5=8F=A3=E7=9A=84=E6=B3=A8=E9=87=8A?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- include/interfaces/armor_in_camera.hpp | 9 ++++++++ .../interfaces/armor_in_gimbal_control.hpp | 9 ++++++++ include/interfaces/armor_in_image.hpp | 17 ++++++++++++++ include/interfaces/car_state.hpp | 14 +++++++++-- include/interfaces/drawable.hpp | 4 ++++ include/interfaces/fire_controller.hpp | 18 ++++++++++++++- include/interfaces/identifier.hpp | 11 ++++++++- include/interfaces/pnp_solver.hpp | 10 ++++++++ include/interfaces/predictor.hpp | 17 +++++++++++++- .../interfaces/predictor_update_package.hpp | 20 ++++++++++++++-- include/interfaces/sync_block.hpp | 5 ++++ include/interfaces/sync_load.hpp | 5 ++++ include/interfaces/target_predictor.hpp | 23 ++++++++++++++++--- include/interfaces/time_stamped.hpp | 3 +++ src/v1/auto_aim_system_v1.cpp | 4 ++-- src/v1/predictor/predictor_manager.hpp | 9 ++++---- 16 files changed, 162 insertions(+), 16 deletions(-) diff --git a/include/interfaces/armor_in_camera.hpp b/include/interfaces/armor_in_camera.hpp index 24bf577..52ea074 100644 --- a/include/interfaces/armor_in_camera.hpp +++ b/include/interfaces/armor_in_camera.hpp @@ -6,10 +6,19 @@ #include namespace world_exe::interfaces { + +/** + * @brief + 某一确定时刻 + 相机坐标系下 + 装甲板集合 + */ class IArmorInCamera { public: + /// 获取时间戳，标志其内容装甲板的准确时间点 COMBINE_TIME_STAMPED; + /// 获取某个车辆ID的装甲板集合 virtual const std::vector& GetArmors( const enumeration::ArmorIdFlag& armor_id) const = 0; }; diff --git a/include/interfaces/armor_in_gimbal_control.hpp b/include/interfaces/armor_in_gimbal_control.hpp index dccdbd6..cedad6e 100644 --- a/include/interfaces/armor_in_gimbal_control.hpp +++ b/include/interfaces/armor_in_gimbal_control.hpp @@ -6,11 +6,20 @@ #include namespace world_exe::interfaces { + +/** + * @brief + 某一确定时刻 + 云台控制指令使用的坐标系（世界坐标系）下 + 装甲板集合 + */ class IArmorInGimbalControl { public: + /// 获取时间戳，标志其内容装甲板的准确时间点 COMBINE_TIME_STAMPED; + /// 获取某个车辆ID的装甲板集合 virtual const std::vector& GetArmors( const enumeration::ArmorIdFlag& armor_id) const = 0; }; diff --git a/include/interfaces/armor_in_image.hpp b/include/interfaces/armor_in_image.hpp index b68b285..e07fdc5 100644 --- a/include/interfaces/armor_in_image.hpp +++ b/include/interfaces/armor_in_image.hpp @@ -7,11 +7,28 @@ #include namespace world_exe::interfaces { + +/** + * @brief + 某一确定时刻 + 相机的画面坐标系下 + 装甲板集合 + * @warning + ArmorImageSpacing 为二维装甲板 + 其实际坐标系可以看作以相机光轴为x，光心为原点的 x-forward y-up 实体坐标系 + */ class IArmorInImage { public: + /// 获取时间戳，标志其内容装甲板的准确时间点 COMBINE_TIME_STAMPED; + /** + * @brief 获取某个车辆ID的装甲板集合 + * + * @param armor_id 要查找的车辆ID + * @return const std::vector& + */ virtual const std::vector& GetArmors( const enumeration::ArmorIdFlag& armor_id) const = 0; }; diff --git a/include/interfaces/car_state.hpp b/include/interfaces/car_state.hpp index f2b6bae..6c2863a 100644 --- a/include/interfaces/car_state.hpp +++ b/include/interfaces/car_state.hpp @@ -4,10 +4,20 @@ #include namespace world_exe::interfaces { + +/** + * @brief + ** 确定自瞄系统中某个ID代表的车辆是否可以进行云台锁定、开火 + * @todo 命名鬼才，可能要改下命名 + */ class ICarState { public: - virtual const ICarState& Update(const enumeration::CarIDFlag& car_detected) = 0; - virtual const enumeration::CarIDFlag& GetAllowdToFires() const = 0; + /** + * @brief + 获取可以开火的车辆ID + 示例： 某辆车在镜头中出现的时间够长，确定已经被自瞄系统追踪上 + */ + virtual const enumeration::CarIDFlag& GetAllowdToFires() const = 0; }; } \ No newline at end of file diff --git a/include/interfaces/drawable.hpp b/include/interfaces/drawable.hpp index a3087a0..9e50cac 100644 --- a/include/interfaces/drawable.hpp +++ b/include/interfaces/drawable.hpp @@ -4,6 +4,10 @@ #include namespace world_exe::interfaces { + +/** + * @brief 可以进行可视化调试，提供绘制方法 + */ class IDrawable { public: virtual void Draw(cv::InputArray, cv::OutputArray) = 0; diff --git a/include/interfaces/fire_controller.hpp b/include/interfaces/fire_controller.hpp index 77a9710..881fdbd 100644 --- a/include/interfaces/fire_controller.hpp +++ b/include/interfaces/fire_controller.hpp @@ -5,9 +5,25 @@ #include namespace world_exe::interfaces { + +/** + * @brief 火控系统，获取云台控制方向 + */ class IFireControl { public: + /** + * @brief 计算云台和发射系统控制量 + * + * @param time_duration 额外时间差 典型值：当前时刻到当前帧传感器传入内容的时间差 + * @return const data::FireControl + */ virtual const data::FireControl CalculateTarget(const std::time_t& time_duration) const = 0; - virtual const enumeration::CarIDFlag GetAttackCarId() const = 0; + + /** + * @brief 获取当前火控系统锁定的车辆ID + * + * @return const enumeration::CarIDFlag : 火控系统锁定的车辆ID + */ + virtual const enumeration::CarIDFlag GetAttackCarId() const = 0; }; } \ No newline at end of file diff --git a/include/interfaces/identifier.hpp b/include/interfaces/identifier.hpp index 510f56e..db441da 100644 --- a/include/interfaces/identifier.hpp +++ b/include/interfaces/identifier.hpp @@ -6,9 +6,18 @@ #include namespace world_exe::interfaces { + +/** + * @brief 识别器 + */ class IIdentifier { public: - /// return : all armors , all armors id + /** + * @brief 识别传入画面中所有的装甲板及其ID + * + * @param input_image 相机捕捉到的画面 + * @return const std::tuple, enumeration::CarIDFlag> + */ virtual const std::tuple, enumeration::CarIDFlag> identify( const cv::Mat& input_image) = 0; }; diff --git a/include/interfaces/pnp_solver.hpp b/include/interfaces/pnp_solver.hpp index 3c908d2..a340df8 100644 --- a/include/interfaces/pnp_solver.hpp +++ b/include/interfaces/pnp_solver.hpp @@ -3,8 +3,18 @@ #include "interfaces/armor_in_camera.hpp" #include "interfaces/armor_in_image.hpp" namespace world_exe::interfaces { + +/** + * @brief Pnp求解器 + 指一切可以通过2维装甲板求解3维装甲板的算法器 + */ class IPnpSolver { public: + /** + * @brief 求解装甲板的pnp问题 + * @param std::shared_ptr 二维装甲板 + * @return std::shared_ptr 三维装甲板 + */ virtual std::shared_ptr SolvePnp( std::shared_ptr) = 0; }; diff --git a/include/interfaces/predictor.hpp b/include/interfaces/predictor.hpp index f4e34fe..b257638 100644 --- a/include/interfaces/predictor.hpp +++ b/include/interfaces/predictor.hpp @@ -5,9 +5,24 @@ #include namespace world_exe::interfaces { +/** + * @brief 预测器，通常通过某一个时刻的状态向量x 及其状态转移矩阵: x' = Ax + Bu 获取未来状态向量x' + * 继承自这个接口的类一般是Record, 创建后内容不会改变 + */ class IPredictor { public: - virtual const enumeration::ArmorIdFlag& GetId() const = 0; + /** + * @brief 获取当前预测器可以进行预测的所有车辆ID + * + * @return const enumeration::ArmorIdFlag& + */ + virtual const enumeration::ArmorIdFlag& GetId() const = 0; + /** + * @brief 求解出未来某个时间的装甲板集合 + * + * @param time_stamp 未来的某个时间点 + * @return const IArmorInGimbalControl& + */ virtual const IArmorInGimbalControl& Predictor(const std::time_t& time_stamp) const = 0; }; } \ No newline at end of file diff --git a/include/interfaces/predictor_update_package.hpp b/include/interfaces/predictor_update_package.hpp index 5828dfb..026e941 100644 --- a/include/interfaces/predictor_update_package.hpp +++ b/include/interfaces/predictor_update_package.hpp @@ -6,13 +6,29 @@ #include namespace world_exe::interfaces { + +/** + * @brief 给出滤波器需要的所有数据 + * @todo 应该使用结构体，但是先写着无所谓 + */ class IPreDictorUpdatePackage { public: + /** + * @brief 传感器数据获取时的时间戳 + */ COMBINE_TIME_STAMPED; + /** + * @brief 求解好的装甲板三维信息 + * + * @return std::shared_ptr + */ virtual std::shared_ptr GetArmors() const = 0; - /// - /// Affine form image to gimbal_control + /** + * @brief 相机坐标系到世界坐标系的仿射变换 + * + * @return Eigen::Affine3d + */ virtual Eigen::Affine3d GetTransform() const = 0; }; } \ No newline at end of file diff --git a/include/interfaces/sync_block.hpp b/include/interfaces/sync_block.hpp index e44c39c..98e996c 100644 --- a/include/interfaces/sync_block.hpp +++ b/include/interfaces/sync_block.hpp @@ -3,6 +3,11 @@ #include namespace world_exe::interfaces { +/** + * @brief 不必管他 + * + * @tparam T + */ template class ISyncBlock { public: diff --git a/include/interfaces/sync_load.hpp b/include/interfaces/sync_load.hpp index 37b2991..7a6fd35 100644 --- a/include/interfaces/sync_load.hpp +++ b/include/interfaces/sync_load.hpp @@ -6,6 +6,11 @@ namespace world_exe::interfaces { +/** + * @brief 不必管他 + * + * @tparam T + */ template class ISyncLoad { public: diff --git a/include/interfaces/target_predictor.hpp b/include/interfaces/target_predictor.hpp index 01324b3..4340364 100644 --- a/include/interfaces/target_predictor.hpp +++ b/include/interfaces/target_predictor.hpp @@ -3,16 +3,33 @@ #include "armor_in_gimbal_control.hpp" #include "enum/armor_id.hpp" #include "interfaces/predictor.hpp" -#include "predictor_update_package.hpp" #include #include namespace world_exe::interfaces { + +/** + * @brief 类似IPredictor但是其内容不保证稳定 + * + */ class ITargetPredictor { public: + /** + * @brief + * + * @param id + * @param time_stamp + * @return std::shared_ptr + */ virtual std::shared_ptr Predict( - const enumeration::ArmorIdFlag& id, const std::time_t& time_stamp) = 0; - virtual void Update(std::shared_ptr data) = 0; + const enumeration::ArmorIdFlag& id, const std::time_t& time_stamp) = 0; + + /** + * @brief 按照传入的id生成IPredictor + * + * @param id + * @return const IPredictor& + */ virtual const IPredictor& GetPredictor(const enumeration::ArmorIdFlag& id) const = 0; }; } \ No newline at end of file diff --git a/include/interfaces/time_stamped.hpp b/include/interfaces/time_stamped.hpp index acc377e..20d5463 100644 --- a/include/interfaces/time_stamped.hpp +++ b/include/interfaces/time_stamped.hpp @@ -5,6 +5,9 @@ #define COMBINE_TIME_STAMPED virtual const ITimeStamped& GetTimeStamped() const = 0 namespace world_exe::interfaces { +/** + * @brief 时间戳，通常是SteadyClock + */ class ITimeStamped { public: virtual const std::time_t& GetTimeStamp() const = 0; diff --git a/src/v1/auto_aim_system_v1.cpp b/src/v1/auto_aim_system_v1.cpp index d149a96..443bb4e 100644 --- a/src/v1/auto_aim_system_v1.cpp +++ b/src/v1/auto_aim_system_v1.cpp @@ -93,7 +93,7 @@ template class SystemV1ImplTemp : public world_exe::v1::SystemV1::System ParamsForSystemV1::car_id_identify_event, // [this](const auto& data) { FLOW_IN(ParamsForSystemV1::car_id_identify_event, enumeration::CarIDFlag) - car_state_->Update(data); + state_machine->Update(data); core::EventBus::Publish( // ParamsForSystemV1::car_tracing_event, car_state_->GetAllowdToFires()); FLOW_OUT(ParamsForSystemV1::car_id_identify_event, enumeration::CarIDFlag) @@ -136,7 +136,7 @@ template class SystemV1ImplTemp : public world_exe::v1::SystemV1::System FLOW_IN( ParamsForSystemV1::tracker_update_event, interfaces::IPreDictorUpdatePackage) fire_control->SetTimeStamp(data->GetTimeStamped().GetTimeStamp()); - tracker_->Update(data); + predictor->Update(data); FLOW_OUT( ParamsForSystemV1::tracker_update_event, interfaces::IPreDictorUpdatePackage) }); diff --git a/src/v1/predictor/predictor_manager.hpp b/src/v1/predictor/predictor_manager.hpp index bf64970..b99d80f 100644 --- a/src/v1/predictor/predictor_manager.hpp +++ b/src/v1/predictor/predictor_manager.hpp @@ -1,19 +1,20 @@ #pragma once +#include "interfaces/predictor_update_package.hpp" #include "interfaces/target_predictor.hpp" namespace world_exe::v1::predictor { -class PredictorManager : public world_exe::interfaces::ITargetPredictor { +class PredictorManager final : public world_exe::interfaces::ITargetPredictor { public: PredictorManager(); ~PredictorManager(); - void Update(std::shared_ptr data) override; + void Update(std::shared_ptr data); virtual std::shared_ptr Predict( - const enumeration::ArmorIdFlag& id, const std::time_t& time_stamp) override; + const enumeration::ArmorIdFlag& id, const std::time_t& time_stamp); - const interfaces::IPredictor& GetPredictor(const enumeration::ArmorIdFlag& id) const override; + const interfaces::IPredictor& GetPredictor(const enumeration::ArmorIdFlag& id) const; private: class Impl; From 570d10024d869e90410c0cf18e2cec1c4ed93fda Mon Sep 17 00:00:00 2001 From: alray <1780284652@qq.com> Date: Fri, 12 Sep 2025 08:38:39 +0800 Subject: [PATCH 17/53] =?UTF-8?q?=E4=BF=AE=E5=A4=8D=E9=94=99=E8=AF=AF?= =?UTF-8?q?=E7=9A=84=E5=87=BD=E6=95=B0=E7=AD=BE=E5=90=8D?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit target_predictor --- include/interfaces/predictor.hpp | 3 ++- include/interfaces/target_predictor.hpp | 2 +- src/v1/auto_aim_system_v1.cpp | 4 ++-- src/v1/fire_controller/fire_controller.cpp | 4 ++-- src/v1/predictor/car/car_predictor.cpp | 13 ++++++------- src/v1/predictor/car/car_predictor.hpp | 2 +- src/v1/predictor/predictor_manager.cpp | 12 ++++++------ src/v1/predictor/predictor_manager.hpp | 2 +- 8 files changed, 21 insertions(+), 21 deletions(-) diff --git a/include/interfaces/predictor.hpp b/include/interfaces/predictor.hpp index b257638..68c4fb7 100644 --- a/include/interfaces/predictor.hpp +++ b/include/interfaces/predictor.hpp @@ -23,6 +23,7 @@ class IPredictor { * @param time_stamp 未来的某个时间点 * @return const IArmorInGimbalControl& */ - virtual const IArmorInGimbalControl& Predictor(const std::time_t& time_stamp) const = 0; + virtual std::shared_ptr Predictor( + const std::time_t& time_stamp) const = 0; }; } \ No newline at end of file diff --git a/include/interfaces/target_predictor.hpp b/include/interfaces/target_predictor.hpp index 4340364..70492cb 100644 --- a/include/interfaces/target_predictor.hpp +++ b/include/interfaces/target_predictor.hpp @@ -30,6 +30,6 @@ class ITargetPredictor { * @param id * @return const IPredictor& */ - virtual const IPredictor& GetPredictor(const enumeration::ArmorIdFlag& id) const = 0; + virtual std::shared_ptr GetPredictor(const enumeration::ArmorIdFlag& id) const = 0; }; } \ No newline at end of file diff --git a/src/v1/auto_aim_system_v1.cpp b/src/v1/auto_aim_system_v1.cpp index 443bb4e..7e93773 100644 --- a/src/v1/auto_aim_system_v1.cpp +++ b/src/v1/auto_aim_system_v1.cpp @@ -1,8 +1,8 @@ #include "./auto_aim_system_v1.hpp" +#include "core/event_bus.hpp" #include "data/fire_control.hpp" #include "data/sync_data.hpp" #include "enum/armor_id.hpp" -#include "event_bus.hpp" #include "fire_controller/fire_controller.hpp" #include "identifier/identifier.hpp" #include "interfaces/armor_in_camera.hpp" @@ -157,7 +157,7 @@ template class SystemV1ImplTemp : public world_exe::v1::SystemV1::System [this](const auto& data) { FLOW_IN(ParamsForSystemV1::get_lastest_predictor_event, enumeration::CarIDFlag) if (data != enumeration::CarIDFlag::None) - core::EventBus::Publish( + core::EventBus::Publish>( ParamsForSystemV1::get_lastest_predictor_event, tracker_->GetPredictor(data)); FLOW_OUT(ParamsForSystemV1::get_lastest_predictor_event, enumeration::CarIDFlag) diff --git a/src/v1/fire_controller/fire_controller.cpp b/src/v1/fire_controller/fire_controller.cpp index d508a17..54a16b4 100644 --- a/src/v1/fire_controller/fire_controller.cpp +++ b/src/v1/fire_controller/fire_controller.cpp @@ -48,7 +48,7 @@ class world_exe::v1::fire_control::TracingFireControl::Impl { const world_exe::data::FireControl CalculateTarget(const std::time_t& time_duration) { time_t fly_time = 0; const auto& pre1 = predictor_->Predictor(fly_time + time_duration + control_delay_); - const auto& pre2 = pre1.GetArmors(predictor_->GetId()); + const auto& pre2 = pre1->GetArmors(predictor_->GetId()); double min_angular_dis = 1e9; int index = -1, index_ = 0; for (const auto vec : pre2) { @@ -67,7 +67,7 @@ class world_exe::v1::fire_control::TracingFireControl::Impl { for (int i = 5; i-- > 0;) { const auto& armors_in_gimbal_control = predictor_->Predictor(fly_time + time_duration + control_delay_); - const auto& armors = armors_in_gimbal_control.GetArmors(predictor_->GetId()); + const auto& armors = armors_in_gimbal_control->GetArmors(predictor_->GetId()); const auto& [fly_time, dir] = trajectory_solver::gravity_only(armors[index].position, velocity_begin_, gravity_); } diff --git a/src/v1/predictor/car/car_predictor.cpp b/src/v1/predictor/car/car_predictor.cpp index 7d503c1..cc67c1f 100644 --- a/src/v1/predictor/car/car_predictor.cpp +++ b/src/v1/predictor/car/car_predictor.cpp @@ -17,11 +17,12 @@ class CarPredictor::Impl { const enumeration::ArmorIdFlag& GetId() const { return id_; } - const interfaces::IArmorInGimbalControl& Predictor(const std::time_t& time_stamp) { - target_armors_.SetWithSingleId(ekf_.get_predict_output_armor(id_, - (time_stamp - create_time_stamp_.GetTimeStamp()) / 1.e9), + std::shared_ptr Predictor(const std::time_t& time_stamp) { + auto ptr = std::make_shared(); + ptr->SetWithSingleId(ekf_.get_predict_output_armor( + id_, (time_stamp - create_time_stamp_.GetTimeStamp()) / 1.e9), time_stamp); - return target_armors_; + return ptr; } inline void SetId(const enumeration::CarIDFlag& id) { id_ = id; } @@ -36,8 +37,6 @@ class CarPredictor::Impl { enumeration::CarIDFlag id_; CarPredictEkf ekf_; PredictTimeStamp create_time_stamp_; - - PredictArmorInGimbalControl target_armors_; }; CarPredictor::CarPredictor() @@ -52,7 +51,7 @@ CarPredictor::~CarPredictor() = default; const enumeration::ArmorIdFlag& CarPredictor::GetId() const { return pimpl_->GetId(); } -const interfaces::IArmorInGimbalControl& CarPredictor::Predictor( +std::shared_ptr CarPredictor::Predictor( const std::time_t& time_stamp) const { return pimpl_->Predictor(time_stamp); } diff --git a/src/v1/predictor/car/car_predictor.hpp b/src/v1/predictor/car/car_predictor.hpp index 22889fa..acebd0b 100644 --- a/src/v1/predictor/car/car_predictor.hpp +++ b/src/v1/predictor/car/car_predictor.hpp @@ -17,7 +17,7 @@ class CarPredictor : public interfaces::IPredictor { void SetTimeStamp(const interfaces::ITimeStamped& time_stamp); const enumeration::ArmorIdFlag& GetId() const override; - const interfaces::IArmorInGimbalControl& Predictor( + std::shared_ptr Predictor( const std::time_t& time_stamp) const override; private: diff --git a/src/v1/predictor/predictor_manager.cpp b/src/v1/predictor/predictor_manager.cpp index c2a004b..e964dd2 100644 --- a/src/v1/predictor/predictor_manager.cpp +++ b/src/v1/predictor/predictor_manager.cpp @@ -90,10 +90,11 @@ class PredictorManager::Impl { return std::make_shared(armors, last_update_time_stamp_); }; - const interfaces::IPredictor& GetPredictor(const enumeration::ArmorIdFlag& id) { - predictor.SetId(id); - predictor.SetEkf(predictors_[util::enumeration::GetIndex(id)]); - predictor.SetTimeStamp(last_update_time_stamp_); + std::shared_ptr GetPredictor(const enumeration::ArmorIdFlag& id) { + auto predictor = std::make_shared(); + predictor->SetId(id); + predictor->SetEkf(predictors_[util::enumeration::GetIndex(id)]); + predictor->SetTimeStamp(last_update_time_stamp_); return predictor; } @@ -103,7 +104,6 @@ class PredictorManager::Impl { Eigen::Affine3d transform_from_camera_to_gimbal_; PredictArmorInGimbalControl predictted_armors_; - CarPredictor predictor; }; PredictorManager::PredictorManager() @@ -120,7 +120,7 @@ void PredictorManager::Update(std::shared_ptrUpdate(data); }; -const interfaces::IPredictor& PredictorManager::GetPredictor( +std::shared_ptr PredictorManager::GetPredictor( const enumeration::ArmorIdFlag& id) const { return pimpl_->GetPredictor(id); }; diff --git a/src/v1/predictor/predictor_manager.hpp b/src/v1/predictor/predictor_manager.hpp index b99d80f..77b53f6 100644 --- a/src/v1/predictor/predictor_manager.hpp +++ b/src/v1/predictor/predictor_manager.hpp @@ -14,7 +14,7 @@ class PredictorManager final : public world_exe::interfaces::ITargetPredictor { virtual std::shared_ptr Predict( const enumeration::ArmorIdFlag& id, const std::time_t& time_stamp); - const interfaces::IPredictor& GetPredictor(const enumeration::ArmorIdFlag& id) const; + std::shared_ptr GetPredictor(const enumeration::ArmorIdFlag& id) const; private: class Impl; From 7fe53dff2f4302b7f51ea22f8f03e69a2bf1e90f Mon Sep 17 00:00:00 2001 From: heyeuu <2829004293@qq.com> Date: Mon, 15 Sep 2025 01:33:04 +0800 Subject: [PATCH 18/53] feat: add state machine --- .../state_machine/car_state_manager.hpp | 55 +++++++++++++++++++ src/tongji/state_machine/state_machine.cpp | 40 ++++++++++++++ src/tongji/state_machine/state_machine.hpp | 24 ++++++++ 3 files changed, 119 insertions(+) create mode 100644 src/tongji/state_machine/car_state_manager.hpp create mode 100644 src/tongji/state_machine/state_machine.cpp create mode 100644 src/tongji/state_machine/state_machine.hpp diff --git a/src/tongji/state_machine/car_state_manager.hpp b/src/tongji/state_machine/car_state_manager.hpp new file mode 100644 index 0000000..da80ff9 --- /dev/null +++ b/src/tongji/state_machine/car_state_manager.hpp @@ -0,0 +1,55 @@ +#pragma once + +#include + +namespace world_exe::tongji::car_state { + +class CarStateManager { +public: + CarStateManager(int switch_threshold = 5) + : switch_threshold_(switch_threshold) { } + + void Update(bool detected, std::chrono::steady_clock::time_point now) { + if (detected) { + count_ = std::min(count_ + 1, switch_threshold_); + last_seen_ = now; + } else { + count_ = std::max(count_ - 1, 0); + } + is_locked_ = (count_ >= switch_threshold_); + } + + bool IsLocked() const { return is_locked_; } + bool IsConverged() const { return is_converged_; } + bool IsDiverged() const { return is_diverged_; } + + void SetPriority(int p); + void SetThreshold(const int& value) { switch_threshold_ = value; } + void SetDiverged(bool diverged) { + is_diverged_ = diverged; + if (diverged) is_converged_ = false; + } + void SetPriority(const int& p) { priority_ = p; } + int GetPriority() const { return priority_; } + + std::chrono::steady_clock::time_point LastSeen() const { return last_seen_; } + + void IncrementUpdateCount() { + update_count_++; + if (update_count_ > 3 && !is_diverged_) { + is_converged_ = true; + } + } + +private: + int count_ = 0; + int switch_threshold_; + int update_count_ = 0; + + bool is_locked_ = false; + bool is_converged_ = false; + bool is_diverged_ = false; + int priority_ = 100; // 默认最低优先级 + std::chrono::steady_clock::time_point last_seen_; +}; +} \ No newline at end of file diff --git a/src/tongji/state_machine/state_machine.cpp b/src/tongji/state_machine/state_machine.cpp new file mode 100644 index 0000000..918adb3 --- /dev/null +++ b/src/tongji/state_machine/state_machine.cpp @@ -0,0 +1,40 @@ +#include "state_machine.hpp" + +#include "enum/car_id.hpp" + +namespace world_exe::tongji::state_machine { + +StateMachine::StateMachine(int switch_threshold) + : switch_threshold_(switch_threshold) { + for (auto& manager : car_states_) { + manager = car_state::CarStateManager(switch_threshold_); + } +} + +const enumeration::CarIDFlag& StateMachine::GetAllowdToFires() const { return tracing_state_; } + +const interfaces::ICarState& StateMachine::Update( + const enumeration::CarIDFlag& car_detected, std::chrono::steady_clock::time_point now) { + tracing_state_ = enumeration::CarIDFlag::None; + + for (int i = 0; i < static_cast(enumeration::CarIDFlag::Count); ++i) { + auto id = static_cast(1 << i); + bool detected = (static_cast(car_detected) >> i) & 0x01; + + car_states_[i].Update(detected, now); + + if (detected) { + car_states_[i].IncrementUpdateCount(); + } + + if (car_states_[i].IsLocked() && car_states_[i].IsConverged() + && !car_states_[i].IsDiverged()) { + tracing_state_ = + static_cast(static_cast(tracing_state_) | (1 << i)); + } + } + + return *this; +} + +} diff --git a/src/tongji/state_machine/state_machine.hpp b/src/tongji/state_machine/state_machine.hpp new file mode 100644 index 0000000..f6b55c9 --- /dev/null +++ b/src/tongji/state_machine/state_machine.hpp @@ -0,0 +1,24 @@ +#pragma once + +#include + +#include "car_state_manager.hpp" +#include "enum/car_id.hpp" +#include "interfaces/car_state.hpp" + +namespace world_exe::tongji::state_machine { +class StateMachine final : public interfaces::ICarState { +public: + StateMachine(int switch_threshold = 5); + + const enumeration::CarIDFlag& GetAllowdToFires() const override; + + const interfaces::ICarState& Update( + const enumeration::CarIDFlag& car_detected, std::chrono::steady_clock::time_point now); + +private: + enumeration::CarIDFlag tracing_state_ = enumeration::CarIDFlag::None; + std::array car_states_; + int switch_threshold_; +}; +} \ No newline at end of file From 244d7c57b789deec2626e59b9c73e84be572eb9b Mon Sep 17 00:00:00 2001 From: heyeuu <2829004293@qq.com> Date: Mon, 15 Sep 2025 21:02:48 +0800 Subject: [PATCH 19/53] feat: Add predictor module --- src/tongji/identifier/classifier.hpp | 2 +- src/tongji/predictor/car_predictor.hpp | 420 ++++++++++++++++++ src/tongji/predictor/car_predictor_boss.cpp | 56 +++ src/tongji/predictor/car_predictor_boss.hpp | 23 + .../predictor/car_predictor_manager.cpp | 134 ++++++ .../predictor/car_predictor_manager.hpp | 26 ++ .../predictor/in_gimbal_control_armor.hpp | 59 +++ src/tongji/predictor/time_stamp.hpp | 24 + .../state_machine/car_state_manager.hpp | 15 +- src/tongji/state_machine/state_machine.cpp | 2 +- src/tongji/state_machine/state_machine.hpp | 4 +- src/util/extended_kalman_filter.hpp | 115 +++++ src/util/math.hpp | 26 ++ 13 files changed, 898 insertions(+), 8 deletions(-) create mode 100644 src/tongji/predictor/car_predictor.hpp create mode 100644 src/tongji/predictor/car_predictor_boss.cpp create mode 100644 src/tongji/predictor/car_predictor_boss.hpp create mode 100644 src/tongji/predictor/car_predictor_manager.cpp create mode 100644 src/tongji/predictor/car_predictor_manager.hpp create mode 100644 src/tongji/predictor/in_gimbal_control_armor.hpp create mode 100644 src/tongji/predictor/time_stamp.hpp create mode 100644 src/util/extended_kalman_filter.hpp diff --git a/src/tongji/identifier/classifier.hpp b/src/tongji/identifier/classifier.hpp index 175c534..b83bbac 100644 --- a/src/tongji/identifier/classifier.hpp +++ b/src/tongji/identifier/classifier.hpp @@ -20,7 +20,7 @@ class Classifier final { compiled_model_ = core_.compile_model( ovmodel, "AUTO", ov::hint::performance_mode(ov::hint::PerformanceMode::LATENCY)); } -// + void Classify(const cv::Mat& armor_pattern, enumeration::ArmorIdFlag& armor_id, double& armor_confidence) { if (armor_pattern.empty()) { diff --git a/src/tongji/predictor/car_predictor.hpp b/src/tongji/predictor/car_predictor.hpp new file mode 100644 index 0000000..4ce3fbe --- /dev/null +++ b/src/tongji/predictor/car_predictor.hpp @@ -0,0 +1,420 @@ +#pragma once + +#include +#include + +#include "data/armor_gimbal_control_spacing.hpp" +#include "enum/armor_id.hpp" +#include "enum/car_id.hpp" +#include "interfaces/predictor_update_package.hpp" +#include "tongji/predictor/time_stamp.hpp" +#include "util/extended_kalman_filter.hpp" +#include "util/math.hpp" + +namespace world_exe::tongji::predictor { + +struct PredictorStatus { + bool jumped = false; + bool switched = false; + bool converged = false; + bool diverged = false; + bool lost = false; + bool reidentified = false; + int switch_count = 0; + int update_count = 0; + int lost_count = 0; +}; + +class CarPredictor { +public: + CarPredictor() = default; + explicit CarPredictor(const enumeration::ArmorIdFlag armor_id, + const Eigen::Vector3d& armor_xyz_in_world, const Eigen::Vector3d& armor_ypr_in_world, + std::time_t time_stamp, double radius, int armor_num, Eigen::VectorXd P0_dig) + : armor_id_(armor_id) + , last_id(0) + , radius_(radius) + , armor_num_(armor_num) + , last_predict_time_stamp_(time_stamp) { + const Eigen::VectorXd& xyz = armor_xyz_in_world; + const Eigen::VectorXd& ypr = armor_ypr_in_world; + + // 旋转中心的坐标 + auto center_x = xyz[0] + radius * std::cos(ypr[0]); + auto center_y = xyz[1] + radius * std::sin(ypr[0]); + auto center_z = xyz[2]; + + // x vx y vy z vz a w r l h + // a: angle + // w: angular velocity + // l: r2 - r1 + // h: z2 - z1 + Eigen::VectorXd x0 { { center_x, 0, center_y, 0, center_z, 0, ypr[0], 0, radius, 0, + 0 } }; // 初始化预测量 + P0_dig_ = P0_dig; + Eigen::MatrixXd P0 = P0_dig.asDiagonal(); + + // 防止夹角求和出现异常值 + x_add_ = [](const Eigen::VectorXd& a, const Eigen::VectorXd& b) -> Eigen::VectorXd { + Eigen::VectorXd c = a + b; + c[6] = util::math::clamp_pm_pi(c[6]); + return c; + }; + + ekf_ = util::ExtendedKalmanFilter(x0, P0, x_add_); // 初始化滤波器（预测量、预测量协方差） + } + + void SetEkf(const util::ExtendedKalmanFilter& ekf) { ekf_ = ekf; } + + std::vector GetPredictedArmors( + enumeration::ArmorIdFlag armor_id, const TimeStamp time_stamp) { + PredictTo(time_stamp.GetTimeStamp()); + UpdateStatus(); + + // 装甲板匹配 + int id; + auto min_angle_error = 1e10; + const std::vector& xyza_list = armor_xyza_list(); + + std::vector> xyza_i_list; + for (int i = 0; i < armor_num_; i++) { + xyza_i_list.push_back({ xyza_list[i], i }); + } + + std::sort(xyza_i_list.begin(), xyza_i_list.end(), + [](const std::pair& a, const std::pair& b) { + Eigen::Vector3d ypd1 = util::math::xyz2ypd(a.first.head(3)); + Eigen::Vector3d ypd2 = util::math::xyz2ypd(b.first.head(3)); + return ypd1[2] < ypd2[2]; + }); + + std::vector armors; + for (int i = 0; i < armor_num_; i++) { + xyza_i_list.push_back({ xyza_list[i], i }); + const auto pos = xyza_i_list[i].first.head<3>(); + const auto yaw = util::math::clamp_pm_pi(ekf_.x[6]); + const double pitch = 15. / 180. * std::numbers::pi; + armors.push_back(data::ArmorGimbalControlSpacing { + armor_id_, pos, util::math::euler_to_quaternion(yaw, pitch, 0.) }); + } + return armors; + } + + void Update(std::shared_ptr data) { + const auto& transform = data->GetTransform(); + const auto& rotation_transform = Eigen::Quaterniond(transform.linear()); + const auto armors = data->GetArmors()->GetArmors(armor_id_); + + if (armors.empty()) return; + for (auto armor : armors) { + const auto& armor_in_world_position = transform * armor.position; + const auto& armor_in_world_oritaiton = rotation_transform * armor.orientation; + this->update(armor_in_world_position, + util::math::quaternion_to_euler(armor_in_world_oritaiton, 2, 1, 0), + util::math::xyz2ypd(armor_in_world_position)); + } + } + + const PredictorStatus& GetStatus() const { return status_; } + + void PredictTo(const TimeStamp& now) { + if (last_predict_time_stamp_.GetTimeStamp() == 0) { + last_predict_time_stamp_ = now; + return; + } + const double& dt = now.SecondsSince(last_predict_time_stamp_); + last_predict_time_stamp_ = now; + + if (dt <= 0.0 || dt > 1.0) { + // util::logger::logger()->warn( + // "[CarPredictor] Abnormal dt: {:.3f}, skipping prediction", dt); + return; + } + + predict(dt); + } + + void Reset(const Eigen::Vector3d& xyz, const Eigen::Vector3d& ypr, std::time_t time_stamp) { + last_predict_time_stamp_ = TimeStamp::FromRaw(time_stamp); + status_ = PredictorStatus {}; // 清空状态计数器 + + // 重新构造滤波器状态 + auto center_x = xyz[0] + radius_ * std::cos(ypr[0]); + auto center_y = xyz[1] + radius_ * std::sin(ypr[0]); + auto center_z = xyz[2]; + + Eigen::VectorXd x0 { { center_x, 0, center_y, 0, center_z, 0, ypr[0], 0, radius_, 0, 0 } }; + Eigen::MatrixXd P0 = P0_dig_.asDiagonal(); + + ekf_ = util::ExtendedKalmanFilter(x0, P0_dig_, x_add_); + } + + void TickStatus() { + static int last_update_count = -1; + if (status_.update_count == last_update_count) { + status_.lost_count++; + } else { + status_.lost_count = 0; + } + last_update_count = status_.update_count; + status_.lost = (status_.lost_count > 30 || IsDiverged()); + status_.reidentified = (!status_.lost && IsConverged() && status_.update_count > 3); + } + + bool IsLocked() const { return status_.converged && !status_.diverged; } + bool IsLost() const { return status_.lost; } + bool IsReidentified() const { return status_.reidentified; } + +private: + bool IsConverged() const { return status_.converged; } + bool IsDiverged() const { return status_.diverged; } + + int MatchArmor(const Eigen::Vector3d& armor_xyz_in_world, + const Eigen::Vector3d& armor_ypr_in_world, const Eigen::Vector3d& armor_ypd_in_world) { + + const auto& xyza_list = armor_xyza_list(); + std::vector> xyza_i_list; + + for (int i = 0; i < armor_num_; ++i) { + xyza_i_list.emplace_back(xyza_list[i], i); + } + + std::sort(xyza_i_list.begin(), xyza_i_list.end(), [](const auto& a, const auto& b) { + auto ypd1 = util::math::xyz2ypd(a.first.head(3)); + auto ypd2 = util::math::xyz2ypd(b.first.head(3)); + return ypd1[2] < ypd2[2]; + }); + + int best_id = 0; + double min_error = 1e10; + + for (int i = 0; i < std::min(3, armor_num_); ++i) { + const auto& xyza = xyza_i_list[i].first; + auto ypd = util::math::xyz2ypd(xyza.head(3)); + double error = std::abs(util::math::clamp_pm_pi(armor_ypr_in_world[0] - xyza[3])) + + std::abs(util::math::clamp_pm_pi(armor_ypd_in_world[0] - ypd[0])); + + if (error < min_error) { + min_error = error; + best_id = xyza_i_list[i].second; + } + } + + return best_id; + } + + auto F(double dt) const -> Eigen::MatrixXd { + // 状态转移矩阵 + // clang-format off + return Eigen::MatrixXd { + {1, dt, 0, 0, 0, 0, 0, 0, 0, 0, 0}, + {0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0}, + {0, 0, 1, dt, 0, 0, 0, 0, 0, 0, 0}, + {0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0}, + {0, 0, 0, 0, 1, dt, 0, 0, 0, 0, 0}, + {0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0}, + {0, 0, 0, 0, 0, 0, 1, dt, 0, 0, 0}, + {0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0}, + {0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0}, + {0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0}, + {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1} + }; + // clang-format on + } + + auto Q(double dt) const -> Eigen::MatrixXd { + // Piecewise White Noise Model + // https://github.com/rlabbe/Kalman-and-Bayesian-Filters-in-Python/blob/master/07-Kalman-Filter-Math.ipynb + double v1, v2; + if (armor_id_ == enumeration::CarIDFlag::Outpost) { + v1 = 10; // 前哨站加速度方差 + v2 = 0.1; // 前哨站角加速度方差 + } else { + v1 = 100; // 加速度方差 + v2 = 400; // 角加速度方差 + } + auto a = dt * dt * dt * dt / 4; + auto b = dt * dt * dt / 2; + auto c = dt * dt; + // 预测过程噪声偏差的方差 + // clang-format off + return Eigen::MatrixXd { + {a * v1, b * v1, 0, 0, 0, 0, 0, 0, 0, 0, 0}, + {b * v1, c * v1, 0, 0, 0, 0, 0, 0, 0, 0, 0}, + { 0, 0, a * v1, b * v1, 0, 0, 0, 0, 0, 0, 0}, + { 0, 0, b * v1, c * v1, 0, 0, 0, 0, 0, 0, 0}, + { 0, 0, 0, 0, a * v1, b * v1, 0, 0, 0, 0, 0}, + { 0, 0, 0, 0, b * v1, c * v1, 0, 0, 0, 0, 0}, + { 0, 0, 0, 0, 0, 0, a * v2, b * v2, 0, 0, 0}, + { 0, 0, 0, 0, 0, 0, b * v2, c * v2, 0, 0, 0}, + { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}, + { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}, + { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0} + }; + // clang-format on + } + + void predict(const double& dt) { + // 防止夹角求和出现异常值 + auto f = [&](const Eigen::VectorXd& x) -> Eigen::VectorXd { + Eigen::VectorXd x_prior = this->F(dt) * x; + x_prior[6] = util::math::clamp_pm_pi(x_prior[6]); + return x_prior; + }; + + // 前哨站转速特判 + if (IsConverged() && this->armor_id_ == enumeration::CarIDFlag::Outpost + && std::abs(this->ekf_.x[7]) > 2) + this->ekf_.x[7] = this->ekf_.x[7] > 0 ? 2.51 : -2.51; + + ekf_.predict(this->F(dt), this->Q(dt), f); + } + + void UpdateStatus() { + // 发散判断 + double r = ekf_.x[8]; + double l = ekf_.x[9]; + status_.diverged = !(r > 0.05 && r < 0.5 && r + l > 0.05 && r + l < 0.5); + + // 收敛判断 + if (armor_id_ == enumeration::CarIDFlag::Outpost) { + status_.converged = (status_.update_count > 10 && !status_.diverged); + } else { + status_.converged = (status_.update_count > 3 && !status_.diverged); + } + } + + void update(const Eigen::Vector3d& armor_xyz_in_world, + const Eigen::Vector3d& armor_ypr_in_world, const Eigen::Vector3d& armor_ypd_in_world) { + // 装甲板匹配 + int id = MatchArmor(armor_xyz_in_world, armor_ypr_in_world, armor_ypd_in_world); + + if (id != 0) status_.jumped = true; + status_.switched = (id != last_id); + if (status_.switched) status_.switch_count++; + + last_id = id; + update_ypda(armor_xyz_in_world, armor_ypr_in_world, armor_ypd_in_world, id); + status_.update_count++; + UpdateStatus(); + } + + void update_ypda(const Eigen::Vector3d& armor_xyz_in_world, + const Eigen::Vector3d& armor_ypr_in_world, const Eigen::Vector3d& armor_ypd_in_world, + int id) { + // 观测jacobi + Eigen::MatrixXd H = h_jacobian(ekf_.x, id); + // Eigen::VectorXd R_dig{{4e-3, 4e-3, 1, 9e-2}}; + auto center_yaw = std::atan2(armor_xyz_in_world[1], armor_xyz_in_world[0]); + auto delta_angle = util::math::clamp_pm_pi(armor_ypr_in_world[0] - center_yaw); + Eigen::VectorXd R_dig { { 4e-3, 4e-3, log(std::abs(delta_angle) + 1) + 1, + log(std::abs(armor_ypd_in_world[2]) + 1) / 200 + 9e-2 } }; + + // 测量过程噪声偏差的方差 + Eigen::MatrixXd R = R_dig.asDiagonal(); + + // 定义非线性转换函数h: x -> z + auto h = [&](const Eigen::VectorXd& x) -> Eigen::Vector4d { + Eigen::VectorXd xyz = h_armor_xyz(x, id); + Eigen::VectorXd ypd = util::math::xyz2ypd(xyz); + auto angle = util::math::clamp_pm_pi(x[6] + id * 2 * CV_PI / armor_num_); + return { ypd[0], ypd[1], ypd[2], angle }; + }; + + // 防止夹角求差出现异常值 + auto z_subtract = [](const Eigen::VectorXd& a, + const Eigen::VectorXd& b) -> Eigen::VectorXd { + Eigen::VectorXd c = a - b; + c[0] = util::math::clamp_pm_pi(c[0]); + c[1] = util::math::clamp_pm_pi(c[1]); + c[3] = util::math::clamp_pm_pi(c[3]); + return c; + }; + + const Eigen::VectorXd& ypd = armor_ypd_in_world; + const Eigen::VectorXd& ypr = armor_ypr_in_world; + Eigen::VectorXd z { { ypd[0], ypd[1], ypd[2], ypr[0] } }; // 获得观测量 + + ekf_.update(z, H, R, h, z_subtract); + } + + Eigen::VectorXd ekf_x() const { return ekf_.x; } + + const util::ExtendedKalmanFilter& ekf() const { return ekf_; } + + std::vector armor_xyza_list() const { + std::vector _armor_xyza_list; + + for (int i = 0; i < armor_num_; i++) { + auto angle = util::math::clamp_pm_pi(ekf_.x[6] + i * 2 * CV_PI / armor_num_); + Eigen::Vector3d xyz = h_armor_xyz(ekf_.x, i); + _armor_xyza_list.push_back({ xyz[0], xyz[1], xyz[2], angle }); + } + return _armor_xyza_list; + } + + // 计算出装甲板中心的坐标（考虑长短轴） + Eigen::Vector3d h_armor_xyz(const Eigen::VectorXd& x, int id) const { + auto angle = util::math::clamp_pm_pi(x[6] + id * 2 * CV_PI / armor_num_); + auto use_l_h = (armor_num_ == 4) && (id == 1 || id == 3); + + auto r = (use_l_h) ? x[8] + x[9] : x[8]; + auto armor_x = x[0] - r * std::cos(angle); + auto armor_y = x[2] - r * std::sin(angle); + auto armor_z = (use_l_h) ? x[4] + x[10] : x[4]; + + return { armor_x, armor_y, armor_z }; + } + + Eigen::MatrixXd h_jacobian(const Eigen::VectorXd& x, int id) const { + auto angle = util::math::clamp_pm_pi(x[6] + id * 2 * CV_PI / armor_num_); + auto use_l_h = (armor_num_ == 4) && (id == 1 || id == 3); + + auto r = (use_l_h) ? x[8] + x[9] : x[8]; + auto dx_da = r * std::sin(angle); + auto dy_da = -r * std::cos(angle); + + auto dx_dr = -std::cos(angle); + auto dy_dr = -std::sin(angle); + auto dx_dl = (use_l_h) ? -std::cos(angle) : 0.0; + auto dy_dl = (use_l_h) ? -std::sin(angle) : 0.0; + + auto dz_dh = (use_l_h) ? 1.0 : 0.0; + + // clang-format off + Eigen::MatrixXd H_armor_xyza{ + {1, 0, 0, 0, 0, 0, dx_da, 0, dx_dr, dx_dl, 0}, + {0, 0, 1, 0, 0, 0, dy_da, 0, dy_dr, dy_dl, 0}, + {0, 0, 0, 0, 1, 0, 0, 0, 0, 0, dz_dh}, + {0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0} + }; + // clang-format on + + Eigen::VectorXd armor_xyz = h_armor_xyz(x, id); + Eigen::MatrixXd H_armor_ypd = util::math::xyz2ypd_jacobian(armor_xyz); + // clang-format off + Eigen::MatrixXd H_armor_ypda{ + {H_armor_ypd(0, 0), H_armor_ypd(0, 1), H_armor_ypd(0, 2), 0}, + {H_armor_ypd(1, 0), H_armor_ypd(1, 1), H_armor_ypd(1, 2), 0}, + {H_armor_ypd(2, 0), H_armor_ypd(2, 1), H_armor_ypd(2, 2), 0}, + { 0, 0, 0, 1} + }; + // clang-format on + + return H_armor_ypda * H_armor_xyza; + } + + int armor_num_; + double radius_; + PredictorStatus status_; + + enumeration::ArmorIdFlag armor_id_; + int last_id = -1; // debug only + + std::function x_add_; + Eigen::VectorXd P0_dig_; + util::ExtendedKalmanFilter ekf_; + TimeStamp last_predict_time_stamp_; +}; + +} \ No newline at end of file diff --git a/src/tongji/predictor/car_predictor_boss.cpp b/src/tongji/predictor/car_predictor_boss.cpp new file mode 100644 index 0000000..af82658 --- /dev/null +++ b/src/tongji/predictor/car_predictor_boss.cpp @@ -0,0 +1,56 @@ +#include "car_predictor_boss.hpp" + +#include +#include +#include + +#include "enum/car_id.hpp" +#include "enum/enum_tools.hpp" +#include "tongji/predictor/car_predictor_manager.hpp" + +namespace world_exe::tongji::predictor { +class CarPredictorBoss::Impl { +public: + Impl(std::shared_ptr manager) + : manager_(manager) { + if (!manager_) { + throw std::invalid_argument("CarPredictorManager must not be null"); + } + } + + std ::shared_ptr Predict( + const enumeration ::ArmorIdFlag& id, const std ::time_t& time_stamp) { + for (int i = 0; i < 8; ++i) { + auto car_id = static_cast( + static_cast(enumeration::CarIDFlag::Hero) << i); + + if (enumeration::IsFlagContains(id, car_id)) { + manager_->SetPredictorBySingleID(car_id, time_stamp); + } + } + return manager_->Predictor(time_stamp); + } + + std ::shared_ptr GetPredictor( + const enumeration ::ArmorIdFlag& id) const { + return manager_; + } + +private: + std::shared_ptr manager_; +}; + +std ::shared_ptr CarPredictorBoss::Predict( + const enumeration ::ArmorIdFlag& id, const std ::time_t& time_stamp) { + return pimpl_->Predict(id, time_stamp); +} + +std ::shared_ptr CarPredictorBoss::GetPredictor( + const enumeration ::ArmorIdFlag& id) const { + return pimpl_->GetPredictor(id); +} + +CarPredictorBoss::CarPredictorBoss(std::shared_ptr manager) + : pimpl_(std::make_unique(manager)) { } + +} \ No newline at end of file diff --git a/src/tongji/predictor/car_predictor_boss.hpp b/src/tongji/predictor/car_predictor_boss.hpp new file mode 100644 index 0000000..35b7198 --- /dev/null +++ b/src/tongji/predictor/car_predictor_boss.hpp @@ -0,0 +1,23 @@ +#pragma once + +#include + +#include "interfaces/target_predictor.hpp" +#include "tongji/predictor/car_predictor_manager.hpp" + +namespace world_exe::tongji::predictor { +class CarPredictorBoss final : public interfaces::ITargetPredictor { +public: + CarPredictorBoss(std::shared_ptr manager); + + std ::shared_ptr GetPredictor( + const enumeration ::ArmorIdFlag& id) const override; + + std ::shared_ptr Predict( + const enumeration ::ArmorIdFlag& id, const std ::time_t& time_stamp) override; + +private: + class Impl; + std::unique_ptr pimpl_; +}; +} \ No newline at end of file diff --git a/src/tongji/predictor/car_predictor_manager.cpp b/src/tongji/predictor/car_predictor_manager.cpp new file mode 100644 index 0000000..1572cde --- /dev/null +++ b/src/tongji/predictor/car_predictor_manager.cpp @@ -0,0 +1,134 @@ +#include "car_predictor_manager.hpp" + +#include +#include +#include +#include + +#include "enum/armor_id.hpp" +#include "enum/car_id.hpp" +#include "tongji/predictor/car_predictor.hpp" +#include "tongji/predictor/in_gimbal_control_armor.hpp" +#include "tongji/predictor/time_stamp.hpp" + +namespace world_exe::tongji::predictor { +class CarPredictorManager::Impl { +public: + Impl() = default; + + const enumeration ::CarIDFlag& GetId() { + if (ids_dirty_) { + ids_ = enumeration::CarIDFlag::None; + for (const auto& [id, _] : predictors_) { + ids_ = static_cast( + static_cast(ids_) | static_cast(id)); + } + ids_dirty_ = false; + } + return ids_; + } + + auto Predictor(const std ::time_t& time_stamp) { + InGimbalControlArmor target_armors; + for (auto& predictor : predictors_) { + target_armors.Add( + predictor.first, predictor.second.GetPredictedArmors(predictor.first, time_stamp)); + } + return std::make_shared(target_armors); + } + + void Tick(const TimeStamp& now) { + std::vector to_remove; + + for (auto& [id, predictor] : predictors_) { + predictor.PredictTo(now); + predictor.TickStatus(); + + if (predictor.IsLost()) { + to_remove.push_back(id); + } + } + // logger()->info("Removing lost predictor: {}", static_cast(id)); + for (auto id : to_remove) { + predictors_.erase(id); + } + } + + void SetPredictorBySingleID(const enumeration::CarIDFlag& id, + const Eigen::Vector3d& armor_xyz_in_world, const Eigen::Vector3d& armor_ypr_in_world, + std::time_t init_time_stamp) { + + auto it = predictors_.find(id); + if (it != predictors_.end()) { + it->second.Reset(armor_xyz_in_world, armor_ypr_in_world, init_time_stamp); + } else { + bool is_balance = (id == enumeration::CarIDFlag::InfantryIII + | id == enumeration::CarIDFlag::InfantryIV + | id == enumeration::CarIDFlag::InfantryV); + + Eigen::VectorXd P0_dig(11); + double radius; + int armor_num; + + if (is_balance) { + P0_dig << 1, 64, 1, 64, 1, 64, 0.4, 100, 1, 1, 1; + radius = 0.2; + armor_num = 2; + } else if (id == enumeration::CarIDFlag::Outpost) { + P0_dig << 1, 64, 1, 64, 1, 81, 0.4, 100, 1e-4, 0, 0; + radius = 0.2765; + armor_num = 3; + } else if (id == enumeration::CarIDFlag::Base) { + P0_dig << 1, 64, 1, 64, 1, 64, 0.4, 100, 1e-4, 0, 0; + radius = 0.3205; + armor_num = 3; + } else { + P0_dig << 1, 64, 1, 64, 1, 64, 0.4, 100, 1, 1, 1; + radius = 0.2; + armor_num = 4; + } + + predictors_.emplace(id, + CarPredictor { id, armor_xyz_in_world, armor_ypr_in_world, init_time_stamp, radius, + armor_num, P0_dig }); + ids_dirty_ = true; + } + } + + void RemovePredictorBySingleID(const enumeration::CarIDFlag& id) { + predictors_.erase(id); + ids_dirty_ = true; + } + + bool HasPredictor(const enumeration::CarIDFlag& id) const { + return predictors_.find(id) != predictors_.end(); + } + +private: + enumeration::CarIDFlag ids_ { enumeration::CarIDFlag::None }; + std::unordered_map predictors_; + mutable bool ids_dirty_ = true; +}; + +const enumeration ::ArmorIdFlag& CarPredictorManager::GetId() const { return pimpl_->GetId(); } + +std::shared_ptr CarPredictorManager::Predictor( + const std ::time_t& time_stamp) const { + return pimpl_->Predictor(time_stamp); +} + +void CarPredictorManager::SetPredictorBySingleID(const enumeration::CarIDFlag& id, + Eigen::Vector3d armor_xyz_in_world, Eigen::Vector3d armor_ypr_in_world, time_t time_stamp) { + return pimpl_->SetPredictorBySingleID(id, armor_xyz_in_world, armor_ypr_in_world, time_stamp); +} + +void CarPredictorManager::SetPredictorBySingleID( + const enumeration::CarIDFlag& id, time_t time_stamp) { + return pimpl_->SetPredictorBySingleID( + id, Eigen::Vector3d::Zero(), Eigen::Vector3d::Zero(), time_stamp); +} + +CarPredictorManager::CarPredictorManager() + : pimpl_(std::make_unique()) { } + +} diff --git a/src/tongji/predictor/car_predictor_manager.hpp b/src/tongji/predictor/car_predictor_manager.hpp new file mode 100644 index 0000000..149a223 --- /dev/null +++ b/src/tongji/predictor/car_predictor_manager.hpp @@ -0,0 +1,26 @@ +#pragma once + +#include + +#include "enum/car_id.hpp" +#include "interfaces/predictor.hpp" + +namespace world_exe::tongji::predictor { +class CarPredictorManager final : public interfaces::IPredictor { +public: + CarPredictorManager(); + const enumeration ::ArmorIdFlag& GetId() const override; + + std::shared_ptr Predictor( + const std ::time_t& time_stamp) const override; + + void SetPredictorBySingleID(const enumeration::CarIDFlag& id, + Eigen::Vector3d armor_xyz_in_world, Eigen::Vector3d armor_ypr_in_world, time_t time_stamp); + + void SetPredictorBySingleID(const enumeration::CarIDFlag& id, time_t time_stamp); + +private: + class Impl; + std::unique_ptr pimpl_; +}; +} \ No newline at end of file diff --git a/src/tongji/predictor/in_gimbal_control_armor.hpp b/src/tongji/predictor/in_gimbal_control_armor.hpp new file mode 100644 index 0000000..9f9fdae --- /dev/null +++ b/src/tongji/predictor/in_gimbal_control_armor.hpp @@ -0,0 +1,59 @@ +#pragma once + +#include + +#include "data/armor_gimbal_control_spacing.hpp" +#include "enum/car_id.hpp" +#include "interfaces/armor_in_gimbal_control.hpp" +#include "tongji/predictor/time_stamp.hpp" +#include "util/index.hpp" + +namespace world_exe::tongji::predictor { + +class InGimbalControlArmor final : public interfaces::IArmorInGimbalControl { +public: + InGimbalControlArmor() = default; + InGimbalControlArmor(const std::array, 8>& armors, + TimeStamp time_stamp) + : armors_(armors) + , time_stamp_(time_stamp) { } + + const std ::vector& GetArmors( + const enumeration ::ArmorIdFlag& armor_id) const override { + auto armor_id_index = util::enumeration::GetIndex(armor_id); + return armors_.at(armor_id_index); + } + + void Set(const std::array, 8>& armors, + const predictor::TimeStamp& predict_time_stamp) { + armors_ = armors; + time_stamp_ = predict_time_stamp; + } + + void Add( + enumeration::CarIDFlag car_id, const std::vector& armors) { + auto& target_vector = armors_.at(util::enumeration::GetIndex(car_id)); + target_vector.insert(target_vector.end(), armors.begin(), armors.end()); + } + + void Clear() { + for (auto& vec : armors_) { + vec.clear(); + } + } + + void SetArmorsByCarId( + const std::vector& armors, TimeStamp time_stamp) { + if (armors.empty()) return; + armors_[util::enumeration::GetIndex(armors.at(0).id)] = armors; + } + + const interfaces::ITimeStamped& GetTimeStamped() const override { return time_stamp_; } + + void SetTimeStamp(const TimeStamp time_stamp) { time_stamp_ = time_stamp; } + +private: + TimeStamp time_stamp_; + std::array, 8> armors_; +}; +} \ No newline at end of file diff --git a/src/tongji/predictor/time_stamp.hpp b/src/tongji/predictor/time_stamp.hpp new file mode 100644 index 0000000..ebcbc26 --- /dev/null +++ b/src/tongji/predictor/time_stamp.hpp @@ -0,0 +1,24 @@ +#pragma once + +#include "interfaces/time_stamped.hpp" + +namespace world_exe::tongji::predictor { +class TimeStamp : public interfaces::ITimeStamped { +public: + TimeStamp() = default; + TimeStamp(const std::time_t& time_stamp) + : time_stamp_(time_stamp) { } + + static TimeStamp FromRaw(std::time_t raw) { return TimeStamp(raw); } + + inline void SetTimeStamp(const time_t& time_stamp) { time_stamp_ = time_stamp; } + + double SecondsSince(const TimeStamp& other) const { + return std::difftime(time_stamp_, other.time_stamp_); + } + const std::time_t& GetTimeStamp() const override { return time_stamp_; }; + +private: + std::time_t time_stamp_; +}; +} \ No newline at end of file diff --git a/src/tongji/state_machine/car_state_manager.hpp b/src/tongji/state_machine/car_state_manager.hpp index da80ff9..8dca5eb 100644 --- a/src/tongji/state_machine/car_state_manager.hpp +++ b/src/tongji/state_machine/car_state_manager.hpp @@ -1,6 +1,8 @@ #pragma once -#include +#include + +#include "tongji/predictor/time_stamp.hpp" namespace world_exe::tongji::car_state { @@ -9,9 +11,10 @@ class CarStateManager { CarStateManager(int switch_threshold = 5) : switch_threshold_(switch_threshold) { } - void Update(bool detected, std::chrono::steady_clock::time_point now) { + void Update(bool detected, std::time_t now_raw) { if (detected) { count_ = std::min(count_ + 1, switch_threshold_); + auto now = predictor::TimeStamp::FromRaw(now_raw); last_seen_ = now; } else { count_ = std::max(count_ - 1, 0); @@ -19,6 +22,10 @@ class CarStateManager { is_locked_ = (count_ >= switch_threshold_); } + bool IsLost(const predictor::TimeStamp& now, double timeout_sec) const { + return now.SecondsSince(last_seen_) > timeout_sec; + } + bool IsLocked() const { return is_locked_; } bool IsConverged() const { return is_converged_; } bool IsDiverged() const { return is_diverged_; } @@ -32,7 +39,7 @@ class CarStateManager { void SetPriority(const int& p) { priority_ = p; } int GetPriority() const { return priority_; } - std::chrono::steady_clock::time_point LastSeen() const { return last_seen_; } + predictor::TimeStamp LastSeen() const { return last_seen_; } void IncrementUpdateCount() { update_count_++; @@ -50,6 +57,6 @@ class CarStateManager { bool is_converged_ = false; bool is_diverged_ = false; int priority_ = 100; // 默认最低优先级 - std::chrono::steady_clock::time_point last_seen_; + predictor::TimeStamp last_seen_; }; } \ No newline at end of file diff --git a/src/tongji/state_machine/state_machine.cpp b/src/tongji/state_machine/state_machine.cpp index 918adb3..442148d 100644 --- a/src/tongji/state_machine/state_machine.cpp +++ b/src/tongji/state_machine/state_machine.cpp @@ -14,7 +14,7 @@ StateMachine::StateMachine(int switch_threshold) const enumeration::CarIDFlag& StateMachine::GetAllowdToFires() const { return tracing_state_; } const interfaces::ICarState& StateMachine::Update( - const enumeration::CarIDFlag& car_detected, std::chrono::steady_clock::time_point now) { + const enumeration::CarIDFlag& car_detected, std::time_t now) { tracing_state_ = enumeration::CarIDFlag::None; for (int i = 0; i < static_cast(enumeration::CarIDFlag::Count); ++i) { diff --git a/src/tongji/state_machine/state_machine.hpp b/src/tongji/state_machine/state_machine.hpp index f6b55c9..8fb9872 100644 --- a/src/tongji/state_machine/state_machine.hpp +++ b/src/tongji/state_machine/state_machine.hpp @@ -1,6 +1,6 @@ #pragma once -#include +#include #include "car_state_manager.hpp" #include "enum/car_id.hpp" @@ -14,7 +14,7 @@ class StateMachine final : public interfaces::ICarState { const enumeration::CarIDFlag& GetAllowdToFires() const override; const interfaces::ICarState& Update( - const enumeration::CarIDFlag& car_detected, std::chrono::steady_clock::time_point now); + const enumeration::CarIDFlag& car_detected, std::time_t now); private: enumeration::CarIDFlag tracing_state_ = enumeration::CarIDFlag::None; diff --git a/src/util/extended_kalman_filter.hpp b/src/util/extended_kalman_filter.hpp new file mode 100644 index 0000000..9129372 --- /dev/null +++ b/src/util/extended_kalman_filter.hpp @@ -0,0 +1,115 @@ +#pragma once + +#include +#include +#include +#include +#include + +namespace world_exe::util { +class ExtendedKalmanFilter { +public: + Eigen::VectorXd x; + Eigen::MatrixXd P; + + ExtendedKalmanFilter() = default; + + ExtendedKalmanFilter( + const Eigen::VectorXd& x0, const Eigen::MatrixXd& P0, + std::function x_add = + [](const Eigen::VectorXd& a, const Eigen::VectorXd& b) { return a + b; }) { + data["residual_yaw"] = 0.0; + data["residual_pitch"] = 0.0; + data["residual_distance"] = 0.0; + data["residual_angle"] = 0.0; + data["nis"] = 0.0; + data["nees"] = 0.0; + data["nis_fail"] = 0.0; + data["nees_fail"] = 0.0; + data["recent_nis_failures"] = 0.0; + } + + Eigen::VectorXd predict(const Eigen::MatrixXd& F, const Eigen::MatrixXd& Q) { + return predict(F, Q, [&](const Eigen::VectorXd& x) { return F * x; }); + } + + Eigen::VectorXd predict(const Eigen::MatrixXd& F, const Eigen::MatrixXd& Q, + std::function f) { + P = F * P * F.transpose() + Q; + x = f(x); + return x; + } + + Eigen::VectorXd update( + const Eigen::VectorXd& z, const Eigen::MatrixXd& H, const Eigen::MatrixXd& R, + std::function z_subtract = + [](const Eigen::VectorXd& a, const Eigen::VectorXd& b) { return a - b; }) { + return update(z, H, R, [&](const Eigen::VectorXd& x) { return H * x; }, z_subtract); + } + + Eigen::VectorXd update( + const Eigen::VectorXd& z, const Eigen::MatrixXd& H, const Eigen::MatrixXd& R, + std::function h, + std::function z_subtract = + [](const Eigen::VectorXd& a, const Eigen::VectorXd& b) { return a - b; }) { + Eigen::VectorXd x_prior = x; + Eigen::MatrixXd K = P * H.transpose() * (H * P * H.transpose() + R).inverse(); + + // Stable Compution of the Posterior Covariance + // https://github.com/rlabbe/Kalman-and-Bayesian-Filters-in-Python/blob/master/07-Kalman-Filter-Math.ipynb + P = (I - K * H) * P * (I - K * H).transpose() + K * R * K.transpose(); + + x = x_add(x, K * z_subtract(z, h(x))); + + /// 卡方检验 + Eigen::VectorXd residual = z_subtract(z, h(x)); + // 新增检验 + Eigen::MatrixXd S = H * P * H.transpose() + R; + double nis = residual.transpose() * S.inverse() * residual; + double nees = (x - x_prior).transpose() * P.inverse() * (x - x_prior); + + // 卡方检验阈值（自由度=4，取置信水平95%） + constexpr double nis_threshold = 0.711; + constexpr double nees_threshold = 0.711; + + if (nis > nis_threshold) nis_count_++, data["nis_fail"] = 1; + if (nees > nees_threshold) nees_count_++, data["nees_fail"] = 1; + total_count_++; + last_nis = nis; + + recent_nis_failures.push_back(nis > nis_threshold ? 1 : 0); + + if (recent_nis_failures.size() > window_size) { + recent_nis_failures.pop_front(); + } + + int recent_failures = + std::accumulate(recent_nis_failures.begin(), recent_nis_failures.end(), 0); + double recent_rate = static_cast(recent_failures) / recent_nis_failures.size(); + + data["residual_yaw"] = residual[0]; + data["residual_pitch"] = residual[1]; + data["residual_distance"] = residual[2]; + data["residual_angle"] = residual[3]; + data["nis"] = nis; + data["nees"] = nees; + data["recent_nis_failures"] = recent_rate; + + return x; + } + + std::map data; // 卡方检验数据 + std::deque recent_nis_failures { 0 }; + size_t window_size = 100; + double last_nis; + +private: + Eigen::MatrixXd I; + std::function x_add; + + int nees_count_ = 0; + int nis_count_ = 0; + int total_count_ = 0; +}; + +} // namespace tools diff --git a/src/util/math.hpp b/src/util/math.hpp index a3ea6b0..6d91a7e 100644 --- a/src/util/math.hpp +++ b/src/util/math.hpp @@ -198,6 +198,32 @@ static inline Eigen::Vector3d xyz2ypd(const Eigen::Vector3d& xyz) { return { yaw, pitch, distance }; } +static Eigen::Matrix xyz2ypd_jacobian(const Eigen::Vector3d& xyz) { + auto x = xyz[0], y = xyz[1], z = xyz[2]; + + auto dyaw_dx = -y / (x * x + y * y); + auto dyaw_dy = x / (x * x + y * y); + auto dyaw_dz = 0.0; + + auto dpitch_dx = -(x * z) / ((z * z / (x * x + y * y) + 1) * std::pow((x * x + y * y), 1.5)); + auto dpitch_dy = -(y * z) / ((z * z / (x * x + y * y) + 1) * std::pow((x * x + y * y), 1.5)); + auto dpitch_dz = 1 / ((z * z / (x * x + y * y) + 1) * std::pow((x * x + y * y), 0.5)); + + auto ddistance_dx = x / std::pow((x * x + y * y + z * z), 0.5); + auto ddistance_dy = y / std::pow((x * x + y * y + z * z), 0.5); + auto ddistance_dz = z / std::pow((x * x + y * y + z * z), 0.5); + + // clang-format off + Eigen::Matrix J{ + {dyaw_dx, dyaw_dy, dyaw_dz}, + {dpitch_dx, dpitch_dy, dpitch_dz}, + {ddistance_dx, ddistance_dy, ddistance_dz} + }; + // clang-format on + + return J; +} + template T square(T const& a) { return a * a; }; } // namespace rmcs_auto_aim::util::math \ No newline at end of file From af9017c8648adb9902c5d7b4bff4d93a6cd175a6 Mon Sep 17 00:00:00 2001 From: heyeuu <2829004293@qq.com> Date: Mon, 15 Sep 2025 23:21:17 +0800 Subject: [PATCH 20/53] feat(env): Add libfmt-dev and libspdlog-dev dependencies --- env/ubuntu22.04.sh | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/env/ubuntu22.04.sh b/env/ubuntu22.04.sh index 7a28bea..e5c97b5 100644 --- a/env/ubuntu22.04.sh +++ b/env/ubuntu22.04.sh @@ -9,7 +9,7 @@ sudo gpg --output /etc/apt/trusted.gpg.d/intel.gpg --dearmor GPG-PUB-KEY-INTEL-S rm GPG-PUB-KEY-INTEL-SW-PRODUCTS.PUB echo "deb https://apt.repos.intel.com/openvino ubuntu22 main" | sudo tee /etc/apt/sources.list.d/intel-openvino.list sudo apt update -sudo apt install -y libtbb-dev libeigen3-dev libopencv-dev openvino gcc-13 g++-13 libceres-dev libdwarf-dev libbackward-cpp-dev binutils-dev libdw-dev +sudo apt install -y libtbb-dev libeigen3-dev libopencv-dev openvino gcc-13 g++-13 libceres-dev libdwarf-dev libbackward-cpp-dev binutils-dev libdw-dev libfmt-dev libspdlog-dev sudo ln -s /usr/include/libdwarf/libdwarf.h /usr/include/libdwarf.h From d0241830a1c1eff8b0a6f98e1ac2ed470958168b Mon Sep 17 00:00:00 2001 From: heyeuu <2829004293@qq.com> Date: Thu, 18 Sep 2025 02:17:26 +0800 Subject: [PATCH 21/53] refactor(interface): Rename the file of armors and change the armor interface implements to be read-only and immutable. --- src/tongji/identifier/identified_armor.hpp | 30 ++++++++++++++++++++ src/tongji/solver/solved_armor.hpp | 33 ++++++++++++++++++++++ 2 files changed, 63 insertions(+) create mode 100644 src/tongji/identifier/identified_armor.hpp create mode 100644 src/tongji/solver/solved_armor.hpp diff --git a/src/tongji/identifier/identified_armor.hpp b/src/tongji/identifier/identified_armor.hpp new file mode 100644 index 0000000..b6ac9ff --- /dev/null +++ b/src/tongji/identifier/identified_armor.hpp @@ -0,0 +1,30 @@ +#pragma once + +#include "enum/armor_id.hpp" +#include "interfaces/armor_in_image.hpp" +#include "interfaces/time_stamped.hpp" +#include "util/index.hpp" + +namespace world_exe::tongji::identifier { +class IdentifiedArmor final : public interfaces::IArmorInImage, public interfaces::ITimeStamped { +public: + explicit IdentifiedArmor(const std::vector& armors) { + for (const auto& armor : armors) { + armors_[util::enumeration::GetIndex(armor.id)].emplace_back(armor); + } + } + + const interfaces::ITimeStamped& GetTimeStamped() const override { return *this; } + + const std::time_t& GetTimeStamp() const override { return time_stamp_; }; + + const std::vector& GetArmors( + const enumeration::ArmorIdFlag& armor_id) const override { + return armors_[util::enumeration::GetIndex(armor_id)]; + } + +private: + std::time_t time_stamp_ { 0 }; + std::array, 8> armors_; +}; +} \ No newline at end of file diff --git a/src/tongji/solver/solved_armor.hpp b/src/tongji/solver/solved_armor.hpp new file mode 100644 index 0000000..43a79eb --- /dev/null +++ b/src/tongji/solver/solved_armor.hpp @@ -0,0 +1,33 @@ +#pragma once + +#include +#include + +#include "interfaces/armor_in_camera.hpp" +#include "interfaces/time_stamped.hpp" +#include "util/index.hpp" + +namespace world_exe::tongji::solver { +class SolvedArmor final : public interfaces::IArmorInCamera, public interfaces::ITimeStamped { +public: + explicit SolvedArmor(const std::vector& armors) { + for (const auto& armor : armors) { + armors_[util::enumeration::GetIndex(armor.id)].emplace_back(armor); + } + time_stamp_ = 0; + } + + const interfaces::ITimeStamped& GetTimeStamped() const override { return *this; } + + const std::time_t& GetTimeStamp() const override { return time_stamp_; }; + + const std::vector& GetArmors( + const enumeration::ArmorIdFlag& armor_id) const override { + return armors_[util::enumeration::GetIndex(armor_id)]; + } + +private: + std::time_t time_stamp_ { 0 }; + std::array, 8> armors_; +}; +} \ No newline at end of file From deef68016ccf6508bc4eb8b9643b8429af263313 Mon Sep 17 00:00:00 2001 From: heyeuu <2829004293@qq.com> Date: Thu, 18 Sep 2025 02:21:43 +0800 Subject: [PATCH 22/53] WIP: partially implement ITargetPredictor interface Note: The implement of ITargetPredictor interface is not fully implemented yet. --- src/tongji/identifier/identifier.cpp | 4 +- src/tongji/identifier/identifier_armor.hpp | 36 -- src/tongji/predictor/car_predictor.hpp | 420 ------------------ src/tongji/predictor/car_predictor_boss.cpp | 56 --- .../predictor/car_predictor_manager.cpp | 134 ------ .../predictor/car_predictor_manager.hpp | 26 -- .../predictor/in_gimbal_control_armor.hpp | 43 +- src/tongji/predictor/target.hpp | 331 ++++++++++++++ src/tongji/predictor/target_predictor.cpp | 97 ++++ ...redictor_boss.hpp => target_predictor.hpp} | 10 +- .../predictor/target_snapshot_predictor.cpp | 88 ++++ .../predictor/target_snapshot_predictor.hpp | 22 + src/tongji/predictor/time_stamp.hpp | 5 +- src/tongji/solver/solver.cpp | 4 +- src/tongji/solver/solver_armor.hpp | 39 -- .../state_machine/car_state_manager.hpp | 4 +- src/util/extended_kalman_filter.hpp | 6 +- 17 files changed, 562 insertions(+), 763 deletions(-) delete mode 100644 src/tongji/identifier/identifier_armor.hpp delete mode 100644 src/tongji/predictor/car_predictor.hpp delete mode 100644 src/tongji/predictor/car_predictor_boss.cpp delete mode 100644 src/tongji/predictor/car_predictor_manager.cpp delete mode 100644 src/tongji/predictor/car_predictor_manager.hpp create mode 100644 src/tongji/predictor/target.hpp create mode 100644 src/tongji/predictor/target_predictor.cpp rename src/tongji/predictor/{car_predictor_boss.hpp => target_predictor.hpp} (70%) create mode 100644 src/tongji/predictor/target_snapshot_predictor.cpp create mode 100644 src/tongji/predictor/target_snapshot_predictor.hpp delete mode 100644 src/tongji/solver/solver_armor.hpp diff --git a/src/tongji/identifier/identifier.cpp b/src/tongji/identifier/identifier.cpp index 0b2c382..c314f74 100644 --- a/src/tongji/identifier/identifier.cpp +++ b/src/tongji/identifier/identifier.cpp @@ -17,7 +17,7 @@ #include "data/armor_image_spaceing.hpp" #include "enum/armor_id.hpp" -#include "identifier_armor.hpp" +#include "identified_armor.hpp" #include "interfaces/armor_in_image.hpp" #include "tongji/identifier/classifier.hpp" #include "util/logger.hpp" @@ -191,7 +191,7 @@ class Identifier::Impl { all_car_id |= static_cast(armor.id); } - return { std::make_shared(armor_plates), + return { std::make_shared(armor_plates), static_cast(all_car_id) }; } diff --git a/src/tongji/identifier/identifier_armor.hpp b/src/tongji/identifier/identifier_armor.hpp deleted file mode 100644 index ec94241..0000000 --- a/src/tongji/identifier/identifier_armor.hpp +++ /dev/null @@ -1,36 +0,0 @@ -#pragma once - -#include "enum/armor_id.hpp" -#include "interfaces/armor_in_image.hpp" -#include "interfaces/time_stamped.hpp" -#include "util/index.hpp" - -namespace world_exe::tongji::identifier { -class IdentifierArmor final : public interfaces::IArmorInImage, public interfaces::ITimeStamped { -public: - explicit IdentifierArmor(const std::vector& armors) { - SetArmors(armors); - } - - const interfaces::ITimeStamped& GetTimeStamped() const override { return *this; } - - const std::time_t& GetTimeStamp() const override { return time_stamp_; }; - - void SetArmors(const std::vector& armors) { - std::array, 8> temp_armors; - for (const auto& armor : armors) { - temp_armors[util::enumeration::GetIndex(armor.id)].emplace_back(armor); - } - armors_ = std::move(temp_armors); - } - - const std::vector& GetArmors( - const enumeration::ArmorIdFlag& armor_id) const override { - return armors_[util::enumeration::GetIndex(armor_id)]; - } - -private: - std::time_t time_stamp_ { 0 }; - std::array, 8> armors_; -}; -} \ No newline at end of file diff --git a/src/tongji/predictor/car_predictor.hpp b/src/tongji/predictor/car_predictor.hpp deleted file mode 100644 index 4ce3fbe..0000000 --- a/src/tongji/predictor/car_predictor.hpp +++ /dev/null @@ -1,420 +0,0 @@ -#pragma once - -#include -#include - -#include "data/armor_gimbal_control_spacing.hpp" -#include "enum/armor_id.hpp" -#include "enum/car_id.hpp" -#include "interfaces/predictor_update_package.hpp" -#include "tongji/predictor/time_stamp.hpp" -#include "util/extended_kalman_filter.hpp" -#include "util/math.hpp" - -namespace world_exe::tongji::predictor { - -struct PredictorStatus { - bool jumped = false; - bool switched = false; - bool converged = false; - bool diverged = false; - bool lost = false; - bool reidentified = false; - int switch_count = 0; - int update_count = 0; - int lost_count = 0; -}; - -class CarPredictor { -public: - CarPredictor() = default; - explicit CarPredictor(const enumeration::ArmorIdFlag armor_id, - const Eigen::Vector3d& armor_xyz_in_world, const Eigen::Vector3d& armor_ypr_in_world, - std::time_t time_stamp, double radius, int armor_num, Eigen::VectorXd P0_dig) - : armor_id_(armor_id) - , last_id(0) - , radius_(radius) - , armor_num_(armor_num) - , last_predict_time_stamp_(time_stamp) { - const Eigen::VectorXd& xyz = armor_xyz_in_world; - const Eigen::VectorXd& ypr = armor_ypr_in_world; - - // 旋转中心的坐标 - auto center_x = xyz[0] + radius * std::cos(ypr[0]); - auto center_y = xyz[1] + radius * std::sin(ypr[0]); - auto center_z = xyz[2]; - - // x vx y vy z vz a w r l h - // a: angle - // w: angular velocity - // l: r2 - r1 - // h: z2 - z1 - Eigen::VectorXd x0 { { center_x, 0, center_y, 0, center_z, 0, ypr[0], 0, radius, 0, - 0 } }; // 初始化预测量 - P0_dig_ = P0_dig; - Eigen::MatrixXd P0 = P0_dig.asDiagonal(); - - // 防止夹角求和出现异常值 - x_add_ = [](const Eigen::VectorXd& a, const Eigen::VectorXd& b) -> Eigen::VectorXd { - Eigen::VectorXd c = a + b; - c[6] = util::math::clamp_pm_pi(c[6]); - return c; - }; - - ekf_ = util::ExtendedKalmanFilter(x0, P0, x_add_); // 初始化滤波器（预测量、预测量协方差） - } - - void SetEkf(const util::ExtendedKalmanFilter& ekf) { ekf_ = ekf; } - - std::vector GetPredictedArmors( - enumeration::ArmorIdFlag armor_id, const TimeStamp time_stamp) { - PredictTo(time_stamp.GetTimeStamp()); - UpdateStatus(); - - // 装甲板匹配 - int id; - auto min_angle_error = 1e10; - const std::vector& xyza_list = armor_xyza_list(); - - std::vector> xyza_i_list; - for (int i = 0; i < armor_num_; i++) { - xyza_i_list.push_back({ xyza_list[i], i }); - } - - std::sort(xyza_i_list.begin(), xyza_i_list.end(), - [](const std::pair& a, const std::pair& b) { - Eigen::Vector3d ypd1 = util::math::xyz2ypd(a.first.head(3)); - Eigen::Vector3d ypd2 = util::math::xyz2ypd(b.first.head(3)); - return ypd1[2] < ypd2[2]; - }); - - std::vector armors; - for (int i = 0; i < armor_num_; i++) { - xyza_i_list.push_back({ xyza_list[i], i }); - const auto pos = xyza_i_list[i].first.head<3>(); - const auto yaw = util::math::clamp_pm_pi(ekf_.x[6]); - const double pitch = 15. / 180. * std::numbers::pi; - armors.push_back(data::ArmorGimbalControlSpacing { - armor_id_, pos, util::math::euler_to_quaternion(yaw, pitch, 0.) }); - } - return armors; - } - - void Update(std::shared_ptr data) { - const auto& transform = data->GetTransform(); - const auto& rotation_transform = Eigen::Quaterniond(transform.linear()); - const auto armors = data->GetArmors()->GetArmors(armor_id_); - - if (armors.empty()) return; - for (auto armor : armors) { - const auto& armor_in_world_position = transform * armor.position; - const auto& armor_in_world_oritaiton = rotation_transform * armor.orientation; - this->update(armor_in_world_position, - util::math::quaternion_to_euler(armor_in_world_oritaiton, 2, 1, 0), - util::math::xyz2ypd(armor_in_world_position)); - } - } - - const PredictorStatus& GetStatus() const { return status_; } - - void PredictTo(const TimeStamp& now) { - if (last_predict_time_stamp_.GetTimeStamp() == 0) { - last_predict_time_stamp_ = now; - return; - } - const double& dt = now.SecondsSince(last_predict_time_stamp_); - last_predict_time_stamp_ = now; - - if (dt <= 0.0 || dt > 1.0) { - // util::logger::logger()->warn( - // "[CarPredictor] Abnormal dt: {:.3f}, skipping prediction", dt); - return; - } - - predict(dt); - } - - void Reset(const Eigen::Vector3d& xyz, const Eigen::Vector3d& ypr, std::time_t time_stamp) { - last_predict_time_stamp_ = TimeStamp::FromRaw(time_stamp); - status_ = PredictorStatus {}; // 清空状态计数器 - - // 重新构造滤波器状态 - auto center_x = xyz[0] + radius_ * std::cos(ypr[0]); - auto center_y = xyz[1] + radius_ * std::sin(ypr[0]); - auto center_z = xyz[2]; - - Eigen::VectorXd x0 { { center_x, 0, center_y, 0, center_z, 0, ypr[0], 0, radius_, 0, 0 } }; - Eigen::MatrixXd P0 = P0_dig_.asDiagonal(); - - ekf_ = util::ExtendedKalmanFilter(x0, P0_dig_, x_add_); - } - - void TickStatus() { - static int last_update_count = -1; - if (status_.update_count == last_update_count) { - status_.lost_count++; - } else { - status_.lost_count = 0; - } - last_update_count = status_.update_count; - status_.lost = (status_.lost_count > 30 || IsDiverged()); - status_.reidentified = (!status_.lost && IsConverged() && status_.update_count > 3); - } - - bool IsLocked() const { return status_.converged && !status_.diverged; } - bool IsLost() const { return status_.lost; } - bool IsReidentified() const { return status_.reidentified; } - -private: - bool IsConverged() const { return status_.converged; } - bool IsDiverged() const { return status_.diverged; } - - int MatchArmor(const Eigen::Vector3d& armor_xyz_in_world, - const Eigen::Vector3d& armor_ypr_in_world, const Eigen::Vector3d& armor_ypd_in_world) { - - const auto& xyza_list = armor_xyza_list(); - std::vector> xyza_i_list; - - for (int i = 0; i < armor_num_; ++i) { - xyza_i_list.emplace_back(xyza_list[i], i); - } - - std::sort(xyza_i_list.begin(), xyza_i_list.end(), [](const auto& a, const auto& b) { - auto ypd1 = util::math::xyz2ypd(a.first.head(3)); - auto ypd2 = util::math::xyz2ypd(b.first.head(3)); - return ypd1[2] < ypd2[2]; - }); - - int best_id = 0; - double min_error = 1e10; - - for (int i = 0; i < std::min(3, armor_num_); ++i) { - const auto& xyza = xyza_i_list[i].first; - auto ypd = util::math::xyz2ypd(xyza.head(3)); - double error = std::abs(util::math::clamp_pm_pi(armor_ypr_in_world[0] - xyza[3])) - + std::abs(util::math::clamp_pm_pi(armor_ypd_in_world[0] - ypd[0])); - - if (error < min_error) { - min_error = error; - best_id = xyza_i_list[i].second; - } - } - - return best_id; - } - - auto F(double dt) const -> Eigen::MatrixXd { - // 状态转移矩阵 - // clang-format off - return Eigen::MatrixXd { - {1, dt, 0, 0, 0, 0, 0, 0, 0, 0, 0}, - {0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0}, - {0, 0, 1, dt, 0, 0, 0, 0, 0, 0, 0}, - {0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0}, - {0, 0, 0, 0, 1, dt, 0, 0, 0, 0, 0}, - {0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0}, - {0, 0, 0, 0, 0, 0, 1, dt, 0, 0, 0}, - {0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0}, - {0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0}, - {0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0}, - {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1} - }; - // clang-format on - } - - auto Q(double dt) const -> Eigen::MatrixXd { - // Piecewise White Noise Model - // https://github.com/rlabbe/Kalman-and-Bayesian-Filters-in-Python/blob/master/07-Kalman-Filter-Math.ipynb - double v1, v2; - if (armor_id_ == enumeration::CarIDFlag::Outpost) { - v1 = 10; // 前哨站加速度方差 - v2 = 0.1; // 前哨站角加速度方差 - } else { - v1 = 100; // 加速度方差 - v2 = 400; // 角加速度方差 - } - auto a = dt * dt * dt * dt / 4; - auto b = dt * dt * dt / 2; - auto c = dt * dt; - // 预测过程噪声偏差的方差 - // clang-format off - return Eigen::MatrixXd { - {a * v1, b * v1, 0, 0, 0, 0, 0, 0, 0, 0, 0}, - {b * v1, c * v1, 0, 0, 0, 0, 0, 0, 0, 0, 0}, - { 0, 0, a * v1, b * v1, 0, 0, 0, 0, 0, 0, 0}, - { 0, 0, b * v1, c * v1, 0, 0, 0, 0, 0, 0, 0}, - { 0, 0, 0, 0, a * v1, b * v1, 0, 0, 0, 0, 0}, - { 0, 0, 0, 0, b * v1, c * v1, 0, 0, 0, 0, 0}, - { 0, 0, 0, 0, 0, 0, a * v2, b * v2, 0, 0, 0}, - { 0, 0, 0, 0, 0, 0, b * v2, c * v2, 0, 0, 0}, - { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}, - { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}, - { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0} - }; - // clang-format on - } - - void predict(const double& dt) { - // 防止夹角求和出现异常值 - auto f = [&](const Eigen::VectorXd& x) -> Eigen::VectorXd { - Eigen::VectorXd x_prior = this->F(dt) * x; - x_prior[6] = util::math::clamp_pm_pi(x_prior[6]); - return x_prior; - }; - - // 前哨站转速特判 - if (IsConverged() && this->armor_id_ == enumeration::CarIDFlag::Outpost - && std::abs(this->ekf_.x[7]) > 2) - this->ekf_.x[7] = this->ekf_.x[7] > 0 ? 2.51 : -2.51; - - ekf_.predict(this->F(dt), this->Q(dt), f); - } - - void UpdateStatus() { - // 发散判断 - double r = ekf_.x[8]; - double l = ekf_.x[9]; - status_.diverged = !(r > 0.05 && r < 0.5 && r + l > 0.05 && r + l < 0.5); - - // 收敛判断 - if (armor_id_ == enumeration::CarIDFlag::Outpost) { - status_.converged = (status_.update_count > 10 && !status_.diverged); - } else { - status_.converged = (status_.update_count > 3 && !status_.diverged); - } - } - - void update(const Eigen::Vector3d& armor_xyz_in_world, - const Eigen::Vector3d& armor_ypr_in_world, const Eigen::Vector3d& armor_ypd_in_world) { - // 装甲板匹配 - int id = MatchArmor(armor_xyz_in_world, armor_ypr_in_world, armor_ypd_in_world); - - if (id != 0) status_.jumped = true; - status_.switched = (id != last_id); - if (status_.switched) status_.switch_count++; - - last_id = id; - update_ypda(armor_xyz_in_world, armor_ypr_in_world, armor_ypd_in_world, id); - status_.update_count++; - UpdateStatus(); - } - - void update_ypda(const Eigen::Vector3d& armor_xyz_in_world, - const Eigen::Vector3d& armor_ypr_in_world, const Eigen::Vector3d& armor_ypd_in_world, - int id) { - // 观测jacobi - Eigen::MatrixXd H = h_jacobian(ekf_.x, id); - // Eigen::VectorXd R_dig{{4e-3, 4e-3, 1, 9e-2}}; - auto center_yaw = std::atan2(armor_xyz_in_world[1], armor_xyz_in_world[0]); - auto delta_angle = util::math::clamp_pm_pi(armor_ypr_in_world[0] - center_yaw); - Eigen::VectorXd R_dig { { 4e-3, 4e-3, log(std::abs(delta_angle) + 1) + 1, - log(std::abs(armor_ypd_in_world[2]) + 1) / 200 + 9e-2 } }; - - // 测量过程噪声偏差的方差 - Eigen::MatrixXd R = R_dig.asDiagonal(); - - // 定义非线性转换函数h: x -> z - auto h = [&](const Eigen::VectorXd& x) -> Eigen::Vector4d { - Eigen::VectorXd xyz = h_armor_xyz(x, id); - Eigen::VectorXd ypd = util::math::xyz2ypd(xyz); - auto angle = util::math::clamp_pm_pi(x[6] + id * 2 * CV_PI / armor_num_); - return { ypd[0], ypd[1], ypd[2], angle }; - }; - - // 防止夹角求差出现异常值 - auto z_subtract = [](const Eigen::VectorXd& a, - const Eigen::VectorXd& b) -> Eigen::VectorXd { - Eigen::VectorXd c = a - b; - c[0] = util::math::clamp_pm_pi(c[0]); - c[1] = util::math::clamp_pm_pi(c[1]); - c[3] = util::math::clamp_pm_pi(c[3]); - return c; - }; - - const Eigen::VectorXd& ypd = armor_ypd_in_world; - const Eigen::VectorXd& ypr = armor_ypr_in_world; - Eigen::VectorXd z { { ypd[0], ypd[1], ypd[2], ypr[0] } }; // 获得观测量 - - ekf_.update(z, H, R, h, z_subtract); - } - - Eigen::VectorXd ekf_x() const { return ekf_.x; } - - const util::ExtendedKalmanFilter& ekf() const { return ekf_; } - - std::vector armor_xyza_list() const { - std::vector _armor_xyza_list; - - for (int i = 0; i < armor_num_; i++) { - auto angle = util::math::clamp_pm_pi(ekf_.x[6] + i * 2 * CV_PI / armor_num_); - Eigen::Vector3d xyz = h_armor_xyz(ekf_.x, i); - _armor_xyza_list.push_back({ xyz[0], xyz[1], xyz[2], angle }); - } - return _armor_xyza_list; - } - - // 计算出装甲板中心的坐标（考虑长短轴） - Eigen::Vector3d h_armor_xyz(const Eigen::VectorXd& x, int id) const { - auto angle = util::math::clamp_pm_pi(x[6] + id * 2 * CV_PI / armor_num_); - auto use_l_h = (armor_num_ == 4) && (id == 1 || id == 3); - - auto r = (use_l_h) ? x[8] + x[9] : x[8]; - auto armor_x = x[0] - r * std::cos(angle); - auto armor_y = x[2] - r * std::sin(angle); - auto armor_z = (use_l_h) ? x[4] + x[10] : x[4]; - - return { armor_x, armor_y, armor_z }; - } - - Eigen::MatrixXd h_jacobian(const Eigen::VectorXd& x, int id) const { - auto angle = util::math::clamp_pm_pi(x[6] + id * 2 * CV_PI / armor_num_); - auto use_l_h = (armor_num_ == 4) && (id == 1 || id == 3); - - auto r = (use_l_h) ? x[8] + x[9] : x[8]; - auto dx_da = r * std::sin(angle); - auto dy_da = -r * std::cos(angle); - - auto dx_dr = -std::cos(angle); - auto dy_dr = -std::sin(angle); - auto dx_dl = (use_l_h) ? -std::cos(angle) : 0.0; - auto dy_dl = (use_l_h) ? -std::sin(angle) : 0.0; - - auto dz_dh = (use_l_h) ? 1.0 : 0.0; - - // clang-format off - Eigen::MatrixXd H_armor_xyza{ - {1, 0, 0, 0, 0, 0, dx_da, 0, dx_dr, dx_dl, 0}, - {0, 0, 1, 0, 0, 0, dy_da, 0, dy_dr, dy_dl, 0}, - {0, 0, 0, 0, 1, 0, 0, 0, 0, 0, dz_dh}, - {0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0} - }; - // clang-format on - - Eigen::VectorXd armor_xyz = h_armor_xyz(x, id); - Eigen::MatrixXd H_armor_ypd = util::math::xyz2ypd_jacobian(armor_xyz); - // clang-format off - Eigen::MatrixXd H_armor_ypda{ - {H_armor_ypd(0, 0), H_armor_ypd(0, 1), H_armor_ypd(0, 2), 0}, - {H_armor_ypd(1, 0), H_armor_ypd(1, 1), H_armor_ypd(1, 2), 0}, - {H_armor_ypd(2, 0), H_armor_ypd(2, 1), H_armor_ypd(2, 2), 0}, - { 0, 0, 0, 1} - }; - // clang-format on - - return H_armor_ypda * H_armor_xyza; - } - - int armor_num_; - double radius_; - PredictorStatus status_; - - enumeration::ArmorIdFlag armor_id_; - int last_id = -1; // debug only - - std::function x_add_; - Eigen::VectorXd P0_dig_; - util::ExtendedKalmanFilter ekf_; - TimeStamp last_predict_time_stamp_; -}; - -} \ No newline at end of file diff --git a/src/tongji/predictor/car_predictor_boss.cpp b/src/tongji/predictor/car_predictor_boss.cpp deleted file mode 100644 index af82658..0000000 --- a/src/tongji/predictor/car_predictor_boss.cpp +++ /dev/null @@ -1,56 +0,0 @@ -#include "car_predictor_boss.hpp" - -#include -#include -#include - -#include "enum/car_id.hpp" -#include "enum/enum_tools.hpp" -#include "tongji/predictor/car_predictor_manager.hpp" - -namespace world_exe::tongji::predictor { -class CarPredictorBoss::Impl { -public: - Impl(std::shared_ptr manager) - : manager_(manager) { - if (!manager_) { - throw std::invalid_argument("CarPredictorManager must not be null"); - } - } - - std ::shared_ptr Predict( - const enumeration ::ArmorIdFlag& id, const std ::time_t& time_stamp) { - for (int i = 0; i < 8; ++i) { - auto car_id = static_cast( - static_cast(enumeration::CarIDFlag::Hero) << i); - - if (enumeration::IsFlagContains(id, car_id)) { - manager_->SetPredictorBySingleID(car_id, time_stamp); - } - } - return manager_->Predictor(time_stamp); - } - - std ::shared_ptr GetPredictor( - const enumeration ::ArmorIdFlag& id) const { - return manager_; - } - -private: - std::shared_ptr manager_; -}; - -std ::shared_ptr CarPredictorBoss::Predict( - const enumeration ::ArmorIdFlag& id, const std ::time_t& time_stamp) { - return pimpl_->Predict(id, time_stamp); -} - -std ::shared_ptr CarPredictorBoss::GetPredictor( - const enumeration ::ArmorIdFlag& id) const { - return pimpl_->GetPredictor(id); -} - -CarPredictorBoss::CarPredictorBoss(std::shared_ptr manager) - : pimpl_(std::make_unique(manager)) { } - -} \ No newline at end of file diff --git a/src/tongji/predictor/car_predictor_manager.cpp b/src/tongji/predictor/car_predictor_manager.cpp deleted file mode 100644 index 1572cde..0000000 --- a/src/tongji/predictor/car_predictor_manager.cpp +++ /dev/null @@ -1,134 +0,0 @@ -#include "car_predictor_manager.hpp" - -#include -#include -#include -#include - -#include "enum/armor_id.hpp" -#include "enum/car_id.hpp" -#include "tongji/predictor/car_predictor.hpp" -#include "tongji/predictor/in_gimbal_control_armor.hpp" -#include "tongji/predictor/time_stamp.hpp" - -namespace world_exe::tongji::predictor { -class CarPredictorManager::Impl { -public: - Impl() = default; - - const enumeration ::CarIDFlag& GetId() { - if (ids_dirty_) { - ids_ = enumeration::CarIDFlag::None; - for (const auto& [id, _] : predictors_) { - ids_ = static_cast( - static_cast(ids_) | static_cast(id)); - } - ids_dirty_ = false; - } - return ids_; - } - - auto Predictor(const std ::time_t& time_stamp) { - InGimbalControlArmor target_armors; - for (auto& predictor : predictors_) { - target_armors.Add( - predictor.first, predictor.second.GetPredictedArmors(predictor.first, time_stamp)); - } - return std::make_shared(target_armors); - } - - void Tick(const TimeStamp& now) { - std::vector to_remove; - - for (auto& [id, predictor] : predictors_) { - predictor.PredictTo(now); - predictor.TickStatus(); - - if (predictor.IsLost()) { - to_remove.push_back(id); - } - } - // logger()->info("Removing lost predictor: {}", static_cast(id)); - for (auto id : to_remove) { - predictors_.erase(id); - } - } - - void SetPredictorBySingleID(const enumeration::CarIDFlag& id, - const Eigen::Vector3d& armor_xyz_in_world, const Eigen::Vector3d& armor_ypr_in_world, - std::time_t init_time_stamp) { - - auto it = predictors_.find(id); - if (it != predictors_.end()) { - it->second.Reset(armor_xyz_in_world, armor_ypr_in_world, init_time_stamp); - } else { - bool is_balance = (id == enumeration::CarIDFlag::InfantryIII - | id == enumeration::CarIDFlag::InfantryIV - | id == enumeration::CarIDFlag::InfantryV); - - Eigen::VectorXd P0_dig(11); - double radius; - int armor_num; - - if (is_balance) { - P0_dig << 1, 64, 1, 64, 1, 64, 0.4, 100, 1, 1, 1; - radius = 0.2; - armor_num = 2; - } else if (id == enumeration::CarIDFlag::Outpost) { - P0_dig << 1, 64, 1, 64, 1, 81, 0.4, 100, 1e-4, 0, 0; - radius = 0.2765; - armor_num = 3; - } else if (id == enumeration::CarIDFlag::Base) { - P0_dig << 1, 64, 1, 64, 1, 64, 0.4, 100, 1e-4, 0, 0; - radius = 0.3205; - armor_num = 3; - } else { - P0_dig << 1, 64, 1, 64, 1, 64, 0.4, 100, 1, 1, 1; - radius = 0.2; - armor_num = 4; - } - - predictors_.emplace(id, - CarPredictor { id, armor_xyz_in_world, armor_ypr_in_world, init_time_stamp, radius, - armor_num, P0_dig }); - ids_dirty_ = true; - } - } - - void RemovePredictorBySingleID(const enumeration::CarIDFlag& id) { - predictors_.erase(id); - ids_dirty_ = true; - } - - bool HasPredictor(const enumeration::CarIDFlag& id) const { - return predictors_.find(id) != predictors_.end(); - } - -private: - enumeration::CarIDFlag ids_ { enumeration::CarIDFlag::None }; - std::unordered_map predictors_; - mutable bool ids_dirty_ = true; -}; - -const enumeration ::ArmorIdFlag& CarPredictorManager::GetId() const { return pimpl_->GetId(); } - -std::shared_ptr CarPredictorManager::Predictor( - const std ::time_t& time_stamp) const { - return pimpl_->Predictor(time_stamp); -} - -void CarPredictorManager::SetPredictorBySingleID(const enumeration::CarIDFlag& id, - Eigen::Vector3d armor_xyz_in_world, Eigen::Vector3d armor_ypr_in_world, time_t time_stamp) { - return pimpl_->SetPredictorBySingleID(id, armor_xyz_in_world, armor_ypr_in_world, time_stamp); -} - -void CarPredictorManager::SetPredictorBySingleID( - const enumeration::CarIDFlag& id, time_t time_stamp) { - return pimpl_->SetPredictorBySingleID( - id, Eigen::Vector3d::Zero(), Eigen::Vector3d::Zero(), time_stamp); -} - -CarPredictorManager::CarPredictorManager() - : pimpl_(std::make_unique()) { } - -} diff --git a/src/tongji/predictor/car_predictor_manager.hpp b/src/tongji/predictor/car_predictor_manager.hpp deleted file mode 100644 index 149a223..0000000 --- a/src/tongji/predictor/car_predictor_manager.hpp +++ /dev/null @@ -1,26 +0,0 @@ -#pragma once - -#include - -#include "enum/car_id.hpp" -#include "interfaces/predictor.hpp" - -namespace world_exe::tongji::predictor { -class CarPredictorManager final : public interfaces::IPredictor { -public: - CarPredictorManager(); - const enumeration ::ArmorIdFlag& GetId() const override; - - std::shared_ptr Predictor( - const std ::time_t& time_stamp) const override; - - void SetPredictorBySingleID(const enumeration::CarIDFlag& id, - Eigen::Vector3d armor_xyz_in_world, Eigen::Vector3d armor_ypr_in_world, time_t time_stamp); - - void SetPredictorBySingleID(const enumeration::CarIDFlag& id, time_t time_stamp); - -private: - class Impl; - std::unique_ptr pimpl_; -}; -} \ No newline at end of file diff --git a/src/tongji/predictor/in_gimbal_control_armor.hpp b/src/tongji/predictor/in_gimbal_control_armor.hpp index 9f9fdae..a8f48c9 100644 --- a/src/tongji/predictor/in_gimbal_control_armor.hpp +++ b/src/tongji/predictor/in_gimbal_control_armor.hpp @@ -1,59 +1,36 @@ #pragma once +#include #include #include "data/armor_gimbal_control_spacing.hpp" -#include "enum/car_id.hpp" +#include "enum/armor_id.hpp" #include "interfaces/armor_in_gimbal_control.hpp" #include "tongji/predictor/time_stamp.hpp" -#include "util/index.hpp" namespace world_exe::tongji::predictor { class InGimbalControlArmor final : public interfaces::IArmorInGimbalControl { public: - InGimbalControlArmor() = default; - InGimbalControlArmor(const std::array, 8>& armors, + InGimbalControlArmor( + std::unordered_map> + armors, TimeStamp time_stamp) : armors_(armors) , time_stamp_(time_stamp) { } const std ::vector& GetArmors( const enumeration ::ArmorIdFlag& armor_id) const override { - auto armor_id_index = util::enumeration::GetIndex(armor_id); - return armors_.at(armor_id_index); - } - - void Set(const std::array, 8>& armors, - const predictor::TimeStamp& predict_time_stamp) { - armors_ = armors; - time_stamp_ = predict_time_stamp; - } - - void Add( - enumeration::CarIDFlag car_id, const std::vector& armors) { - auto& target_vector = armors_.at(util::enumeration::GetIndex(car_id)); - target_vector.insert(target_vector.end(), armors.begin(), armors.end()); - } - - void Clear() { - for (auto& vec : armors_) { - vec.clear(); - } - } - - void SetArmorsByCarId( - const std::vector& armors, TimeStamp time_stamp) { - if (armors.empty()) return; - armors_[util::enumeration::GetIndex(armors.at(0).id)] = armors; + static const std::vector empty; + auto it = armors_.find(armor_id); + return it != armors_.end() ? it->second : empty; } const interfaces::ITimeStamped& GetTimeStamped() const override { return time_stamp_; } - void SetTimeStamp(const TimeStamp time_stamp) { time_stamp_ = time_stamp; } - private: TimeStamp time_stamp_; - std::array, 8> armors_; + std::unordered_map> + armors_; }; } \ No newline at end of file diff --git a/src/tongji/predictor/target.hpp b/src/tongji/predictor/target.hpp new file mode 100644 index 0000000..48d88dc --- /dev/null +++ b/src/tongji/predictor/target.hpp @@ -0,0 +1,331 @@ +#pragma once + +#include +#include + +#include "enum/armor_id.hpp" +#include "enum/car_id.hpp" +#include "interfaces/predictor_update_package.hpp" +#include "tongji/predictor/time_stamp.hpp" +#include "util/extended_kalman_filter.hpp" +#include "util/math.hpp" + +namespace world_exe::tongji::predictor { + +struct TargetStatus { + bool jumped = false; + bool switched = false; + bool converged = false; + bool diverged = false; + bool lost = false; + bool reidentified = false; + int last_id = -1; + int switch_count = 0; + int update_count = 0; + int lost_count = 0; +}; + +class Target { +public: + TargetStatus status_; + + Target(const Eigen::Vector3d& armor_xyz_in_world, const Eigen::Vector3d& armor_ypr_in_world, + const enumeration::ArmorIdFlag& id, const std::time_t& t, const double& radius, + const int& armor_num, const Eigen::VectorXd& P0_dig) + : time_stamp_(t) + , car_id_(id) + , armor_num_(armor_num) { + + auto r = radius; + + // 旋转中心的坐标 + auto center_x = armor_xyz_in_world[0] + r * std::cos(armor_ypr_in_world[0]); + auto center_y = armor_xyz_in_world[1] + r * std::sin(armor_ypr_in_world[0]); + auto center_z = armor_xyz_in_world[2]; + + // x vx y vy z vz a w r l h + // a: angle + // w: angular velocity + // l: r2 - r1 + // h: z2 - z1 + Eigen::VectorXd x0 { { center_x, 0, center_y, 0, center_z, 0, armor_ypr_in_world[0], 0, r, + 0, 0 } }; // 初始化预测量 + Eigen::MatrixXd P0 = P0_dig.asDiagonal(); + + // 防止夹角求和出现异常值 + auto x_add = [](const Eigen::VectorXd& a, const Eigen::VectorXd& b) -> Eigen::VectorXd { + Eigen::VectorXd c = a + b; + c[6] = util::math::clamp_pm_pi(c[6]); + return c; + }; + + ekf_ = util::ExtendedKalmanFilter(x0, P0, x_add); // 初始化滤波器（预测量、预测量协方差） + } + + Eigen::VectorXd GetEkf_x() const { return ekf_.x; } + + void Predict(double dt) { + // 防止夹角求和出现异常值 + auto f = [&](const Eigen::VectorXd& x) -> Eigen::VectorXd { + Eigen::VectorXd x_prior = this->F(dt) * x; + x_prior[6] = util::math::clamp_pm_pi(x_prior[6]); + return x_prior; + }; + + // 前哨站转速特判 + if (this->Convergened() && this->car_id_ == enumeration::CarIDFlag::Outpost + && std::abs(this->ekf_.x[7]) > 2) + this->ekf_.x[7] = this->ekf_.x[7] > 0 ? 2.51 : -2.51; + + ekf_.Predict(this->F(dt), this->Q(dt), f); + } + + std::time_t GetTimeStamp() const { return time_stamp_.GetTimeStamp(); } + int GetArmorNum() const { return armor_num_; } + enumeration::CarIDFlag GetId() const { return car_id_; } + + void Update(std::shared_ptr data) { + const auto& transform = data->GetTransform(); + const auto& rotation_transform = Eigen::Quaterniond(transform.linear()); + const auto armors = data->GetArmors()->GetArmors(car_id_); + + if (armors.empty()) return; + for (auto armor : armors) { + const auto& armor_in_world_position = transform * armor.position; + const auto& armor_in_world_oritaiton = rotation_transform * armor.orientation; + this->Update(armor_in_world_position, + util::math::quaternion_to_euler(armor_in_world_oritaiton, 2, 1, 0), + util::math::xyz2ypd(armor_in_world_position)); + } + } + + bool Convergened() { + if (this->car_id_ != enumeration::CarIDFlag::Outpost && status_.update_count > 3 + && !this->Diverged()) { + status_.converged = true; + } + + // 前哨站特殊判断 + if (this->car_id_ == enumeration::CarIDFlag::Outpost && status_.update_count > 10 + && !this->Diverged()) { + status_.converged = true; + } + + return status_.converged; + } + + bool Diverged() const { + auto r_ok = ekf_.x[8] > 0.05 && ekf_.x[8] < 0.5; + auto l_ok = ekf_.x[8] + ekf_.x[9] > 0.05 && ekf_.x[8] + ekf_.x[9] < 0.5; + + if (r_ok && l_ok) return false; + + // util::logger::logger()->debug("[Target] r={:.3f}, l={:.3f}", ekf_.x[8], ekf_.x[9]); + return true; + } + +private: + void Update(const Eigen::Vector3d& armor_xyz_in_world, + const Eigen::Vector3d& armor_ypr_in_world, const Eigen::Vector3d& armor_ypd_in_world) { + // 装甲板匹配 + int id = MatchArmor(armor_xyz_in_world, armor_ypr_in_world, armor_ypd_in_world); + if (id != 0) status_.jumped = true; + status_.switched = (id != status_.last_id); + if (status_.switched) status_.switch_count++; + + status_.last_id = id; + Update_ypda(armor_xyz_in_world, armor_ypr_in_world, armor_ypd_in_world, id); + status_.update_count++; + } + + void Update_ypda(const Eigen::Vector3d& armor_xyz_in_world, + const Eigen::Vector3d& armor_ypr_in_world, const Eigen::Vector3d& armor_ypd_in_world, + int id) { + // 观测jacobi + Eigen::MatrixXd H = h_jacobian(ekf_.x, id); + // Eigen::VectorXd R_dig{{4e-3, 4e-3, 1, 9e-2}}; + auto center_yaw = std::atan2(armor_xyz_in_world[1], armor_xyz_in_world[0]); + auto delta_angle = util::math::clamp_pm_pi(armor_ypr_in_world[0] - center_yaw); + Eigen::VectorXd R_dig { { 4e-3, 4e-3, log(std::abs(delta_angle) + 1) + 1, + log(std::abs(armor_ypd_in_world[2]) + 1) / 200 + 9e-2 } }; + + // 测量过程噪声偏差的方差 + Eigen::MatrixXd R = R_dig.asDiagonal(); + + // 定义非线性转换函数h: x -> z + auto h = [&](const Eigen::VectorXd& x) -> Eigen::Vector4d { + Eigen::VectorXd xyz = h_armor_xyz(x, id); + Eigen::VectorXd ypd = util::math::xyz2ypd(xyz); + auto angle = util::math::clamp_pm_pi(x[6] + id * 2 * CV_PI / armor_num_); + return { ypd[0], ypd[1], ypd[2], angle }; + }; + + // 防止夹角求差出现异常值 + auto z_subtract = [](const Eigen::VectorXd& a, + const Eigen::VectorXd& b) -> Eigen::VectorXd { + Eigen::VectorXd c = a - b; + c[0] = util::math::clamp_pm_pi(c[0]); + c[1] = util::math::clamp_pm_pi(c[1]); + c[3] = util::math::clamp_pm_pi(c[3]); + return c; + }; + + const Eigen::VectorXd& ypd = armor_ypd_in_world; + const Eigen::VectorXd& ypr = armor_ypr_in_world; + Eigen::VectorXd z { { ypd[0], ypd[1], ypd[2], ypr[0] } }; // 获得观测量 + + ekf_.update(z, H, R, h, z_subtract); + } + + Eigen::MatrixXd h_jacobian(const Eigen::VectorXd& x, int id) const { + auto angle = util::math::clamp_pm_pi(x[6] + id * 2 * CV_PI / armor_num_); + auto use_l_h = (armor_num_ == 4) && (id == 1 || id == 3); + + auto r = (use_l_h) ? x[8] + x[9] : x[8]; + auto dx_da = r * std::sin(angle); + auto dy_da = -r * std::cos(angle); + + auto dx_dr = -std::cos(angle); + auto dy_dr = -std::sin(angle); + auto dx_dl = (use_l_h) ? -std::cos(angle) : 0.0; + auto dy_dl = (use_l_h) ? -std::sin(angle) : 0.0; + + auto dz_dh = (use_l_h) ? 1.0 : 0.0; + + // clang-format off + Eigen::MatrixXd H_armor_xyza{ + {1, 0, 0, 0, 0, 0, dx_da, 0, dx_dr, dx_dl, 0}, + {0, 0, 1, 0, 0, 0, dy_da, 0, dy_dr, dy_dl, 0}, + {0, 0, 0, 0, 1, 0, 0, 0, 0, 0, dz_dh}, + {0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0} + }; + // clang-format on + + Eigen::VectorXd armor_xyz = h_armor_xyz(x, id); + Eigen::MatrixXd H_armor_ypd = util::math::xyz2ypd_jacobian(armor_xyz); + // clang-format off + Eigen::MatrixXd H_armor_ypda{ + {H_armor_ypd(0, 0), H_armor_ypd(0, 1), H_armor_ypd(0, 2), 0}, + {H_armor_ypd(1, 0), H_armor_ypd(1, 1), H_armor_ypd(1, 2), 0}, + {H_armor_ypd(2, 0), H_armor_ypd(2, 1), H_armor_ypd(2, 2), 0}, + { 0, 0, 0, 1} + }; + // clang-format on + + return H_armor_ypda * H_armor_xyza; + } + + int MatchArmor(const Eigen::Vector3d& armor_xyz_in_world, + const Eigen::Vector3d& armor_ypr_in_world, const Eigen::Vector3d& armor_ypd_in_world) { + + const auto& xyza_list = armor_xyza_list(); + std::vector> xyza_i_list; + + for (int i = 0; i < armor_num_; ++i) { + xyza_i_list.emplace_back(xyza_list[i], i); + } + + std::sort(xyza_i_list.begin(), xyza_i_list.end(), [](const auto& a, const auto& b) { + auto ypd1 = util::math::xyz2ypd(a.first.head(3)); + auto ypd2 = util::math::xyz2ypd(b.first.head(3)); + return ypd1[2] < ypd2[2]; + }); + + int best_id = 0; + double min_error = 1e10; + + for (int i = 0; i < std::min(3, armor_num_); ++i) { + const auto& xyza = xyza_i_list[i].first; + auto ypd = util::math::xyz2ypd(xyza.head(3)); + double error = std::abs(util::math::clamp_pm_pi(armor_ypr_in_world[0] - xyza[3])) + + std::abs(util::math::clamp_pm_pi(armor_ypd_in_world[0] - ypd[0])); + + if (error < min_error) { + min_error = error; + best_id = xyza_i_list[i].second; + } + } + + return best_id; + } + + std::vector armor_xyza_list() const { + std::vector _armor_xyza_list; + + for (int i = 0; i < armor_num_; i++) { + auto angle = util::math::clamp_pm_pi(ekf_.x[6] + i * 2 * CV_PI / armor_num_); + Eigen::Vector3d xyz = h_armor_xyz(ekf_.x, i); + _armor_xyza_list.push_back({ xyz[0], xyz[1], xyz[2], angle }); + } + return _armor_xyza_list; + } + + // 计算出装甲板中心的坐标（考虑长短轴） + Eigen::Vector3d h_armor_xyz(const Eigen::VectorXd& x, int id) const { + auto angle = util::math::clamp_pm_pi(x[6] + id * 2 * CV_PI / armor_num_); + auto use_l_h = (armor_num_ == 4) && (id == 1 || id == 3); + + auto r = (use_l_h) ? x[8] + x[9] : x[8]; + auto armor_x = x[0] - r * std::cos(angle); + auto armor_y = x[2] - r * std::sin(angle); + auto armor_z = (use_l_h) ? x[4] + x[10] : x[4]; + + return { armor_x, armor_y, armor_z }; + } + + auto F(double dt) -> Eigen::MatrixXd { + // 状态转移矩阵 + // clang-format off + return (Eigen::MatrixXd(11, 11) << + 1, dt, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 1, dt, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 1, dt, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 1, dt, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1).finished(); +} + +auto Q(double dt)->Eigen::MatrixXd{ + // Piecewise White Noise Model + // https://github.com/rlabbe/Kalman-and-Bayesian-Filters-in-Python/blob/master/07-Kalman-Filter-Math.ipynb + double v1, v2; + if (car_id_ == enumeration::CarIDFlag::Outpost) { + v1 = 10; // 前哨站加速度方差 + v2 = 0.1; // 前哨站角加速度方差 + } else { + v1 = 100; // 加速度方差 + v2 = 400; // 角加速度方差 + } + auto a = dt * dt * dt * dt / 4; + auto b = dt * dt * dt / 2; + auto c = dt * dt; + + // 预测过程噪声偏差的方差 + // clang-format off + return (Eigen::MatrixXd(11, 11) << + a * v1, b * v1, 0, 0, 0, 0, 0, 0, 0, 0, 0, + b * v1, c * v1, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, a * v1, b * v1, 0, 0, 0, 0, 0, 0, 0, + 0, 0, b * v1, c * v1, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, a * v1, b * v1, 0, 0, 0, 0, 0, + 0, 0, 0, 0, b * v1, c * v1, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, a * v2, b * v2, 0, 0, 0, + 0, 0, 0, 0, 0, 0, b * v2, c * v2, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0).finished(); + // clang-format on + } + + TimeStamp time_stamp_; + util::ExtendedKalmanFilter ekf_; + enumeration::CarIDFlag car_id_; + int armor_num_; +}; + +} \ No newline at end of file diff --git a/src/tongji/predictor/target_predictor.cpp b/src/tongji/predictor/target_predictor.cpp new file mode 100644 index 0000000..ee576ca --- /dev/null +++ b/src/tongji/predictor/target_predictor.cpp @@ -0,0 +1,97 @@ +#include "target_predictor.hpp" + +#include +#include +#include + +#include "enum/car_id.hpp" +#include "enum/enum_tools.hpp" +#include "target.hpp" +#include "tongji/predictor/target_snapshot_predictor.hpp" + +namespace world_exe::tongji::predictor { + +std::vector ConstructTargets(const enumeration::ArmorIdFlag& id, std::time_t time_stamp) { + std::vector targets; + + for (int i = 0; i < 8; ++i) { + auto car_id = static_cast(1 << i); + if (!enumeration::IsFlagContains(id, car_id)) continue; + + bool is_balance = (car_id == enumeration::CarIDFlag::InfantryIII + || car_id == enumeration::CarIDFlag::InfantryIV + || car_id == enumeration::CarIDFlag::InfantryV); + + Eigen::VectorXd P0_dig(11); + double radius; + int armor_num; + + if (is_balance) { + P0_dig << 1, 64, 1, 64, 1, 64, 0.4, 100, 1, 1, 1; + radius = 0.2; + armor_num = 2; + } else if (car_id == enumeration::CarIDFlag::Outpost) { + P0_dig << 1, 64, 1, 64, 1, 81, 0.4, 100, 1e-4, 0, 0; + radius = 0.2765; + armor_num = 3; + } else if (car_id == enumeration::CarIDFlag::Base) { + P0_dig << 1, 64, 1, 64, 1, 64, 0.4, 100, 1e-4, 0, 0; + radius = 0.3205; + armor_num = 3; + } else { + P0_dig << 1, 64, 1, 64, 1, 64, 0.4, 100, 1, 1, 1; + radius = 0.2; + armor_num = 4; + } + + Eigen::Vector3d armor_xyz_in_world = Eigen::Vector3d::Zero(); + Eigen::Vector3d armor_ypr_in_world = Eigen::Vector3d::Zero(); + + targets.emplace_back(Target( + armor_xyz_in_world, armor_ypr_in_world, car_id, time_stamp, radius, armor_num, P0_dig)); + return targets; + } + + return targets; +} + +class TargetPredictor::Impl { +public: + Impl(const std::vector& targets) + : selected_targets_(targets) { } + + std::shared_ptr Predict( + const enumeration::ArmorIdFlag& id, const std::time_t& time_stamp) { + + } + + std::shared_ptr GetPredictor(const enumeration::ArmorIdFlag& id) const { + auto targets = ConstructTargets(id, last_update_time_stamp_); + return std::make_shared(targets); + } + + void Update(const std::shared_ptr& data) { + for (auto target : selected_targets_) { + target.Update(data); + } + } + +private: + std::vector selected_targets_; + + std::time_t last_update_time_stamp_ { 0 }; +}; + +std ::shared_ptr TargetPredictor::Predict( + const enumeration ::ArmorIdFlag& id, const std ::time_t& time_stamp) { + return pimpl_->Predict(id, time_stamp); +} + +std ::shared_ptr TargetPredictor::GetPredictor( + const enumeration ::ArmorIdFlag& id) const { + return pimpl_->GetPredictor(id); +} + +TargetPredictor::TargetPredictor(const enumeration::ArmorIdFlag& id, std::time_t t) + : pimpl_(std::make_unique(ConstructTargets(id, t))) { } +} \ No newline at end of file diff --git a/src/tongji/predictor/car_predictor_boss.hpp b/src/tongji/predictor/target_predictor.hpp similarity index 70% rename from src/tongji/predictor/car_predictor_boss.hpp rename to src/tongji/predictor/target_predictor.hpp index 35b7198..8958794 100644 --- a/src/tongji/predictor/car_predictor_boss.hpp +++ b/src/tongji/predictor/target_predictor.hpp @@ -3,19 +3,17 @@ #include #include "interfaces/target_predictor.hpp" -#include "tongji/predictor/car_predictor_manager.hpp" namespace world_exe::tongji::predictor { -class CarPredictorBoss final : public interfaces::ITargetPredictor { +class TargetPredictor final : public interfaces::ITargetPredictor { public: - CarPredictorBoss(std::shared_ptr manager); + TargetPredictor(const enumeration::ArmorIdFlag& id, std::time_t t); + std ::shared_ptr Predict( + const enumeration ::ArmorIdFlag& id, const std ::time_t& time_stamp) override; std ::shared_ptr GetPredictor( const enumeration ::ArmorIdFlag& id) const override; - std ::shared_ptr Predict( - const enumeration ::ArmorIdFlag& id, const std ::time_t& time_stamp) override; - private: class Impl; std::unique_ptr pimpl_; diff --git a/src/tongji/predictor/target_snapshot_predictor.cpp b/src/tongji/predictor/target_snapshot_predictor.cpp new file mode 100644 index 0000000..4bc42f9 --- /dev/null +++ b/src/tongji/predictor/target_snapshot_predictor.cpp @@ -0,0 +1,88 @@ +#include "target_snapshot_predictor.hpp" + +#include +#include +#include + +#include +#include + +#include "data/armor_gimbal_control_spacing.hpp" +#include "enum/armor_id.hpp" +#include "enum/enum_tools.hpp" +#include "in_gimbal_control_armor.hpp" +#include "target.hpp" +#include "util/math.hpp" + +namespace world_exe::tongji::predictor { +class MultiTargetSnapshotPredictor::Impl { +public: + Impl(const std::vector& targets) + : targets_(targets) + , last_time_stamp_(0) { + id_ = enumeration::ArmorIdFlag::Unknow; + for (const auto& target : targets_) { + id_ = static_cast( + static_cast(id_) | static_cast(target.GetId())); + } + } + + const enumeration::ArmorIdFlag& GetId() const { return id_; } + + std::shared_ptr Predictor(const std::time_t& time_stamp) { + std::unordered_map> + armor_map; + const double dt = static_cast(time_stamp - last_time_stamp_); + + for (auto& target : targets_) { + Target iteration_target = target; + iteration_target.Predict(dt); + + Eigen::VectorXd iteration_x = iteration_target.GetEkf_x(); + std::vector armors; + + for (int j = 0; j < iteration_target.GetArmorNum(); ++j) { + auto angle = util::math::clamp_pm_pi( + iteration_x[6] + j * 2 * CV_PI / iteration_target.GetArmorNum()); + Eigen::Vector3d xyz = h_armor_xyz(iteration_x, j, iteration_target.GetArmorNum()); + data::ArmorGimbalControlSpacing armor; + armor.id = iteration_target.GetId(); + armor.position = xyz; + armor.orientation = util::math::euler_to_quaternion(angle, 15 / 180. * CV_PI, 0); + } + armor_map[iteration_target.GetId()] = armors; + } + return std::make_shared(armor_map, last_time_stamp_); + } + +private: + std::vector targets_; + enumeration::ArmorIdFlag id_; + std::time_t last_time_stamp_; + + // 计算出装甲板中心的坐标（考虑长短轴） + Eigen::Vector3d h_armor_xyz(const Eigen::VectorXd& x, int id, int armor_num) const { + auto angle = util::math::clamp_pm_pi(x[6] + id * 2 * CV_PI / armor_num); + auto use_l_h = (armor_num == 4) && (id == 1 || id == 3); + + auto r = (use_l_h) ? x[8] + x[9] : x[8]; + auto armor_x = x[0] - r * std::cos(angle); + auto armor_y = x[2] - r * std::sin(angle); + auto armor_z = (use_l_h) ? x[4] + x[10] : x[4]; + + return { armor_x, armor_y, armor_z }; + } +}; + +MultiTargetSnapshotPredictor::MultiTargetSnapshotPredictor(const std::vector& targets) + : pimpl_(std::make_unique(targets)) { } + +const enumeration ::ArmorIdFlag& MultiTargetSnapshotPredictor::GetId() const { + return pimpl_->GetId(); +} + +std ::shared_ptr MultiTargetSnapshotPredictor::Predictor( + const std ::time_t& time_stamp) const { + return pimpl_->Predictor(time_stamp); +} +} \ No newline at end of file diff --git a/src/tongji/predictor/target_snapshot_predictor.hpp b/src/tongji/predictor/target_snapshot_predictor.hpp new file mode 100644 index 0000000..cea2fad --- /dev/null +++ b/src/tongji/predictor/target_snapshot_predictor.hpp @@ -0,0 +1,22 @@ +#pragma once + +#include + +#include "interfaces/predictor.hpp" +#include "tongji/predictor/target.hpp" + +namespace world_exe::tongji::predictor { + +class MultiTargetSnapshotPredictor final : public interfaces::IPredictor { +public: + MultiTargetSnapshotPredictor(const std::vector& targets); + const enumeration ::ArmorIdFlag& GetId() const override; + + std ::shared_ptr Predictor( + const std ::time_t& time_stamp) const override; + +private: + class Impl; + std::unique_ptr pimpl_; +}; +} diff --git a/src/tongji/predictor/time_stamp.hpp b/src/tongji/predictor/time_stamp.hpp index ebcbc26..90deacf 100644 --- a/src/tongji/predictor/time_stamp.hpp +++ b/src/tongji/predictor/time_stamp.hpp @@ -5,13 +5,10 @@ namespace world_exe::tongji::predictor { class TimeStamp : public interfaces::ITimeStamped { public: - TimeStamp() = default; TimeStamp(const std::time_t& time_stamp) : time_stamp_(time_stamp) { } - static TimeStamp FromRaw(std::time_t raw) { return TimeStamp(raw); } - - inline void SetTimeStamp(const time_t& time_stamp) { time_stamp_ = time_stamp; } + void SetTimeStamp(const std::time_t& time_stamp) { time_stamp_ = time_stamp; } double SecondsSince(const TimeStamp& other) const { return std::difftime(time_stamp_, other.time_stamp_); diff --git a/src/tongji/solver/solver.cpp b/src/tongji/solver/solver.cpp index 2213d5a..2202025 100644 --- a/src/tongji/solver/solver.cpp +++ b/src/tongji/solver/solver.cpp @@ -8,7 +8,7 @@ #include "data/armor_gimbal_control_spacing.hpp" #include "parameters/profile.hpp" #include "parameters/rm_parameters.hpp" -#include "solver_armor.hpp" +#include "solved_armor.hpp" #include "util/coordinate.hpp" #include "util/index.hpp" #include "util/math.hpp" @@ -36,7 +36,7 @@ class Solver::Impl { } } } - return std::make_shared(armor_plates); + return std::make_shared(armor_plates); } std::optional Solve( diff --git a/src/tongji/solver/solver_armor.hpp b/src/tongji/solver/solver_armor.hpp deleted file mode 100644 index 1e8145b..0000000 --- a/src/tongji/solver/solver_armor.hpp +++ /dev/null @@ -1,39 +0,0 @@ -#pragma once - -#include -#include - -#include "interfaces/armor_in_camera.hpp" -#include "interfaces/time_stamped.hpp" -#include "util/index.hpp" - -namespace world_exe::tongji::solver { -class SolverArmor final : public interfaces::IArmorInCamera, public interfaces::ITimeStamped { -public: - explicit SolverArmor(const std::vector& armors) { - SetArmors(armors); - time_stamp_ = 0; - } - - const interfaces::ITimeStamped& GetTimeStamped() const override { return *this; } - - const std::time_t& GetTimeStamp() const override { return time_stamp_; }; - - void SetArmors(const std::vector& armors) { - std::array, 8> temp_armors; - for (const auto& armor : armors) { - temp_armors[util::enumeration::GetIndex(armor.id)].emplace_back(armor); - } - armors_ = std::move(temp_armors); - } - - const std::vector& GetArmors( - const enumeration::ArmorIdFlag& armor_id) const override { - return armors_[util::enumeration::GetIndex(armor_id)]; - } - -private: - std::time_t time_stamp_ { 0 }; - std::array, 8> armors_; -}; -} \ No newline at end of file diff --git a/src/tongji/state_machine/car_state_manager.hpp b/src/tongji/state_machine/car_state_manager.hpp index 8dca5eb..219e457 100644 --- a/src/tongji/state_machine/car_state_manager.hpp +++ b/src/tongji/state_machine/car_state_manager.hpp @@ -14,7 +14,7 @@ class CarStateManager { void Update(bool detected, std::time_t now_raw) { if (detected) { count_ = std::min(count_ + 1, switch_threshold_); - auto now = predictor::TimeStamp::FromRaw(now_raw); + auto now = predictor::TimeStamp(now_raw); last_seen_ = now; } else { count_ = std::max(count_ - 1, 0); @@ -57,6 +57,6 @@ class CarStateManager { bool is_converged_ = false; bool is_diverged_ = false; int priority_ = 100; // 默认最低优先级 - predictor::TimeStamp last_seen_; + predictor::TimeStamp last_seen_ { 0 }; }; } \ No newline at end of file diff --git a/src/util/extended_kalman_filter.hpp b/src/util/extended_kalman_filter.hpp index 9129372..8b1a2e0 100644 --- a/src/util/extended_kalman_filter.hpp +++ b/src/util/extended_kalman_filter.hpp @@ -30,17 +30,17 @@ class ExtendedKalmanFilter { } Eigen::VectorXd predict(const Eigen::MatrixXd& F, const Eigen::MatrixXd& Q) { - return predict(F, Q, [&](const Eigen::VectorXd& x) { return F * x; }); + return Predict(F, Q, [&](const Eigen::VectorXd& x) { return F * x; }); } - Eigen::VectorXd predict(const Eigen::MatrixXd& F, const Eigen::MatrixXd& Q, + Eigen::VectorXd Predict(const Eigen::MatrixXd& F, const Eigen::MatrixXd& Q, std::function f) { P = F * P * F.transpose() + Q; x = f(x); return x; } - Eigen::VectorXd update( + Eigen::VectorXd Update( const Eigen::VectorXd& z, const Eigen::MatrixXd& H, const Eigen::MatrixXd& R, std::function z_subtract = [](const Eigen::VectorXd& a, const Eigen::VectorXd& b) { return a - b; }) { From 00fdd5e790ed974641d6672880bb9866448a22ed Mon Sep 17 00:00:00 2001 From: heyeuu <2829004293@qq.com> Date: Thu, 18 Sep 2025 21:52:23 +0800 Subject: [PATCH 23/53] WIP: partially implement ITargetPredictor interface --- src/tongji/predictor/target.hpp | 1 - src/tongji/predictor/target_predict.cpp | 118 ++++++++++++++++++++++ src/tongji/predictor/target_predict.hpp | 15 +++ src/tongji/predictor/target_predictor.cpp | 29 +----- 4 files changed, 137 insertions(+), 26 deletions(-) create mode 100644 src/tongji/predictor/target_predict.cpp create mode 100644 src/tongji/predictor/target_predict.hpp diff --git a/src/tongji/predictor/target.hpp b/src/tongji/predictor/target.hpp index 48d88dc..3f6ec3c 100644 --- a/src/tongji/predictor/target.hpp +++ b/src/tongji/predictor/target.hpp @@ -119,7 +119,6 @@ class Target { auto l_ok = ekf_.x[8] + ekf_.x[9] > 0.05 && ekf_.x[8] + ekf_.x[9] < 0.5; if (r_ok && l_ok) return false; - // util::logger::logger()->debug("[Target] r={:.3f}, l={:.3f}", ekf_.x[8], ekf_.x[9]); return true; } diff --git a/src/tongji/predictor/target_predict.cpp b/src/tongji/predictor/target_predict.cpp new file mode 100644 index 0000000..bed8a9e --- /dev/null +++ b/src/tongji/predictor/target_predict.cpp @@ -0,0 +1,118 @@ +#include "target_predict.hpp" + +#include +#include +#include + +#include "enum/car_id.hpp" +#include "enum/enum_tools.hpp" +#include "tongji/predictor/in_gimbal_control_armor.hpp" +#include "tongji/predictor/target.hpp" +#include "tongji/predictor/target_snapshot_predictor.hpp" + +namespace world_exe::tongji::predictor { + +class TargetPredict::Impl { +public: + Impl(std::list targets) { + for (auto target : targets) { + targets_map_.insert({ target.GetId(), target }); + } + } + + std::shared_ptr Predict( + const enumeration::ArmorIdFlag& id, const std::time_t& time_stamp) { + std::unordered_map> + armor_map; + + const auto& dt = time_stamp - last_time_stamp_; + last_time_stamp_ = time_stamp; + + for (auto& target_value : targets_map_) { + target_value.second.Predict(dt); + auto x = target_value.second.GetEkf_x(); + + for (int j = 0; j < target_value.second.GetArmorNum(); ++j) { + auto angle = util::math::clamp_pm_pi( + x[6] + j * 2 * CV_PI / target_value.second.GetArmorNum()); + Eigen::Vector3d xyz = h_armor_xyz(x, j, target_value.second.GetArmorNum()); + data::ArmorGimbalControlSpacing armor; + armor.id = target_value.second.GetId(); + armor.position = xyz; + armor.orientation = util::math::euler_to_quaternion(angle, 15 / 180. * CV_PI, 0); + } + } + return std::make_shared(armor_map, last_time_stamp_); + } + + std::shared_ptr GetPredictor(const enumeration::ArmorIdFlag& id) const { + std::vector targets; + for (int i; i < 8; i++) { + const auto& car_id = static_cast(1 << i); + if (enumeration::IsFlagContains(id, car_id)) { + targets.emplace_back(ConstructTargets(car_id, )); // TODO + } + } + return std::make_shared(targets); + } + +private: + std::vector ConstructTargets( + const enumeration::ArmorIdFlag& id, std::time_t& time_stamp) const { + std::vector targets; + for (int i = 0; i < 8; ++i) { + auto car_id = static_cast(1 << i); + if (!enumeration::IsFlagContains(id, car_id)) continue; + + bool is_balance = (car_id == enumeration::CarIDFlag::InfantryIII + || car_id == enumeration::CarIDFlag::InfantryIV + || car_id == enumeration::CarIDFlag::InfantryV); + + Eigen::VectorXd P0_dig(11); + double radius; + int armor_num; + + if (is_balance) { + P0_dig << 1, 64, 1, 64, 1, 64, 0.4, 100, 1, 1, 1; + radius = 0.2; + armor_num = 2; + } else if (car_id == enumeration::CarIDFlag::Outpost) { + P0_dig << 1, 64, 1, 64, 1, 81, 0.4, 100, 1e-4, 0, 0; + radius = 0.2765; + armor_num = 3; + } else if (car_id == enumeration::CarIDFlag::Base) { + P0_dig << 1, 64, 1, 64, 1, 64, 0.4, 100, 1e-4, 0, 0; + radius = 0.3205; + armor_num = 3; + } else { + P0_dig << 1, 64, 1, 64, 1, 64, 0.4, 100, 1, 1, 1; + radius = 0.2; + armor_num = 4; + } + // 不知道这个armor_xyz_in_world、armor_ypr_in_world（可以理解为x）可以从哪里传入 + Eigen::Vector3d armor_xyz_in_world = Eigen::Vector3d::Zero(); + Eigen::Vector3d armor_ypr_in_world = Eigen::Vector3d::Zero(); + targets.emplace_back(Target(armor_xyz_in_world, armor_ypr_in_world, car_id, time_stamp, + radius, armor_num, P0_dig)); + } + return targets; + } + + // 计算出装甲板中心的坐标（考虑长短轴） + Eigen::Vector3d h_armor_xyz(const Eigen::VectorXd& x, int id, int armor_num) const { + auto angle = util::math::clamp_pm_pi(x[6] + id * 2 * CV_PI / armor_num); + auto use_l_h = (armor_num == 4) && (id == 1 || id == 3); + + auto r = (use_l_h) ? x[8] + x[9] : x[8]; + auto armor_x = x[0] - r * std::cos(angle); + auto armor_y = x[2] - r * std::sin(angle); + auto armor_z = (use_l_h) ? x[4] + x[10] : x[4]; + + return { armor_x, armor_y, armor_z }; + } + + std::time_t last_time_stamp_ { 0 }; + std::unordered_map targets_map_; +}; + +} \ No newline at end of file diff --git a/src/tongji/predictor/target_predict.hpp b/src/tongji/predictor/target_predict.hpp new file mode 100644 index 0000000..0acf243 --- /dev/null +++ b/src/tongji/predictor/target_predict.hpp @@ -0,0 +1,15 @@ +#pragma once + +#include + +#include "interfaces/target_predictor.hpp" + +namespace world_exe::tongji::predictor { +class TargetPredict final : public interfaces::ITargetPredictor { +public: + +private: + class Impl; + std::unique_ptr pimpl_; +}; +} \ No newline at end of file diff --git a/src/tongji/predictor/target_predictor.cpp b/src/tongji/predictor/target_predictor.cpp index ee576ca..9697080 100644 --- a/src/tongji/predictor/target_predictor.cpp +++ b/src/tongji/predictor/target_predictor.cpp @@ -7,13 +7,10 @@ #include "enum/car_id.hpp" #include "enum/enum_tools.hpp" #include "target.hpp" -#include "tongji/predictor/target_snapshot_predictor.hpp" namespace world_exe::tongji::predictor { std::vector ConstructTargets(const enumeration::ArmorIdFlag& id, std::time_t time_stamp) { - std::vector targets; - for (int i = 0; i < 8; ++i) { auto car_id = static_cast(1 << i); if (!enumeration::IsFlagContains(id, car_id)) continue; @@ -46,39 +43,21 @@ std::vector ConstructTargets(const enumeration::ArmorIdFlag& id, std::ti Eigen::Vector3d armor_xyz_in_world = Eigen::Vector3d::Zero(); Eigen::Vector3d armor_ypr_in_world = Eigen::Vector3d::Zero(); - - targets.emplace_back(Target( - armor_xyz_in_world, armor_ypr_in_world, car_id, time_stamp, radius, armor_num, P0_dig)); - return targets; } - - return targets; } class TargetPredictor::Impl { public: - Impl(const std::vector& targets) - : selected_targets_(targets) { } + Impl() { } std::shared_ptr Predict( - const enumeration::ArmorIdFlag& id, const std::time_t& time_stamp) { - - } + const enumeration::ArmorIdFlag& id, const std::time_t& time_stamp) { } std::shared_ptr GetPredictor(const enumeration::ArmorIdFlag& id) const { - auto targets = ConstructTargets(id, last_update_time_stamp_); - return std::make_shared(targets); - } - - void Update(const std::shared_ptr& data) { - for (auto target : selected_targets_) { - target.Update(data); - } + } private: - std::vector selected_targets_; - std::time_t last_update_time_stamp_ { 0 }; }; @@ -93,5 +72,5 @@ std ::shared_ptr TargetPredictor::GetPredictor( } TargetPredictor::TargetPredictor(const enumeration::ArmorIdFlag& id, std::time_t t) - : pimpl_(std::make_unique(ConstructTargets(id, t))) { } + : pimpl_() { } } \ No newline at end of file From 9596d7ad9997b3bc5a58eed4db983ad2b20e6bff Mon Sep 17 00:00:00 2001 From: heyeuu <2829004293@qq.com> Date: Thu, 18 Sep 2025 21:54:30 +0800 Subject: [PATCH 24/53] WIP: partially implement ITargetPredictor interface --- src/tongji/predictor/target_predict.cpp | 118 ----------------- src/tongji/predictor/target_predict.hpp | 15 --- src/tongji/predictor/target_predictor.cpp | 150 ++++++++++++++-------- src/tongji/predictor/target_predictor.hpp | 10 +- 4 files changed, 98 insertions(+), 195 deletions(-) delete mode 100644 src/tongji/predictor/target_predict.cpp delete mode 100644 src/tongji/predictor/target_predict.hpp diff --git a/src/tongji/predictor/target_predict.cpp b/src/tongji/predictor/target_predict.cpp deleted file mode 100644 index bed8a9e..0000000 --- a/src/tongji/predictor/target_predict.cpp +++ /dev/null @@ -1,118 +0,0 @@ -#include "target_predict.hpp" - -#include -#include -#include - -#include "enum/car_id.hpp" -#include "enum/enum_tools.hpp" -#include "tongji/predictor/in_gimbal_control_armor.hpp" -#include "tongji/predictor/target.hpp" -#include "tongji/predictor/target_snapshot_predictor.hpp" - -namespace world_exe::tongji::predictor { - -class TargetPredict::Impl { -public: - Impl(std::list targets) { - for (auto target : targets) { - targets_map_.insert({ target.GetId(), target }); - } - } - - std::shared_ptr Predict( - const enumeration::ArmorIdFlag& id, const std::time_t& time_stamp) { - std::unordered_map> - armor_map; - - const auto& dt = time_stamp - last_time_stamp_; - last_time_stamp_ = time_stamp; - - for (auto& target_value : targets_map_) { - target_value.second.Predict(dt); - auto x = target_value.second.GetEkf_x(); - - for (int j = 0; j < target_value.second.GetArmorNum(); ++j) { - auto angle = util::math::clamp_pm_pi( - x[6] + j * 2 * CV_PI / target_value.second.GetArmorNum()); - Eigen::Vector3d xyz = h_armor_xyz(x, j, target_value.second.GetArmorNum()); - data::ArmorGimbalControlSpacing armor; - armor.id = target_value.second.GetId(); - armor.position = xyz; - armor.orientation = util::math::euler_to_quaternion(angle, 15 / 180. * CV_PI, 0); - } - } - return std::make_shared(armor_map, last_time_stamp_); - } - - std::shared_ptr GetPredictor(const enumeration::ArmorIdFlag& id) const { - std::vector targets; - for (int i; i < 8; i++) { - const auto& car_id = static_cast(1 << i); - if (enumeration::IsFlagContains(id, car_id)) { - targets.emplace_back(ConstructTargets(car_id, )); // TODO - } - } - return std::make_shared(targets); - } - -private: - std::vector ConstructTargets( - const enumeration::ArmorIdFlag& id, std::time_t& time_stamp) const { - std::vector targets; - for (int i = 0; i < 8; ++i) { - auto car_id = static_cast(1 << i); - if (!enumeration::IsFlagContains(id, car_id)) continue; - - bool is_balance = (car_id == enumeration::CarIDFlag::InfantryIII - || car_id == enumeration::CarIDFlag::InfantryIV - || car_id == enumeration::CarIDFlag::InfantryV); - - Eigen::VectorXd P0_dig(11); - double radius; - int armor_num; - - if (is_balance) { - P0_dig << 1, 64, 1, 64, 1, 64, 0.4, 100, 1, 1, 1; - radius = 0.2; - armor_num = 2; - } else if (car_id == enumeration::CarIDFlag::Outpost) { - P0_dig << 1, 64, 1, 64, 1, 81, 0.4, 100, 1e-4, 0, 0; - radius = 0.2765; - armor_num = 3; - } else if (car_id == enumeration::CarIDFlag::Base) { - P0_dig << 1, 64, 1, 64, 1, 64, 0.4, 100, 1e-4, 0, 0; - radius = 0.3205; - armor_num = 3; - } else { - P0_dig << 1, 64, 1, 64, 1, 64, 0.4, 100, 1, 1, 1; - radius = 0.2; - armor_num = 4; - } - // 不知道这个armor_xyz_in_world、armor_ypr_in_world（可以理解为x）可以从哪里传入 - Eigen::Vector3d armor_xyz_in_world = Eigen::Vector3d::Zero(); - Eigen::Vector3d armor_ypr_in_world = Eigen::Vector3d::Zero(); - targets.emplace_back(Target(armor_xyz_in_world, armor_ypr_in_world, car_id, time_stamp, - radius, armor_num, P0_dig)); - } - return targets; - } - - // 计算出装甲板中心的坐标（考虑长短轴） - Eigen::Vector3d h_armor_xyz(const Eigen::VectorXd& x, int id, int armor_num) const { - auto angle = util::math::clamp_pm_pi(x[6] + id * 2 * CV_PI / armor_num); - auto use_l_h = (armor_num == 4) && (id == 1 || id == 3); - - auto r = (use_l_h) ? x[8] + x[9] : x[8]; - auto armor_x = x[0] - r * std::cos(angle); - auto armor_y = x[2] - r * std::sin(angle); - auto armor_z = (use_l_h) ? x[4] + x[10] : x[4]; - - return { armor_x, armor_y, armor_z }; - } - - std::time_t last_time_stamp_ { 0 }; - std::unordered_map targets_map_; -}; - -} \ No newline at end of file diff --git a/src/tongji/predictor/target_predict.hpp b/src/tongji/predictor/target_predict.hpp deleted file mode 100644 index 0acf243..0000000 --- a/src/tongji/predictor/target_predict.hpp +++ /dev/null @@ -1,15 +0,0 @@ -#pragma once - -#include - -#include "interfaces/target_predictor.hpp" - -namespace world_exe::tongji::predictor { -class TargetPredict final : public interfaces::ITargetPredictor { -public: - -private: - class Impl; - std::unique_ptr pimpl_; -}; -} \ No newline at end of file diff --git a/src/tongji/predictor/target_predictor.cpp b/src/tongji/predictor/target_predictor.cpp index 9697080..7c41bf0 100644 --- a/src/tongji/predictor/target_predictor.cpp +++ b/src/tongji/predictor/target_predictor.cpp @@ -1,76 +1,118 @@ #include "target_predictor.hpp" -#include +#include #include -#include +#include #include "enum/car_id.hpp" #include "enum/enum_tools.hpp" -#include "target.hpp" +#include "tongji/predictor/in_gimbal_control_armor.hpp" +#include "tongji/predictor/target.hpp" +#include "tongji/predictor/target_snapshot_predictor.hpp" namespace world_exe::tongji::predictor { -std::vector ConstructTargets(const enumeration::ArmorIdFlag& id, std::time_t time_stamp) { - for (int i = 0; i < 8; ++i) { - auto car_id = static_cast(1 << i); - if (!enumeration::IsFlagContains(id, car_id)) continue; - - bool is_balance = (car_id == enumeration::CarIDFlag::InfantryIII - || car_id == enumeration::CarIDFlag::InfantryIV - || car_id == enumeration::CarIDFlag::InfantryV); - - Eigen::VectorXd P0_dig(11); - double radius; - int armor_num; - - if (is_balance) { - P0_dig << 1, 64, 1, 64, 1, 64, 0.4, 100, 1, 1, 1; - radius = 0.2; - armor_num = 2; - } else if (car_id == enumeration::CarIDFlag::Outpost) { - P0_dig << 1, 64, 1, 64, 1, 81, 0.4, 100, 1e-4, 0, 0; - radius = 0.2765; - armor_num = 3; - } else if (car_id == enumeration::CarIDFlag::Base) { - P0_dig << 1, 64, 1, 64, 1, 64, 0.4, 100, 1e-4, 0, 0; - radius = 0.3205; - armor_num = 3; - } else { - P0_dig << 1, 64, 1, 64, 1, 64, 0.4, 100, 1, 1, 1; - radius = 0.2; - armor_num = 4; +class TargetPredict::Impl { +public: + Impl(std::list targets) { + for (auto target : targets) { + targets_map_.insert({ target.GetId(), target }); } - - Eigen::Vector3d armor_xyz_in_world = Eigen::Vector3d::Zero(); - Eigen::Vector3d armor_ypr_in_world = Eigen::Vector3d::Zero(); } -} - -class TargetPredictor::Impl { -public: - Impl() { } std::shared_ptr Predict( - const enumeration::ArmorIdFlag& id, const std::time_t& time_stamp) { } + const enumeration::ArmorIdFlag& id, const std::time_t& time_stamp) { + std::unordered_map> + armor_map; + + const auto& dt = time_stamp - last_time_stamp_; + last_time_stamp_ = time_stamp; + + for (auto& target_value : targets_map_) { + target_value.second.Predict(dt); + auto x = target_value.second.GetEkf_x(); + + for (int j = 0; j < target_value.second.GetArmorNum(); ++j) { + auto angle = util::math::clamp_pm_pi( + x[6] + j * 2 * CV_PI / target_value.second.GetArmorNum()); + Eigen::Vector3d xyz = h_armor_xyz(x, j, target_value.second.GetArmorNum()); + data::ArmorGimbalControlSpacing armor; + armor.id = target_value.second.GetId(); + armor.position = xyz; + armor.orientation = util::math::euler_to_quaternion(angle, 15 / 180. * CV_PI, 0); + } + } + return std::make_shared(armor_map, last_time_stamp_); + } std::shared_ptr GetPredictor(const enumeration::ArmorIdFlag& id) const { - + std::vector targets; + for (int i; i < 8; i++) { + const auto& car_id = static_cast(1 << i); + if (enumeration::IsFlagContains(id, car_id)) { + targets.emplace_back(ConstructTargets(car_id, )); // TODO + } + } + return std::make_shared(targets); } private: - std::time_t last_update_time_stamp_ { 0 }; -}; + std::vector ConstructTargets( + const enumeration::ArmorIdFlag& id, std::time_t& time_stamp) const { + std::vector targets; + for (int i = 0; i < 8; ++i) { + auto car_id = static_cast(1 << i); + if (!enumeration::IsFlagContains(id, car_id)) continue; + + bool is_balance = (car_id == enumeration::CarIDFlag::InfantryIII + || car_id == enumeration::CarIDFlag::InfantryIV + || car_id == enumeration::CarIDFlag::InfantryV); + + Eigen::VectorXd P0_dig(11); + double radius; + int armor_num; + + if (is_balance) { + P0_dig << 1, 64, 1, 64, 1, 64, 0.4, 100, 1, 1, 1; + radius = 0.2; + armor_num = 2; + } else if (car_id == enumeration::CarIDFlag::Outpost) { + P0_dig << 1, 64, 1, 64, 1, 81, 0.4, 100, 1e-4, 0, 0; + radius = 0.2765; + armor_num = 3; + } else if (car_id == enumeration::CarIDFlag::Base) { + P0_dig << 1, 64, 1, 64, 1, 64, 0.4, 100, 1e-4, 0, 0; + radius = 0.3205; + armor_num = 3; + } else { + P0_dig << 1, 64, 1, 64, 1, 64, 0.4, 100, 1, 1, 1; + radius = 0.2; + armor_num = 4; + } + //TODO 不知道这个armor_xyz_in_world、armor_ypr_in_world（可以理解为x）可以从哪里传入 + Eigen::Vector3d armor_xyz_in_world = Eigen::Vector3d::Zero(); + Eigen::Vector3d armor_ypr_in_world = Eigen::Vector3d::Zero(); + targets.emplace_back(Target(armor_xyz_in_world, armor_ypr_in_world, car_id, time_stamp, + radius, armor_num, P0_dig)); + } + return targets; + } + + // 计算出装甲板中心的坐标（考虑长短轴） + Eigen::Vector3d h_armor_xyz(const Eigen::VectorXd& x, int id, int armor_num) const { + auto angle = util::math::clamp_pm_pi(x[6] + id * 2 * CV_PI / armor_num); + auto use_l_h = (armor_num == 4) && (id == 1 || id == 3); -std ::shared_ptr TargetPredictor::Predict( - const enumeration ::ArmorIdFlag& id, const std ::time_t& time_stamp) { - return pimpl_->Predict(id, time_stamp); -} + auto r = (use_l_h) ? x[8] + x[9] : x[8]; + auto armor_x = x[0] - r * std::cos(angle); + auto armor_y = x[2] - r * std::sin(angle); + auto armor_z = (use_l_h) ? x[4] + x[10] : x[4]; -std ::shared_ptr TargetPredictor::GetPredictor( - const enumeration ::ArmorIdFlag& id) const { - return pimpl_->GetPredictor(id); -} + return { armor_x, armor_y, armor_z }; + } + + std::time_t last_time_stamp_ { 0 }; + std::unordered_map targets_map_; +}; -TargetPredictor::TargetPredictor(const enumeration::ArmorIdFlag& id, std::time_t t) - : pimpl_() { } } \ No newline at end of file diff --git a/src/tongji/predictor/target_predictor.hpp b/src/tongji/predictor/target_predictor.hpp index 8958794..0acf243 100644 --- a/src/tongji/predictor/target_predictor.hpp +++ b/src/tongji/predictor/target_predictor.hpp @@ -5,15 +5,9 @@ #include "interfaces/target_predictor.hpp" namespace world_exe::tongji::predictor { -class TargetPredictor final : public interfaces::ITargetPredictor { +class TargetPredict final : public interfaces::ITargetPredictor { public: - TargetPredictor(const enumeration::ArmorIdFlag& id, std::time_t t); - std ::shared_ptr Predict( - const enumeration ::ArmorIdFlag& id, const std ::time_t& time_stamp) override; - - std ::shared_ptr GetPredictor( - const enumeration ::ArmorIdFlag& id) const override; - + private: class Impl; std::unique_ptr pimpl_; From dc596de7bba6069d2ce46a0c81cf0e8e262f9a89 Mon Sep 17 00:00:00 2001 From: heyeuu <2829004293@qq.com> Date: Sat, 20 Sep 2025 15:56:55 +0800 Subject: [PATCH 25/53] refactor(prediction): refactor IPredictor and ITargetPredictor implementations --- .../predictor/in_gimbal_control_armor.hpp | 18 +-- src/tongji/predictor/predictor_record.cpp | 58 ++++++++ ...hot_predictor.hpp => predictor_record.hpp} | 9 +- src/tongji/predictor/target.hpp | 64 ++++++--- src/tongji/predictor/target_predictor.cpp | 131 ++++++------------ src/tongji/predictor/target_predictor.hpp | 14 +- .../predictor/target_snapshot_predictor.cpp | 88 ------------ 7 files changed, 176 insertions(+), 206 deletions(-) create mode 100644 src/tongji/predictor/predictor_record.cpp rename src/tongji/predictor/{target_snapshot_predictor.hpp => predictor_record.hpp} (58%) delete mode 100644 src/tongji/predictor/target_snapshot_predictor.cpp diff --git a/src/tongji/predictor/in_gimbal_control_armor.hpp b/src/tongji/predictor/in_gimbal_control_armor.hpp index a8f48c9..0d6cef2 100644 --- a/src/tongji/predictor/in_gimbal_control_armor.hpp +++ b/src/tongji/predictor/in_gimbal_control_armor.hpp @@ -1,5 +1,6 @@ #pragma once +#include #include #include @@ -12,18 +13,16 @@ namespace world_exe::tongji::predictor { class InGimbalControlArmor final : public interfaces::IArmorInGimbalControl { public: - InGimbalControlArmor( - std::unordered_map> - armors, - TimeStamp time_stamp) - : armors_(armors) + InGimbalControlArmor(const std::unordered_map>& all_armors, + const std::time_t& time_stamp) + : armors_map_(all_armors) , time_stamp_(time_stamp) { } const std ::vector& GetArmors( const enumeration ::ArmorIdFlag& armor_id) const override { - static const std::vector empty; - auto it = armors_.find(armor_id); - return it != armors_.end() ? it->second : empty; + auto it = armors_map_.find(armor_id); + return it != armors_map_.end() ? it->second : empty; } const interfaces::ITimeStamped& GetTimeStamped() const override { return time_stamp_; } @@ -31,6 +30,7 @@ class InGimbalControlArmor final : public interfaces::IArmorInGimbalControl { private: TimeStamp time_stamp_; std::unordered_map> - armors_; + armors_map_; + static const std::vector empty; }; } \ No newline at end of file diff --git a/src/tongji/predictor/predictor_record.cpp b/src/tongji/predictor/predictor_record.cpp new file mode 100644 index 0000000..b040c8c --- /dev/null +++ b/src/tongji/predictor/predictor_record.cpp @@ -0,0 +1,58 @@ +#include "predictor_record.hpp" + +#include +#include +#include + +#include "data/armor_gimbal_control_spacing.hpp" +#include "enum/car_id.hpp" +#include "enum/enum_tools.hpp" +#include "in_gimbal_control_armor.hpp" + +namespace world_exe::tongji::predictor { +class PredictorRecord::Impl { +public: + Impl(const enumeration::ArmorIdFlag& id, + const std::unordered_map> snapshots) + : id_(id) + , snapshots_(snapshots) { } + + const enumeration::ArmorIdFlag& GetId() const { return id_; } + + std::shared_ptr Predictor(const std::time_t& time_stamp) { + std::unordered_map> + result; + + for (int i = 0; i < 8; i++) { + auto decomposed_id = static_cast( + static_cast(enumeration::CarIDFlag::Hero) << i); + if (enumeration::IsFlagContains(id_, decomposed_id)) { + auto it = snapshots_.find(decomposed_id); + if (it == snapshots_.end()) continue; + + auto copy = std::make_shared(*(it->second)); + copy->Predict(time_stamp); + result[decomposed_id] = copy->GetArmorGimbalControlSpacings(); + } + } + + return std::make_shared(result, time_stamp); + } + +private: + std::unordered_map> snapshots_; + enumeration::ArmorIdFlag id_; +}; + +PredictorRecord::PredictorRecord(const enumeration::ArmorIdFlag& id, + const std::unordered_map>& snapshots) + : pimpl_(std::make_unique(id, snapshots)) { } +PredictorRecord::~PredictorRecord() = default; + +const enumeration ::ArmorIdFlag& PredictorRecord::GetId() const { return pimpl_->GetId(); } + +std ::shared_ptr PredictorRecord::Predictor( + const std ::time_t& time_stamp) const { + return pimpl_->Predictor(time_stamp); +} +} \ No newline at end of file diff --git a/src/tongji/predictor/target_snapshot_predictor.hpp b/src/tongji/predictor/predictor_record.hpp similarity index 58% rename from src/tongji/predictor/target_snapshot_predictor.hpp rename to src/tongji/predictor/predictor_record.hpp index cea2fad..409c37f 100644 --- a/src/tongji/predictor/target_snapshot_predictor.hpp +++ b/src/tongji/predictor/predictor_record.hpp @@ -1,15 +1,20 @@ #pragma once #include +#include +#include "enum/armor_id.hpp" #include "interfaces/predictor.hpp" #include "tongji/predictor/target.hpp" namespace world_exe::tongji::predictor { -class MultiTargetSnapshotPredictor final : public interfaces::IPredictor { +class PredictorRecord final : public interfaces::IPredictor { public: - MultiTargetSnapshotPredictor(const std::vector& targets); + PredictorRecord(const enumeration::ArmorIdFlag& id, + const std::unordered_map>& snapshots); + ~PredictorRecord(); + const enumeration ::ArmorIdFlag& GetId() const override; std ::shared_ptr Predictor( diff --git a/src/tongji/predictor/target.hpp b/src/tongji/predictor/target.hpp index 3f6ec3c..7861f1b 100644 --- a/src/tongji/predictor/target.hpp +++ b/src/tongji/predictor/target.hpp @@ -2,11 +2,13 @@ #include #include +#include +#include +#include "data/armor_gimbal_control_spacing.hpp" #include "enum/armor_id.hpp" #include "enum/car_id.hpp" #include "interfaces/predictor_update_package.hpp" -#include "tongji/predictor/time_stamp.hpp" #include "util/extended_kalman_filter.hpp" #include "util/math.hpp" @@ -32,7 +34,7 @@ class Target { Target(const Eigen::Vector3d& armor_xyz_in_world, const Eigen::Vector3d& armor_ypr_in_world, const enumeration::ArmorIdFlag& id, const std::time_t& t, const double& radius, const int& armor_num, const Eigen::VectorXd& P0_dig) - : time_stamp_(t) + : last_time_stamp_(t) , car_id_(id) , armor_num_(armor_num) { @@ -64,26 +66,16 @@ class Target { Eigen::VectorXd GetEkf_x() const { return ekf_.x; } - void Predict(double dt) { - // 防止夹角求和出现异常值 - auto f = [&](const Eigen::VectorXd& x) -> Eigen::VectorXd { - Eigen::VectorXd x_prior = this->F(dt) * x; - x_prior[6] = util::math::clamp_pm_pi(x_prior[6]); - return x_prior; - }; - - // 前哨站转速特判 - if (this->Convergened() && this->car_id_ == enumeration::CarIDFlag::Outpost - && std::abs(this->ekf_.x[7]) > 2) - this->ekf_.x[7] = this->ekf_.x[7] > 0 ? 2.51 : -2.51; - - ekf_.Predict(this->F(dt), this->Q(dt), f); - } - - std::time_t GetTimeStamp() const { return time_stamp_.GetTimeStamp(); } + std::time_t GetTimeStamp() const { return last_time_stamp_; } int GetArmorNum() const { return armor_num_; } enumeration::CarIDFlag GetId() const { return car_id_; } + void Predict(const std::time_t& time_stamp) { + const double dt = std::difftime(time_stamp, last_time_stamp_); + Predict(dt); + last_time_stamp_ = time_stamp; + } + void Update(std::shared_ptr data) { const auto& transform = data->GetTransform(); const auto& rotation_transform = Eigen::Quaterniond(transform.linear()); @@ -123,7 +115,38 @@ class Target { return true; } + std::vector GetArmorGimbalControlSpacings() { + std::vector armors; + for (int id = 0; id < armor_num_; id++) { + auto angle = util::math::clamp_pm_pi(this->ekf_.x[6] + id * 2 * CV_PI / armor_num_); + auto xyz = h_armor_xyz(this->ekf_.x, id); + + data::ArmorGimbalControlSpacing armor; + armor.id = car_id_; + armor.position = xyz; + armor.orientation = util::math::euler_to_quaternion(angle, 15. / 180. * CV_PI, 0); + armors.emplace_back(std::move(armor)); + } + return armors; + } + private: + void Predict(double dt) { + // 防止夹角求和出现异常值 + auto f = [&](const Eigen::VectorXd& x) -> Eigen::VectorXd { + Eigen::VectorXd x_prior = this->F(dt) * x; + x_prior[6] = util::math::clamp_pm_pi(x_prior[6]); + return x_prior; + }; + + // 前哨站转速特判 + if (this->Convergened() && this->car_id_ == enumeration::CarIDFlag::Outpost + && std::abs(this->ekf_.x[7]) > 2) + this->ekf_.x[7] = this->ekf_.x[7] > 0 ? 2.51 : -2.51; + + ekf_.Predict(this->F(dt), this->Q(dt), f); + } + void Update(const Eigen::Vector3d& armor_xyz_in_world, const Eigen::Vector3d& armor_ypr_in_world, const Eigen::Vector3d& armor_ypd_in_world) { // 装甲板匹配 @@ -321,10 +344,9 @@ auto Q(double dt)->Eigen::MatrixXd{ // clang-format on } - TimeStamp time_stamp_; + std::time_t last_time_stamp_; util::ExtendedKalmanFilter ekf_; enumeration::CarIDFlag car_id_; int armor_num_; }; - } \ No newline at end of file diff --git a/src/tongji/predictor/target_predictor.cpp b/src/tongji/predictor/target_predictor.cpp index 7c41bf0..436b823 100644 --- a/src/tongji/predictor/target_predictor.cpp +++ b/src/tongji/predictor/target_predictor.cpp @@ -1,118 +1,79 @@ #include "target_predictor.hpp" -#include #include #include +#include -#include "enum/car_id.hpp" #include "enum/enum_tools.hpp" #include "tongji/predictor/in_gimbal_control_armor.hpp" +#include "tongji/predictor/predictor_record.hpp" #include "tongji/predictor/target.hpp" -#include "tongji/predictor/target_snapshot_predictor.hpp" namespace world_exe::tongji::predictor { class TargetPredict::Impl { public: - Impl(std::list targets) { - for (auto target : targets) { - targets_map_.insert({ target.GetId(), target }); - } + Impl(std::unordered_map> targets) { } + void RegisterTarget(enumeration::ArmorIdFlag id, const Target& target) { + targets_[id] = std::make_shared(target); } + void RemoveTarget(enumeration::ArmorIdFlag id) { } + std::shared_ptr Predict( const enumeration::ArmorIdFlag& id, const std::time_t& time_stamp) { std::unordered_map> - armor_map; - - const auto& dt = time_stamp - last_time_stamp_; - last_time_stamp_ = time_stamp; - - for (auto& target_value : targets_map_) { - target_value.second.Predict(dt); - auto x = target_value.second.GetEkf_x(); - - for (int j = 0; j < target_value.second.GetArmorNum(); ++j) { - auto angle = util::math::clamp_pm_pi( - x[6] + j * 2 * CV_PI / target_value.second.GetArmorNum()); - Eigen::Vector3d xyz = h_armor_xyz(x, j, target_value.second.GetArmorNum()); - data::ArmorGimbalControlSpacing armor; - armor.id = target_value.second.GetId(); - armor.position = xyz; - armor.orientation = util::math::euler_to_quaternion(angle, 15 / 180. * CV_PI, 0); + result; + + for (int i = 0; i < 8; i++) { + auto decomposed_id = static_cast( + static_cast(enumeration::CarIDFlag::Hero) << i); + if (enumeration::IsFlagContains(id, decomposed_id)) { + auto it = targets_.find(decomposed_id); + if (it == targets_.end()) continue; + + it->second->Predict(time_stamp); + result[decomposed_id] = it->second->GetArmorGimbalControlSpacings(); } } - return std::make_shared(armor_map, last_time_stamp_); + + return std::make_shared(result, time_stamp); } std::shared_ptr GetPredictor(const enumeration::ArmorIdFlag& id) const { - std::vector targets; - for (int i; i < 8; i++) { - const auto& car_id = static_cast(1 << i); - if (enumeration::IsFlagContains(id, car_id)) { - targets.emplace_back(ConstructTargets(car_id, )); // TODO - } - } - return std::make_shared(targets); - } + std::unordered_map> snapshot_map; -private: - std::vector ConstructTargets( - const enumeration::ArmorIdFlag& id, std::time_t& time_stamp) const { - std::vector targets; - for (int i = 0; i < 8; ++i) { - auto car_id = static_cast(1 << i); - if (!enumeration::IsFlagContains(id, car_id)) continue; - - bool is_balance = (car_id == enumeration::CarIDFlag::InfantryIII - || car_id == enumeration::CarIDFlag::InfantryIV - || car_id == enumeration::CarIDFlag::InfantryV); - - Eigen::VectorXd P0_dig(11); - double radius; - int armor_num; - - if (is_balance) { - P0_dig << 1, 64, 1, 64, 1, 64, 0.4, 100, 1, 1, 1; - radius = 0.2; - armor_num = 2; - } else if (car_id == enumeration::CarIDFlag::Outpost) { - P0_dig << 1, 64, 1, 64, 1, 81, 0.4, 100, 1e-4, 0, 0; - radius = 0.2765; - armor_num = 3; - } else if (car_id == enumeration::CarIDFlag::Base) { - P0_dig << 1, 64, 1, 64, 1, 64, 0.4, 100, 1e-4, 0, 0; - radius = 0.3205; - armor_num = 3; - } else { - P0_dig << 1, 64, 1, 64, 1, 64, 0.4, 100, 1, 1, 1; - radius = 0.2; - armor_num = 4; + for (int i = 0; i < 8; i++) { + auto decomposed_id = static_cast( + static_cast(enumeration::CarIDFlag::Hero) << i); + if (enumeration::IsFlagContains(id, decomposed_id)) { + + auto it = targets_.find(decomposed_id); + if (it == targets_.end()) continue; + + snapshot_map[decomposed_id] = std::make_shared(*(it->second)); } - //TODO 不知道这个armor_xyz_in_world、armor_ypr_in_world（可以理解为x）可以从哪里传入 - Eigen::Vector3d armor_xyz_in_world = Eigen::Vector3d::Zero(); - Eigen::Vector3d armor_ypr_in_world = Eigen::Vector3d::Zero(); - targets.emplace_back(Target(armor_xyz_in_world, armor_ypr_in_world, car_id, time_stamp, - radius, armor_num, P0_dig)); } - return targets; + return std::make_shared(id, snapshot_map); } - // 计算出装甲板中心的坐标（考虑长短轴） - Eigen::Vector3d h_armor_xyz(const Eigen::VectorXd& x, int id, int armor_num) const { - auto angle = util::math::clamp_pm_pi(x[6] + id * 2 * CV_PI / armor_num); - auto use_l_h = (armor_num == 4) && (id == 1 || id == 3); +private: + std::unordered_map> targets_; +}; - auto r = (use_l_h) ? x[8] + x[9] : x[8]; - auto armor_x = x[0] - r * std::cos(angle); - auto armor_y = x[2] - r * std::sin(angle); - auto armor_z = (use_l_h) ? x[4] + x[10] : x[4]; +TargetPredict::TargetPredict( + std::unordered_map> targets) + : pimpl_(std::make_unique(std::move(targets))) { } +TargetPredict::~TargetPredict() = default; - return { armor_x, armor_y, armor_z }; - } +std ::shared_ptr TargetPredict::Predict( + const enumeration ::ArmorIdFlag& id, const std ::time_t& time_stamp) { + return pimpl_->Predict(id, time_stamp); +} - std::time_t last_time_stamp_ { 0 }; - std::unordered_map targets_map_; -}; +std ::shared_ptr TargetPredict::GetPredictor( + const enumeration ::ArmorIdFlag& id) const { + return pimpl_->GetPredictor(id); +} } \ No newline at end of file diff --git a/src/tongji/predictor/target_predictor.hpp b/src/tongji/predictor/target_predictor.hpp index 0acf243..d13ced6 100644 --- a/src/tongji/predictor/target_predictor.hpp +++ b/src/tongji/predictor/target_predictor.hpp @@ -2,14 +2,26 @@ #include +#include "enum/armor_id.hpp" #include "interfaces/target_predictor.hpp" +#include "target.hpp" namespace world_exe::tongji::predictor { + class TargetPredict final : public interfaces::ITargetPredictor { public: - + TargetPredict(std::unordered_map> targets); + ~TargetPredict(); + + std ::shared_ptr Predict( + const enumeration ::ArmorIdFlag& id, const std ::time_t& time_stamp) override; + + std ::shared_ptr GetPredictor( + const enumeration ::ArmorIdFlag& id) const override; + private: class Impl; std::unique_ptr pimpl_; }; + } \ No newline at end of file diff --git a/src/tongji/predictor/target_snapshot_predictor.cpp b/src/tongji/predictor/target_snapshot_predictor.cpp deleted file mode 100644 index 4bc42f9..0000000 --- a/src/tongji/predictor/target_snapshot_predictor.cpp +++ /dev/null @@ -1,88 +0,0 @@ -#include "target_snapshot_predictor.hpp" - -#include -#include -#include - -#include -#include - -#include "data/armor_gimbal_control_spacing.hpp" -#include "enum/armor_id.hpp" -#include "enum/enum_tools.hpp" -#include "in_gimbal_control_armor.hpp" -#include "target.hpp" -#include "util/math.hpp" - -namespace world_exe::tongji::predictor { -class MultiTargetSnapshotPredictor::Impl { -public: - Impl(const std::vector& targets) - : targets_(targets) - , last_time_stamp_(0) { - id_ = enumeration::ArmorIdFlag::Unknow; - for (const auto& target : targets_) { - id_ = static_cast( - static_cast(id_) | static_cast(target.GetId())); - } - } - - const enumeration::ArmorIdFlag& GetId() const { return id_; } - - std::shared_ptr Predictor(const std::time_t& time_stamp) { - std::unordered_map> - armor_map; - const double dt = static_cast(time_stamp - last_time_stamp_); - - for (auto& target : targets_) { - Target iteration_target = target; - iteration_target.Predict(dt); - - Eigen::VectorXd iteration_x = iteration_target.GetEkf_x(); - std::vector armors; - - for (int j = 0; j < iteration_target.GetArmorNum(); ++j) { - auto angle = util::math::clamp_pm_pi( - iteration_x[6] + j * 2 * CV_PI / iteration_target.GetArmorNum()); - Eigen::Vector3d xyz = h_armor_xyz(iteration_x, j, iteration_target.GetArmorNum()); - data::ArmorGimbalControlSpacing armor; - armor.id = iteration_target.GetId(); - armor.position = xyz; - armor.orientation = util::math::euler_to_quaternion(angle, 15 / 180. * CV_PI, 0); - } - armor_map[iteration_target.GetId()] = armors; - } - return std::make_shared(armor_map, last_time_stamp_); - } - -private: - std::vector targets_; - enumeration::ArmorIdFlag id_; - std::time_t last_time_stamp_; - - // 计算出装甲板中心的坐标（考虑长短轴） - Eigen::Vector3d h_armor_xyz(const Eigen::VectorXd& x, int id, int armor_num) const { - auto angle = util::math::clamp_pm_pi(x[6] + id * 2 * CV_PI / armor_num); - auto use_l_h = (armor_num == 4) && (id == 1 || id == 3); - - auto r = (use_l_h) ? x[8] + x[9] : x[8]; - auto armor_x = x[0] - r * std::cos(angle); - auto armor_y = x[2] - r * std::sin(angle); - auto armor_z = (use_l_h) ? x[4] + x[10] : x[4]; - - return { armor_x, armor_y, armor_z }; - } -}; - -MultiTargetSnapshotPredictor::MultiTargetSnapshotPredictor(const std::vector& targets) - : pimpl_(std::make_unique(targets)) { } - -const enumeration ::ArmorIdFlag& MultiTargetSnapshotPredictor::GetId() const { - return pimpl_->GetId(); -} - -std ::shared_ptr MultiTargetSnapshotPredictor::Predictor( - const std ::time_t& time_stamp) const { - return pimpl_->Predictor(time_stamp); -} -} \ No newline at end of file From 0dcffbf46dd6f0051d4efc5bbf7eb01a8324bb08 Mon Sep 17 00:00:00 2001 From: heyeuu <2829004293@qq.com> Date: Mon, 22 Sep 2025 10:33:57 +0800 Subject: [PATCH 26/53] refactor(predictor): overhaul predictor implementation --- .../predictor/in_gimbal_control_armor.hpp | 2 +- src/tongji/predictor/live_target.hpp | 149 ++++++++ src/tongji/predictor/live_target_manager.cpp | 104 ++++++ ..._predictor.hpp => live_target_manager.hpp} | 8 +- src/tongji/predictor/predict_model.hpp | 258 +++++++++++++ src/tongji/predictor/predictor_record.cpp | 58 --- src/tongji/predictor/target.hpp | 352 ------------------ src/tongji/predictor/target_predictor.cpp | 79 ---- src/tongji/predictor/target_snapshot.hpp | 36 ++ .../predictor/target_snapshot_manager.cpp | 51 +++ ...record.hpp => target_snapshot_manager.hpp} | 10 +- src/util/extended_kalman_filter.hpp | 46 +-- 12 files changed, 624 insertions(+), 529 deletions(-) create mode 100644 src/tongji/predictor/live_target.hpp create mode 100644 src/tongji/predictor/live_target_manager.cpp rename src/tongji/predictor/{target_predictor.hpp => live_target_manager.hpp} (70%) create mode 100644 src/tongji/predictor/predict_model.hpp delete mode 100644 src/tongji/predictor/predictor_record.cpp delete mode 100644 src/tongji/predictor/target.hpp delete mode 100644 src/tongji/predictor/target_predictor.cpp create mode 100644 src/tongji/predictor/target_snapshot.hpp create mode 100644 src/tongji/predictor/target_snapshot_manager.cpp rename src/tongji/predictor/{predictor_record.hpp => target_snapshot_manager.hpp} (67%) diff --git a/src/tongji/predictor/in_gimbal_control_armor.hpp b/src/tongji/predictor/in_gimbal_control_armor.hpp index 0d6cef2..117203a 100644 --- a/src/tongji/predictor/in_gimbal_control_armor.hpp +++ b/src/tongji/predictor/in_gimbal_control_armor.hpp @@ -16,7 +16,7 @@ class InGimbalControlArmor final : public interfaces::IArmorInGimbalControl { InGimbalControlArmor(const std::unordered_map>& all_armors, const std::time_t& time_stamp) - : armors_map_(all_armors) + : armors_map_(std::move(all_armors)) , time_stamp_(time_stamp) { } const std ::vector& GetArmors( diff --git a/src/tongji/predictor/live_target.hpp b/src/tongji/predictor/live_target.hpp new file mode 100644 index 0000000..102c2d6 --- /dev/null +++ b/src/tongji/predictor/live_target.hpp @@ -0,0 +1,149 @@ +#pragma once + +#include +#include +#include + +#include "data/armor_gimbal_control_spacing.hpp" +#include "enum/car_id.hpp" +#include "predict_model.hpp" +#include "util/extended_kalman_filter.hpp" +#include "util/math.hpp" + +namespace world_exe::tongji::predictor { + +struct TargetStatus { + bool jumped = false; + bool switched = false; + bool converged = false; + bool diverged = false; + bool lost = false; + bool reidentified = false; + int last_id = -1; + int switch_count = 0; + int update_count = 0; + int lost_count = 0; +}; + +class LiveTarget { +public: + TargetStatus status_; + + LiveTarget(const Eigen::Vector3d& armor_xyz_in_world, const Eigen::Vector3d& armor_ypr_in_world, + const enumeration::CarIDFlag& car_id, const std::time_t& t) + : last_time_stamp_(t) + , model_(car_id) { + + // x vx y vy z vz a w r l h + // a: angle + // w: angular velocity + // l: r2 - r1 + // h: z2 - z1 + auto center_x = + armor_xyz_in_world[0] + model_.GetRadius() * std::cos(armor_ypr_in_world[0]); + auto center_y = + armor_xyz_in_world[1] + model_.GetRadius() * std::sin(armor_ypr_in_world[0]); + auto center_z = armor_xyz_in_world[2]; + + Eigen::VectorXd x0(11); + x0 << center_x, 0, center_y, 0, center_z, 0, armor_ypr_in_world[0], 0, model_.GetRadius(), + 0, 0; + + Eigen::MatrixXd P0 = model_.GetP0Dig().asDiagonal(); + ekf_ = util::ExtendedKalmanFilter( + x0, P0, model_.x_add); // 初始化滤波器（预测量、预测量协方差） + } + + Eigen::VectorXd GetEkfX() const { return ekf_.x; } + Eigen::VectorXd GetP0Dig() const { return model_.GetP0Dig(); } + const PredictModel& GetModel() const { return model_; } + std::time_t GetTimeStamp() const { return last_time_stamp_; } + + std::vector GetArmorGimbalControlSpacings() { + std::vector armors; + for (int id = 0; id < model_.GetArmorNum(); id++) { + auto angle = + util::math::clamp_pm_pi(this->ekf_.x[6] + id * 2 * CV_PI / model_.GetArmorNum()); + auto xyz = model_.h_armor_xyz(this->ekf_.x, id); + + data::ArmorGimbalControlSpacing armor; + armor.id = model_.GetID(); + armor.position = xyz; + armor.orientation = util::math::euler_to_quaternion(angle, 15. / 180. * CV_PI, 0); + armors.emplace_back(std::move(armor)); + } + return armors; + } + + void Update(const double& dt, const Eigen::Vector3d& armor_xyz_in_world, + const Eigen::Vector3d& armor_ypr_in_world, const Eigen::Vector3d& armor_ypd_in_world) { + + // 装甲板匹配 + int id = + model_.MatchArmor(ekf_.x, armor_xyz_in_world, armor_ypr_in_world, armor_ypd_in_world); + if (id != 0) status_.jumped = true; + status_.switched = (id != status_.last_id); + if (status_.switched) status_.switch_count++; + + status_.last_id = id; + Update_ypda(armor_xyz_in_world, armor_ypr_in_world, armor_ypd_in_world, id, dt); + status_.update_count++; + } + +private: + bool Converged() { + if (model_.GetID() != enumeration::CarIDFlag::Outpost && status_.update_count > 3 + && !this->Diverged()) { + status_.converged = true; + } + + // 前哨站特殊判断 + if (model_.GetID() == enumeration::CarIDFlag::Outpost && status_.update_count > 10 + && !this->Diverged()) { + status_.converged = true; + } + return status_.converged; + } + + bool Diverged() const { + auto r_ok = ekf_.x[8] > 0.05 && ekf_.x[8] < 0.5; + auto l_ok = ekf_.x[8] + ekf_.x[9] > 0.05 && ekf_.x[8] + ekf_.x[9] < 0.5; + + if (r_ok && l_ok) return false; + // util::logger::logger()->debug("[Target] r={:.3f}, l={:.3f}", ekf_.x[8], ekf_.x[9]); + return true; + } + + void Update_ypda(const Eigen::Vector3d& armor_xyz_in_world, + const Eigen::Vector3d& armor_ypr_in_world, const Eigen::Vector3d& armor_ypd_in_world, + const int& id, const double& dt) { + // 观测jacobi + auto H = model_.H(ekf_.x, id); + auto R = model_.R(armor_xyz_in_world, armor_ypr_in_world, armor_ypd_in_world, id); + auto A = model_.A(dt); + auto Q = model_.Q(dt); + auto f = model_.f; + auto h = [this, id](const Eigen::VectorXd& x) { return model_.h(x, id); }; + auto z_subtract = model_.z_subtract; + + const Eigen::VectorXd& ypd = armor_ypd_in_world; + const Eigen::VectorXd& ypr = armor_ypr_in_world; + + // 获得观测量 + Eigen::VectorXd z(4); + z << ypd[0], ypd[1], ypd[2], ypr[0]; + + ekf_.Update(dt, A, Q, f, z, H, R, h, z_subtract); + + // 前哨站转速特判 + if (this->Converged() && model_.GetID() == enumeration::CarIDFlag::Outpost + && std::abs(this->ekf_.x[7]) > 2) + this->ekf_.x[7] = this->ekf_.x[7] > 0 ? 2.51 : -2.51; + } + + std::time_t last_time_stamp_; + util::ExtendedKalmanFilter ekf_; + PredictModel model_; +}; + +} \ No newline at end of file diff --git a/src/tongji/predictor/live_target_manager.cpp b/src/tongji/predictor/live_target_manager.cpp new file mode 100644 index 0000000..fda3cb3 --- /dev/null +++ b/src/tongji/predictor/live_target_manager.cpp @@ -0,0 +1,104 @@ +#include "live_target_manager.hpp" + +#include +#include +#include + +#include "enum/armor_id.hpp" +#include "enum/enum_tools.hpp" +#include "interfaces/predictor_update_package.hpp" +#include "tongji/predictor/in_gimbal_control_armor.hpp" +#include "tongji/predictor/live_target.hpp" +#include "tongji/predictor/target_snapshot_manager.hpp" + +namespace world_exe::tongji::predictor { + +class LiveTargetManager::Impl { +public: + Impl() = default; + + void RegisterTarget(enumeration::ArmorIdFlag id, std::shared_ptr target) { + targets_[id] = std::move(target); + } + + void RemoveTarget(enumeration::ArmorIdFlag id) { targets_.erase(id); } + + std::shared_ptr Predict( + const enumeration::ArmorIdFlag& flag, const std::time_t& time_stamp) { + std::unordered_map> + result; + + for (auto id : ExpandArmorIdFlags(flag)) { + auto it = targets_.find(id); + if (it != targets_.end() && it->second) { + result[id] = it->second->GetArmorGimbalControlSpacings(); + } + } + + return std::make_shared(result, time_stamp); + } + + std::shared_ptr GetPredictor( + const enumeration::ArmorIdFlag& flag) const { + std::unordered_map> snapshot_map; + + for (auto id : ExpandArmorIdFlags(flag)) { + auto it = targets_.find(id); + if (it != targets_.end() && it->second) { + snapshot_map[id] = it->second; + } + } + + return std::make_shared(flag, snapshot_map); + } + + void Update(std::shared_ptr data, const double& dt) { + const auto& transform = data->GetTransform(); + const auto& rotation_transform = Eigen::Quaterniond(transform.linear()); + + for (const auto& [id, target] : targets_) { + const auto armors = data->GetArmors()->GetArmors(id); + + if (armors.empty()) continue; + + for (auto armor : armors) { + const auto& armor_in_world_position = transform * armor.position; + const auto& armor_in_world_oritaiton = rotation_transform * armor.orientation; + target->Update(dt, armor_in_world_position, + util::math::quaternion_to_euler(armor_in_world_oritaiton, 2, 1, 0), + util::math::xyz2ypd(armor_in_world_position)); + } + } + } + +private: + inline std::vector ExpandArmorIdFlags( + enumeration::ArmorIdFlag flags) const { + std::vector result; + for (int i = 0; i < static_cast(enumeration::ArmorIdFlag::Count); ++i) { + auto single = static_cast( + static_cast(enumeration::ArmorIdFlag::Hero) << i); + if ((static_cast(flags) & static_cast(single)) != 0) { + result.push_back(single); + } + } + return result; + } + + std::unordered_map> targets_; +}; + +LiveTargetManager::LiveTargetManager() = default; +LiveTargetManager::~LiveTargetManager() = default; + +std ::shared_ptr LiveTargetManager::Predict( + const enumeration ::ArmorIdFlag& id, const std ::time_t& time_stamp) { + return pimpl_->Predict(id, time_stamp); +} + +std ::shared_ptr LiveTargetManager::GetPredictor( + const enumeration ::ArmorIdFlag& id) const { + return pimpl_->GetPredictor(id); +} + +} \ No newline at end of file diff --git a/src/tongji/predictor/target_predictor.hpp b/src/tongji/predictor/live_target_manager.hpp similarity index 70% rename from src/tongji/predictor/target_predictor.hpp rename to src/tongji/predictor/live_target_manager.hpp index d13ced6..f78580f 100644 --- a/src/tongji/predictor/target_predictor.hpp +++ b/src/tongji/predictor/live_target_manager.hpp @@ -4,14 +4,14 @@ #include "enum/armor_id.hpp" #include "interfaces/target_predictor.hpp" -#include "target.hpp" + namespace world_exe::tongji::predictor { -class TargetPredict final : public interfaces::ITargetPredictor { +class LiveTargetManager final : public interfaces::ITargetPredictor { public: - TargetPredict(std::unordered_map> targets); - ~TargetPredict(); + LiveTargetManager(); + ~LiveTargetManager(); std ::shared_ptr Predict( const enumeration ::ArmorIdFlag& id, const std ::time_t& time_stamp) override; diff --git a/src/tongji/predictor/predict_model.hpp b/src/tongji/predictor/predict_model.hpp new file mode 100644 index 0000000..8fe4b43 --- /dev/null +++ b/src/tongji/predictor/predict_model.hpp @@ -0,0 +1,258 @@ +#pragma once + +#include + +#include +#include + +#include "enum/car_id.hpp" +#include "util/math.hpp" + +namespace world_exe::tongji::predictor { + +class PredictModel { +public: + PredictModel(const enumeration::CarIDFlag& car_id) + : car_id_(car_id) { + + bool is_balance = (car_id == enumeration::CarIDFlag::InfantryIII + || car_id == enumeration::CarIDFlag::InfantryIV + || car_id == enumeration::CarIDFlag::InfantryV); + + P0_dig_.resize(11); + if (is_balance) { + P0_dig_ << 1, 64, 1, 64, 1, 64, 0.4, 100, 1, 1, 1; + } else if (car_id == enumeration::CarIDFlag::Outpost) { + P0_dig_ << 1, 64, 1, 64, 1, 81, 0.4, 100, 1e-4, 0, 0; + } else if (car_id == enumeration::CarIDFlag::Base) { + P0_dig_ << 1, 64, 1, 64, 1, 64, 0.4, 100, 1e-4, 0, 0; + } else { + P0_dig_ << 1, 64, 1, 64, 1, 64, 0.4, 100, 1, 1, 1; + } + + if (car_id == enumeration::CarIDFlag::Outpost) { + radius_ = 0.2765; + } else if (car_id == enumeration::CarIDFlag::Base) { + radius_ = 0.3205; + } else { + radius_ = 0.2; + } + + if (is_balance) { + armor_num_ = 2; + } else if (car_id == enumeration::CarIDFlag::Outpost + | car_id == enumeration::CarIDFlag::Base) { + armor_num_ = 3; + } else { + armor_num_ = 4; + } + } + + auto GetP0Dig() const { return P0_dig_; } + auto GetRadius() const { return radius_; } + auto GetID() const { return car_id_; } + int GetArmorNum() const { return armor_num_; } + + // 防止夹角求和出现异常值 + std::function x_add = + [](const Eigen::VectorXd& a, const Eigen::VectorXd& b) { + Eigen::VectorXd c = a + b; + c(6) = util::math::clamp_pm_pi(c(6)); + return c; + }; + + // 防止夹角求和出现异常值 + std::function f = + [=, this](const Eigen::VectorXd& x, const double& dt) -> Eigen::VectorXd { + Eigen::VectorXd x_prior = A(dt) * x; + x_prior(6) = util::math::clamp_pm_pi(x_prior(6)); + return x_prior; + }; + + int MatchArmor(const Eigen::VectorXd& x, const Eigen::Vector3d& armor_xyz_in_world, + const Eigen::Vector3d& armor_ypr_in_world, + const Eigen::Vector3d& armor_ypd_in_world) const { + + const auto& xyza_list = GetArmorXYZAList(x); + std::vector> xyza_i_list; + + for (int i = 0; i < armor_num_; ++i) { + xyza_i_list.emplace_back(xyza_list[i], i); + } + + std::sort(xyza_i_list.begin(), xyza_i_list.end(), [](const auto& a, const auto& b) { + auto ypd1 = util::math::xyz2ypd(a.first.head(3)); + auto ypd2 = util::math::xyz2ypd(b.first.head(3)); + return ypd1(2) < ypd2(2); + }); + + int best_id = 0; + double min_error = 1e10; + + for (int i = 0; i < std::min(3, armor_num_); ++i) { + const auto& xyza = xyza_i_list[i].first; + auto ypd = util::math::xyz2ypd(xyza.head(3)); + double error = std::abs(util::math::clamp_pm_pi(armor_ypr_in_world(0) - xyza(3))) + + std::abs(util::math::clamp_pm_pi(armor_ypd_in_world(0) - ypd(0))); + + if (error < min_error) { + min_error = error; + best_id = xyza_i_list[i].second; + } + } + return best_id; + } + + // 计算出装甲板中心的坐标（考虑长短轴） + Eigen::Vector3d h_armor_xyz(const Eigen::VectorXd& x, int id) const { + auto angle = util::math::clamp_pm_pi(x(6) + id * 2 * CV_PI / armor_num_); + auto use_l_h = (armor_num_ == 4) && (id == 1 || id == 3); + + auto r = (use_l_h) ? x(8) + x(9) : x(8); + auto armor_x = x(0) - r * std::cos(angle); + auto armor_y = x(2) - r * std::sin(angle); + auto armor_z = (use_l_h) ? x(4) + x(10) : x(4); + + return { armor_x, armor_y, armor_z }; + } + + Eigen::MatrixXd H(const Eigen::VectorXd& x, int id) const { + auto angle = util::math::clamp_pm_pi(x(6) + id * 2 * CV_PI / armor_num_); + auto use_l_h = (armor_num_ == 4) && (id == 1 || id == 3); + + auto r = (use_l_h) ? x(8) + x(9) : x(8); + auto dx_da = r * std::sin(angle); + auto dy_da = -r * std::cos(angle); + + auto dx_dr = -std::cos(angle); + auto dy_dr = -std::sin(angle); + auto dx_dl = (use_l_h) ? -std::cos(angle) : 0.0; + auto dy_dl = (use_l_h) ? -std::sin(angle) : 0.0; + + auto dz_dh = (use_l_h) ? 1.0 : 0.0; + + // clang-format off + Eigen::MatrixXd H_armor_xyza{ + {1, 0, 0, 0, 0, 0, dx_da, 0, dx_dr, dx_dl, 0}, + {0, 0, 1, 0, 0, 0, dy_da, 0, dy_dr, dy_dl, 0}, + {0, 0, 0, 0, 1, 0, 0, 0, 0, 0, dz_dh}, + {0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0} + }; + // clang-format on + + Eigen::VectorXd armor_xyz = h_armor_xyz(x, id); + Eigen::MatrixXd H_armor_ypd = util::math::xyz2ypd_jacobian(armor_xyz); + // clang-format off + Eigen::MatrixXd H_armor_ypda{ + {H_armor_ypd(0, 0), H_armor_ypd(0, 1), H_armor_ypd(0, 2), 0}, + {H_armor_ypd(1, 0), H_armor_ypd(1, 1), H_armor_ypd(1, 2), 0}, + {H_armor_ypd(2, 0), H_armor_ypd(2, 1), H_armor_ypd(2, 2), 0}, + { 0, 0, 0, 1} + }; + // clang-format on + + return H_armor_ypda * H_armor_xyza; + } + + Eigen::MatrixXd R(const Eigen::Vector3d& armor_xyz_in_world, + const Eigen::Vector3d& armor_ypr_in_world, const Eigen::Vector3d& armor_ypd_in_world, + int id) { + // Eigen::VectorXd R_dig{{4e-3, 4e-3, 1, 9e-2}}; + auto center_yaw = std::atan2(armor_xyz_in_world(1), armor_xyz_in_world(0)); + auto delta_angle = util::math::clamp_pm_pi(armor_ypr_in_world(0) - center_yaw); + Eigen::VectorXd R_dig(4); + R_dig << 4e-3, 4e-3, log(std::abs(delta_angle) + 1) + 1, + log(std::abs(armor_ypd_in_world(2)) + 1) / 200 + 9e-2; + + // 测量过程噪声偏差的方差 + return R_dig.asDiagonal(); + } + + Eigen::MatrixXd A(double dt) const { + // 状态转移矩阵 + // clang-format off + return (Eigen::MatrixXd(11, 11) << + 1, dt, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 1, dt, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 1, dt, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 1, dt, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1).finished(); + } + //clang-format on + + Eigen::MatrixXd Q(double dt) const { + // Piecewise White Noise Model + // https://github.com/rlabbe/Kalman-and-Bayesian-Filters-in-Python/blob/master/07-Kalman-Filter-Math.ipynb + double v1, v2; + if (car_id_ == enumeration::CarIDFlag::Outpost) { + v1 = 10; // 前哨站加速度方差 + v2 = 0.1; // 前哨站角加速度方差 + } else { + v1 = 100; // 加速度方差 + v2 = 400; // 角加速度方差 + } + auto a = dt * dt * dt * dt / 4; + auto b = dt * dt * dt / 2; + auto c = dt * dt; + + // 预测过程噪声偏差的方差 + // clang-format off + return (Eigen::MatrixXd(11, 11) << + a * v1, b * v1, 0, 0, 0, 0, 0, 0, 0, 0, 0, + b * v1, c * v1, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, a * v1, b * v1, 0, 0, 0, 0, 0, 0, 0, + 0, 0, b * v1, c * v1, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, a * v1, b * v1, 0, 0, 0, 0, 0, + 0, 0, 0, 0, b * v1, c * v1, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, a * v2, b * v2, 0, 0, 0, + 0, 0, 0, 0, 0, 0, b * v2, c * v2, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0).finished(); + // clang-format on + } + + std::function h = [this](const Eigen::VectorXd& x, + int id) { + Eigen::Vector3d xyz = this->h_armor_xyz(x, id); + Eigen::Vector3d ypd = util::math::xyz2ypd(xyz); + double angle = util::math::clamp_pm_pi(x(6) + id * 2 * CV_PI / this->armor_num_); + return Eigen::Vector4d { ypd(0), ypd(1), ypd(2), angle }; + }; + + // 防止夹角求差出现异常值 + std::function z_subtract = + [](const Eigen::VectorXd& a, const Eigen::VectorXd& b) { + Eigen::VectorXd c = a - b; + c(0) = util::math::clamp_pm_pi(c(0)); + c(1) = util::math::clamp_pm_pi(c(1)); + c(3) = util::math::clamp_pm_pi(c(3)); + return c; + }; + + std::vector GetArmorXYZAList(const Eigen::VectorXd& ekf_x) const { + std::vector _armor_xyza_list; + + for (int i = 0; i < armor_num_; i++) { + auto angle = util::math::clamp_pm_pi(ekf_x(6) + i * 2 * CV_PI / armor_num_); + Eigen::Vector3d xyz = h_armor_xyz(ekf_x, i); + _armor_xyza_list.push_back({ xyz(0), xyz(1), xyz(2), angle }); + } + return _armor_xyza_list; + } + +private: + int armor_num_; + enumeration::CarIDFlag car_id_; + + Eigen::VectorXd P0_dig_; + double radius_; +}; + +} \ No newline at end of file diff --git a/src/tongji/predictor/predictor_record.cpp b/src/tongji/predictor/predictor_record.cpp deleted file mode 100644 index b040c8c..0000000 --- a/src/tongji/predictor/predictor_record.cpp +++ /dev/null @@ -1,58 +0,0 @@ -#include "predictor_record.hpp" - -#include -#include -#include - -#include "data/armor_gimbal_control_spacing.hpp" -#include "enum/car_id.hpp" -#include "enum/enum_tools.hpp" -#include "in_gimbal_control_armor.hpp" - -namespace world_exe::tongji::predictor { -class PredictorRecord::Impl { -public: - Impl(const enumeration::ArmorIdFlag& id, - const std::unordered_map> snapshots) - : id_(id) - , snapshots_(snapshots) { } - - const enumeration::ArmorIdFlag& GetId() const { return id_; } - - std::shared_ptr Predictor(const std::time_t& time_stamp) { - std::unordered_map> - result; - - for (int i = 0; i < 8; i++) { - auto decomposed_id = static_cast( - static_cast(enumeration::CarIDFlag::Hero) << i); - if (enumeration::IsFlagContains(id_, decomposed_id)) { - auto it = snapshots_.find(decomposed_id); - if (it == snapshots_.end()) continue; - - auto copy = std::make_shared(*(it->second)); - copy->Predict(time_stamp); - result[decomposed_id] = copy->GetArmorGimbalControlSpacings(); - } - } - - return std::make_shared(result, time_stamp); - } - -private: - std::unordered_map> snapshots_; - enumeration::ArmorIdFlag id_; -}; - -PredictorRecord::PredictorRecord(const enumeration::ArmorIdFlag& id, - const std::unordered_map>& snapshots) - : pimpl_(std::make_unique(id, snapshots)) { } -PredictorRecord::~PredictorRecord() = default; - -const enumeration ::ArmorIdFlag& PredictorRecord::GetId() const { return pimpl_->GetId(); } - -std ::shared_ptr PredictorRecord::Predictor( - const std ::time_t& time_stamp) const { - return pimpl_->Predictor(time_stamp); -} -} \ No newline at end of file diff --git a/src/tongji/predictor/target.hpp b/src/tongji/predictor/target.hpp deleted file mode 100644 index 7861f1b..0000000 --- a/src/tongji/predictor/target.hpp +++ /dev/null @@ -1,352 +0,0 @@ -#pragma once - -#include -#include -#include -#include - -#include "data/armor_gimbal_control_spacing.hpp" -#include "enum/armor_id.hpp" -#include "enum/car_id.hpp" -#include "interfaces/predictor_update_package.hpp" -#include "util/extended_kalman_filter.hpp" -#include "util/math.hpp" - -namespace world_exe::tongji::predictor { - -struct TargetStatus { - bool jumped = false; - bool switched = false; - bool converged = false; - bool diverged = false; - bool lost = false; - bool reidentified = false; - int last_id = -1; - int switch_count = 0; - int update_count = 0; - int lost_count = 0; -}; - -class Target { -public: - TargetStatus status_; - - Target(const Eigen::Vector3d& armor_xyz_in_world, const Eigen::Vector3d& armor_ypr_in_world, - const enumeration::ArmorIdFlag& id, const std::time_t& t, const double& radius, - const int& armor_num, const Eigen::VectorXd& P0_dig) - : last_time_stamp_(t) - , car_id_(id) - , armor_num_(armor_num) { - - auto r = radius; - - // 旋转中心的坐标 - auto center_x = armor_xyz_in_world[0] + r * std::cos(armor_ypr_in_world[0]); - auto center_y = armor_xyz_in_world[1] + r * std::sin(armor_ypr_in_world[0]); - auto center_z = armor_xyz_in_world[2]; - - // x vx y vy z vz a w r l h - // a: angle - // w: angular velocity - // l: r2 - r1 - // h: z2 - z1 - Eigen::VectorXd x0 { { center_x, 0, center_y, 0, center_z, 0, armor_ypr_in_world[0], 0, r, - 0, 0 } }; // 初始化预测量 - Eigen::MatrixXd P0 = P0_dig.asDiagonal(); - - // 防止夹角求和出现异常值 - auto x_add = [](const Eigen::VectorXd& a, const Eigen::VectorXd& b) -> Eigen::VectorXd { - Eigen::VectorXd c = a + b; - c[6] = util::math::clamp_pm_pi(c[6]); - return c; - }; - - ekf_ = util::ExtendedKalmanFilter(x0, P0, x_add); // 初始化滤波器（预测量、预测量协方差） - } - - Eigen::VectorXd GetEkf_x() const { return ekf_.x; } - - std::time_t GetTimeStamp() const { return last_time_stamp_; } - int GetArmorNum() const { return armor_num_; } - enumeration::CarIDFlag GetId() const { return car_id_; } - - void Predict(const std::time_t& time_stamp) { - const double dt = std::difftime(time_stamp, last_time_stamp_); - Predict(dt); - last_time_stamp_ = time_stamp; - } - - void Update(std::shared_ptr data) { - const auto& transform = data->GetTransform(); - const auto& rotation_transform = Eigen::Quaterniond(transform.linear()); - const auto armors = data->GetArmors()->GetArmors(car_id_); - - if (armors.empty()) return; - for (auto armor : armors) { - const auto& armor_in_world_position = transform * armor.position; - const auto& armor_in_world_oritaiton = rotation_transform * armor.orientation; - this->Update(armor_in_world_position, - util::math::quaternion_to_euler(armor_in_world_oritaiton, 2, 1, 0), - util::math::xyz2ypd(armor_in_world_position)); - } - } - - bool Convergened() { - if (this->car_id_ != enumeration::CarIDFlag::Outpost && status_.update_count > 3 - && !this->Diverged()) { - status_.converged = true; - } - - // 前哨站特殊判断 - if (this->car_id_ == enumeration::CarIDFlag::Outpost && status_.update_count > 10 - && !this->Diverged()) { - status_.converged = true; - } - - return status_.converged; - } - - bool Diverged() const { - auto r_ok = ekf_.x[8] > 0.05 && ekf_.x[8] < 0.5; - auto l_ok = ekf_.x[8] + ekf_.x[9] > 0.05 && ekf_.x[8] + ekf_.x[9] < 0.5; - - if (r_ok && l_ok) return false; - // util::logger::logger()->debug("[Target] r={:.3f}, l={:.3f}", ekf_.x[8], ekf_.x[9]); - return true; - } - - std::vector GetArmorGimbalControlSpacings() { - std::vector armors; - for (int id = 0; id < armor_num_; id++) { - auto angle = util::math::clamp_pm_pi(this->ekf_.x[6] + id * 2 * CV_PI / armor_num_); - auto xyz = h_armor_xyz(this->ekf_.x, id); - - data::ArmorGimbalControlSpacing armor; - armor.id = car_id_; - armor.position = xyz; - armor.orientation = util::math::euler_to_quaternion(angle, 15. / 180. * CV_PI, 0); - armors.emplace_back(std::move(armor)); - } - return armors; - } - -private: - void Predict(double dt) { - // 防止夹角求和出现异常值 - auto f = [&](const Eigen::VectorXd& x) -> Eigen::VectorXd { - Eigen::VectorXd x_prior = this->F(dt) * x; - x_prior[6] = util::math::clamp_pm_pi(x_prior[6]); - return x_prior; - }; - - // 前哨站转速特判 - if (this->Convergened() && this->car_id_ == enumeration::CarIDFlag::Outpost - && std::abs(this->ekf_.x[7]) > 2) - this->ekf_.x[7] = this->ekf_.x[7] > 0 ? 2.51 : -2.51; - - ekf_.Predict(this->F(dt), this->Q(dt), f); - } - - void Update(const Eigen::Vector3d& armor_xyz_in_world, - const Eigen::Vector3d& armor_ypr_in_world, const Eigen::Vector3d& armor_ypd_in_world) { - // 装甲板匹配 - int id = MatchArmor(armor_xyz_in_world, armor_ypr_in_world, armor_ypd_in_world); - if (id != 0) status_.jumped = true; - status_.switched = (id != status_.last_id); - if (status_.switched) status_.switch_count++; - - status_.last_id = id; - Update_ypda(armor_xyz_in_world, armor_ypr_in_world, armor_ypd_in_world, id); - status_.update_count++; - } - - void Update_ypda(const Eigen::Vector3d& armor_xyz_in_world, - const Eigen::Vector3d& armor_ypr_in_world, const Eigen::Vector3d& armor_ypd_in_world, - int id) { - // 观测jacobi - Eigen::MatrixXd H = h_jacobian(ekf_.x, id); - // Eigen::VectorXd R_dig{{4e-3, 4e-3, 1, 9e-2}}; - auto center_yaw = std::atan2(armor_xyz_in_world[1], armor_xyz_in_world[0]); - auto delta_angle = util::math::clamp_pm_pi(armor_ypr_in_world[0] - center_yaw); - Eigen::VectorXd R_dig { { 4e-3, 4e-3, log(std::abs(delta_angle) + 1) + 1, - log(std::abs(armor_ypd_in_world[2]) + 1) / 200 + 9e-2 } }; - - // 测量过程噪声偏差的方差 - Eigen::MatrixXd R = R_dig.asDiagonal(); - - // 定义非线性转换函数h: x -> z - auto h = [&](const Eigen::VectorXd& x) -> Eigen::Vector4d { - Eigen::VectorXd xyz = h_armor_xyz(x, id); - Eigen::VectorXd ypd = util::math::xyz2ypd(xyz); - auto angle = util::math::clamp_pm_pi(x[6] + id * 2 * CV_PI / armor_num_); - return { ypd[0], ypd[1], ypd[2], angle }; - }; - - // 防止夹角求差出现异常值 - auto z_subtract = [](const Eigen::VectorXd& a, - const Eigen::VectorXd& b) -> Eigen::VectorXd { - Eigen::VectorXd c = a - b; - c[0] = util::math::clamp_pm_pi(c[0]); - c[1] = util::math::clamp_pm_pi(c[1]); - c[3] = util::math::clamp_pm_pi(c[3]); - return c; - }; - - const Eigen::VectorXd& ypd = armor_ypd_in_world; - const Eigen::VectorXd& ypr = armor_ypr_in_world; - Eigen::VectorXd z { { ypd[0], ypd[1], ypd[2], ypr[0] } }; // 获得观测量 - - ekf_.update(z, H, R, h, z_subtract); - } - - Eigen::MatrixXd h_jacobian(const Eigen::VectorXd& x, int id) const { - auto angle = util::math::clamp_pm_pi(x[6] + id * 2 * CV_PI / armor_num_); - auto use_l_h = (armor_num_ == 4) && (id == 1 || id == 3); - - auto r = (use_l_h) ? x[8] + x[9] : x[8]; - auto dx_da = r * std::sin(angle); - auto dy_da = -r * std::cos(angle); - - auto dx_dr = -std::cos(angle); - auto dy_dr = -std::sin(angle); - auto dx_dl = (use_l_h) ? -std::cos(angle) : 0.0; - auto dy_dl = (use_l_h) ? -std::sin(angle) : 0.0; - - auto dz_dh = (use_l_h) ? 1.0 : 0.0; - - // clang-format off - Eigen::MatrixXd H_armor_xyza{ - {1, 0, 0, 0, 0, 0, dx_da, 0, dx_dr, dx_dl, 0}, - {0, 0, 1, 0, 0, 0, dy_da, 0, dy_dr, dy_dl, 0}, - {0, 0, 0, 0, 1, 0, 0, 0, 0, 0, dz_dh}, - {0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0} - }; - // clang-format on - - Eigen::VectorXd armor_xyz = h_armor_xyz(x, id); - Eigen::MatrixXd H_armor_ypd = util::math::xyz2ypd_jacobian(armor_xyz); - // clang-format off - Eigen::MatrixXd H_armor_ypda{ - {H_armor_ypd(0, 0), H_armor_ypd(0, 1), H_armor_ypd(0, 2), 0}, - {H_armor_ypd(1, 0), H_armor_ypd(1, 1), H_armor_ypd(1, 2), 0}, - {H_armor_ypd(2, 0), H_armor_ypd(2, 1), H_armor_ypd(2, 2), 0}, - { 0, 0, 0, 1} - }; - // clang-format on - - return H_armor_ypda * H_armor_xyza; - } - - int MatchArmor(const Eigen::Vector3d& armor_xyz_in_world, - const Eigen::Vector3d& armor_ypr_in_world, const Eigen::Vector3d& armor_ypd_in_world) { - - const auto& xyza_list = armor_xyza_list(); - std::vector> xyza_i_list; - - for (int i = 0; i < armor_num_; ++i) { - xyza_i_list.emplace_back(xyza_list[i], i); - } - - std::sort(xyza_i_list.begin(), xyza_i_list.end(), [](const auto& a, const auto& b) { - auto ypd1 = util::math::xyz2ypd(a.first.head(3)); - auto ypd2 = util::math::xyz2ypd(b.first.head(3)); - return ypd1[2] < ypd2[2]; - }); - - int best_id = 0; - double min_error = 1e10; - - for (int i = 0; i < std::min(3, armor_num_); ++i) { - const auto& xyza = xyza_i_list[i].first; - auto ypd = util::math::xyz2ypd(xyza.head(3)); - double error = std::abs(util::math::clamp_pm_pi(armor_ypr_in_world[0] - xyza[3])) - + std::abs(util::math::clamp_pm_pi(armor_ypd_in_world[0] - ypd[0])); - - if (error < min_error) { - min_error = error; - best_id = xyza_i_list[i].second; - } - } - - return best_id; - } - - std::vector armor_xyza_list() const { - std::vector _armor_xyza_list; - - for (int i = 0; i < armor_num_; i++) { - auto angle = util::math::clamp_pm_pi(ekf_.x[6] + i * 2 * CV_PI / armor_num_); - Eigen::Vector3d xyz = h_armor_xyz(ekf_.x, i); - _armor_xyza_list.push_back({ xyz[0], xyz[1], xyz[2], angle }); - } - return _armor_xyza_list; - } - - // 计算出装甲板中心的坐标（考虑长短轴） - Eigen::Vector3d h_armor_xyz(const Eigen::VectorXd& x, int id) const { - auto angle = util::math::clamp_pm_pi(x[6] + id * 2 * CV_PI / armor_num_); - auto use_l_h = (armor_num_ == 4) && (id == 1 || id == 3); - - auto r = (use_l_h) ? x[8] + x[9] : x[8]; - auto armor_x = x[0] - r * std::cos(angle); - auto armor_y = x[2] - r * std::sin(angle); - auto armor_z = (use_l_h) ? x[4] + x[10] : x[4]; - - return { armor_x, armor_y, armor_z }; - } - - auto F(double dt) -> Eigen::MatrixXd { - // 状态转移矩阵 - // clang-format off - return (Eigen::MatrixXd(11, 11) << - 1, dt, 0, 0, 0, 0, 0, 0, 0, 0, 0, - 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, - 0, 0, 1, dt, 0, 0, 0, 0, 0, 0, 0, - 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, - 0, 0, 0, 0, 1, dt, 0, 0, 0, 0, 0, - 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, - 0, 0, 0, 0, 0, 0, 1, dt, 0, 0, 0, - 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, - 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, - 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, - 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1).finished(); -} - -auto Q(double dt)->Eigen::MatrixXd{ - // Piecewise White Noise Model - // https://github.com/rlabbe/Kalman-and-Bayesian-Filters-in-Python/blob/master/07-Kalman-Filter-Math.ipynb - double v1, v2; - if (car_id_ == enumeration::CarIDFlag::Outpost) { - v1 = 10; // 前哨站加速度方差 - v2 = 0.1; // 前哨站角加速度方差 - } else { - v1 = 100; // 加速度方差 - v2 = 400; // 角加速度方差 - } - auto a = dt * dt * dt * dt / 4; - auto b = dt * dt * dt / 2; - auto c = dt * dt; - - // 预测过程噪声偏差的方差 - // clang-format off - return (Eigen::MatrixXd(11, 11) << - a * v1, b * v1, 0, 0, 0, 0, 0, 0, 0, 0, 0, - b * v1, c * v1, 0, 0, 0, 0, 0, 0, 0, 0, 0, - 0, 0, a * v1, b * v1, 0, 0, 0, 0, 0, 0, 0, - 0, 0, b * v1, c * v1, 0, 0, 0, 0, 0, 0, 0, - 0, 0, 0, 0, a * v1, b * v1, 0, 0, 0, 0, 0, - 0, 0, 0, 0, b * v1, c * v1, 0, 0, 0, 0, 0, - 0, 0, 0, 0, 0, 0, a * v2, b * v2, 0, 0, 0, - 0, 0, 0, 0, 0, 0, b * v2, c * v2, 0, 0, 0, - 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, - 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, - 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0).finished(); - // clang-format on - } - - std::time_t last_time_stamp_; - util::ExtendedKalmanFilter ekf_; - enumeration::CarIDFlag car_id_; - int armor_num_; -}; -} \ No newline at end of file diff --git a/src/tongji/predictor/target_predictor.cpp b/src/tongji/predictor/target_predictor.cpp deleted file mode 100644 index 436b823..0000000 --- a/src/tongji/predictor/target_predictor.cpp +++ /dev/null @@ -1,79 +0,0 @@ -#include "target_predictor.hpp" - -#include -#include -#include - -#include "enum/enum_tools.hpp" -#include "tongji/predictor/in_gimbal_control_armor.hpp" -#include "tongji/predictor/predictor_record.hpp" -#include "tongji/predictor/target.hpp" - -namespace world_exe::tongji::predictor { - -class TargetPredict::Impl { -public: - Impl(std::unordered_map> targets) { } - void RegisterTarget(enumeration::ArmorIdFlag id, const Target& target) { - targets_[id] = std::make_shared(target); - } - - void RemoveTarget(enumeration::ArmorIdFlag id) { } - - std::shared_ptr Predict( - const enumeration::ArmorIdFlag& id, const std::time_t& time_stamp) { - std::unordered_map> - result; - - for (int i = 0; i < 8; i++) { - auto decomposed_id = static_cast( - static_cast(enumeration::CarIDFlag::Hero) << i); - if (enumeration::IsFlagContains(id, decomposed_id)) { - auto it = targets_.find(decomposed_id); - if (it == targets_.end()) continue; - - it->second->Predict(time_stamp); - result[decomposed_id] = it->second->GetArmorGimbalControlSpacings(); - } - } - - return std::make_shared(result, time_stamp); - } - - std::shared_ptr GetPredictor(const enumeration::ArmorIdFlag& id) const { - std::unordered_map> snapshot_map; - - for (int i = 0; i < 8; i++) { - auto decomposed_id = static_cast( - static_cast(enumeration::CarIDFlag::Hero) << i); - if (enumeration::IsFlagContains(id, decomposed_id)) { - - auto it = targets_.find(decomposed_id); - if (it == targets_.end()) continue; - - snapshot_map[decomposed_id] = std::make_shared(*(it->second)); - } - } - return std::make_shared(id, snapshot_map); - } - -private: - std::unordered_map> targets_; -}; - -TargetPredict::TargetPredict( - std::unordered_map> targets) - : pimpl_(std::make_unique(std::move(targets))) { } -TargetPredict::~TargetPredict() = default; - -std ::shared_ptr TargetPredict::Predict( - const enumeration ::ArmorIdFlag& id, const std ::time_t& time_stamp) { - return pimpl_->Predict(id, time_stamp); -} - -std ::shared_ptr TargetPredict::GetPredictor( - const enumeration ::ArmorIdFlag& id) const { - return pimpl_->GetPredictor(id); -} - -} \ No newline at end of file diff --git a/src/tongji/predictor/target_snapshot.hpp b/src/tongji/predictor/target_snapshot.hpp new file mode 100644 index 0000000..0dc4a61 --- /dev/null +++ b/src/tongji/predictor/target_snapshot.hpp @@ -0,0 +1,36 @@ +#pragma once + +#include "tongji/predictor/live_target.hpp" +#include "tongji/predictor/predict_model.hpp" +#include "util/extended_kalman_filter.hpp" + +namespace world_exe::tongji::predictor { + +class TargetSnapshot { +public: + TargetSnapshot(const LiveTarget& target) + : model_(target.GetModel()) + , ekf_(target.GetEkfX(), target.GetP0Dig().asDiagonal(), model_.x_add) { } + + std::vector GetArmorGimbalControlSpacings() const { + std::vector armors; + for (int id = 0; id < model_.GetArmorNum(); id++) { + auto angle = + util::math::clamp_pm_pi(this->ekf_.x[6] + id * 2 * CV_PI / model_.GetArmorNum()); + auto xyz = model_.h_armor_xyz(this->ekf_.x, id); + + data::ArmorGimbalControlSpacing armor; + armor.id = model_.GetID(); + armor.position = xyz; + armor.orientation = util::math::euler_to_quaternion(angle, 15. / 180. * CV_PI, 0); + armors.emplace_back(std::move(armor)); + } + return armors; + } + +private: + PredictModel model_; + util::ExtendedKalmanFilter ekf_; +}; + +} \ No newline at end of file diff --git a/src/tongji/predictor/target_snapshot_manager.cpp b/src/tongji/predictor/target_snapshot_manager.cpp new file mode 100644 index 0000000..a660c9b --- /dev/null +++ b/src/tongji/predictor/target_snapshot_manager.cpp @@ -0,0 +1,51 @@ +#include "target_snapshot_manager.hpp" + +#include +#include + +#include "data/armor_gimbal_control_spacing.hpp" +#include "enum/enum_tools.hpp" +#include "in_gimbal_control_armor.hpp" +#include "tongji/predictor/target_snapshot.hpp" + +namespace world_exe::tongji::predictor { +class TargetSnapshotManager::Impl { +public: + Impl(const enumeration::ArmorIdFlag& id, + const std::unordered_map>& snapshots) + : id_(id) { + for (const auto& [flag, target] : snapshots) { + snapshots_.emplace(flag, TargetSnapshot(*target)); + } + } + + const enumeration::ArmorIdFlag& GetId() const { return id_; } + + std::shared_ptr Predictor( + const std::time_t& time_stamp) const { + std::unordered_map> + result; + + for (const auto& [flag, snapshot] : snapshots_) { + result[flag] = snapshot.GetArmorGimbalControlSpacings(); + } + return std::make_shared(result, time_stamp); + } + +private: + std::unordered_map snapshots_; + enumeration::ArmorIdFlag id_; +}; + +TargetSnapshotManager::TargetSnapshotManager(const enumeration::ArmorIdFlag& id, + const std::unordered_map> snapshots) + : pimpl_(std::make_unique(id, snapshots)) { } +TargetSnapshotManager::~TargetSnapshotManager() = default; + +const enumeration ::ArmorIdFlag& TargetSnapshotManager::GetId() const { return pimpl_->GetId(); } + +std ::shared_ptr TargetSnapshotManager::Predictor( + const std ::time_t& time_stamp) const { + return pimpl_->Predictor(time_stamp); +} +} \ No newline at end of file diff --git a/src/tongji/predictor/predictor_record.hpp b/src/tongji/predictor/target_snapshot_manager.hpp similarity index 67% rename from src/tongji/predictor/predictor_record.hpp rename to src/tongji/predictor/target_snapshot_manager.hpp index 409c37f..02394fd 100644 --- a/src/tongji/predictor/predictor_record.hpp +++ b/src/tongji/predictor/target_snapshot_manager.hpp @@ -5,15 +5,15 @@ #include "enum/armor_id.hpp" #include "interfaces/predictor.hpp" -#include "tongji/predictor/target.hpp" +#include "tongji/predictor/live_target.hpp" namespace world_exe::tongji::predictor { -class PredictorRecord final : public interfaces::IPredictor { +class TargetSnapshotManager final : public interfaces::IPredictor { public: - PredictorRecord(const enumeration::ArmorIdFlag& id, - const std::unordered_map>& snapshots); - ~PredictorRecord(); + TargetSnapshotManager(const enumeration::ArmorIdFlag& id, + const std::unordered_map> snapshots); + ~TargetSnapshotManager(); const enumeration ::ArmorIdFlag& GetId() const override; diff --git a/src/util/extended_kalman_filter.hpp b/src/util/extended_kalman_filter.hpp index 8b1a2e0..73fc86b 100644 --- a/src/util/extended_kalman_filter.hpp +++ b/src/util/extended_kalman_filter.hpp @@ -29,51 +29,38 @@ class ExtendedKalmanFilter { data["recent_nis_failures"] = 0.0; } - Eigen::VectorXd predict(const Eigen::MatrixXd& F, const Eigen::MatrixXd& Q) { - return Predict(F, Q, [&](const Eigen::VectorXd& x) { return F * x; }); - } - - Eigen::VectorXd Predict(const Eigen::MatrixXd& F, const Eigen::MatrixXd& Q, - std::function f) { - P = F * P * F.transpose() + Q; - x = f(x); - return x; - } - Eigen::VectorXd Update( - const Eigen::VectorXd& z, const Eigen::MatrixXd& H, const Eigen::MatrixXd& R, - std::function z_subtract = - [](const Eigen::VectorXd& a, const Eigen::VectorXd& b) { return a - b; }) { - return update(z, H, R, [&](const Eigen::VectorXd& x) { return H * x; }, z_subtract); - } - - Eigen::VectorXd update( + const double& dt, const Eigen::MatrixXd& A, const Eigen::MatrixXd& Q, + std::function f, const Eigen::VectorXd& z, const Eigen::MatrixXd& H, const Eigen::MatrixXd& R, std::function h, std::function z_subtract = [](const Eigen::VectorXd& a, const Eigen::VectorXd& b) { return a - b; }) { - Eigen::VectorXd x_prior = x; - Eigen::MatrixXd K = P * H.transpose() * (H * P * H.transpose() + R).inverse(); + + auto x_n = f(x, dt); + + auto P_n = A * P * A.transpose() + Q; + + auto residual = z_subtract(z, h(x_n)); + + auto S = H * P * H.transpose() + R; + + auto K = P_n * H.transpose() * S.inverse(); + + x = x_add(x, K * residual); // Stable Compution of the Posterior Covariance // https://github.com/rlabbe/Kalman-and-Bayesian-Filters-in-Python/blob/master/07-Kalman-Filter-Math.ipynb P = (I - K * H) * P * (I - K * H).transpose() + K * R * K.transpose(); - x = x_add(x, K * z_subtract(z, h(x))); - /// 卡方检验 - Eigen::VectorXd residual = z_subtract(z, h(x)); // 新增检验 - Eigen::MatrixXd S = H * P * H.transpose() + R; - double nis = residual.transpose() * S.inverse() * residual; - double nees = (x - x_prior).transpose() * P.inverse() * (x - x_prior); + double nis = residual.transpose() * S.inverse() * residual; // 卡方检验阈值（自由度=4，取置信水平95%） - constexpr double nis_threshold = 0.711; - constexpr double nees_threshold = 0.711; + constexpr double nis_threshold = 0.711; if (nis > nis_threshold) nis_count_++, data["nis_fail"] = 1; - if (nees > nees_threshold) nees_count_++, data["nees_fail"] = 1; total_count_++; last_nis = nis; @@ -92,7 +79,6 @@ class ExtendedKalmanFilter { data["residual_distance"] = residual[2]; data["residual_angle"] = residual[3]; data["nis"] = nis; - data["nees"] = nees; data["recent_nis_failures"] = recent_rate; return x; From b873347d5b2aa608594e92c870318b17eaab99f1 Mon Sep 17 00:00:00 2001 From: heyeuu <2829004293@qq.com> Date: Mon, 15 Sep 2025 01:33:04 +0800 Subject: [PATCH 27/53] feat: add state machine --- src/tongji/state_machine/car_state_manager.hpp | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/src/tongji/state_machine/car_state_manager.hpp b/src/tongji/state_machine/car_state_manager.hpp index 219e457..799183e 100644 --- a/src/tongji/state_machine/car_state_manager.hpp +++ b/src/tongji/state_machine/car_state_manager.hpp @@ -1,8 +1,12 @@ #pragma once +<<<<<<< HEAD #include #include "tongji/predictor/time_stamp.hpp" +======= +#include +>>>>>>> 04b0cf7 (feat: add state machine) namespace world_exe::tongji::car_state { From e74646462618f49cc4a2321f1e6eb265ac5b6e0e Mon Sep 17 00:00:00 2001 From: heyeuu <2829004293@qq.com> Date: Tue, 23 Sep 2025 21:19:07 +0800 Subject: [PATCH 28/53] merge: fix conflicts from git merge --- src/tongji/state_machine/car_state_manager.hpp | 4 ---- 1 file changed, 4 deletions(-) diff --git a/src/tongji/state_machine/car_state_manager.hpp b/src/tongji/state_machine/car_state_manager.hpp index 799183e..219e457 100644 --- a/src/tongji/state_machine/car_state_manager.hpp +++ b/src/tongji/state_machine/car_state_manager.hpp @@ -1,12 +1,8 @@ #pragma once -<<<<<<< HEAD #include #include "tongji/predictor/time_stamp.hpp" -======= -#include ->>>>>>> 04b0cf7 (feat: add state machine) namespace world_exe::tongji::car_state { From 2d7eb5231a51141bc9b3cf9306934225fb341ca7 Mon Sep 17 00:00:00 2001 From: heyeuu <2829004293@qq.com> Date: Thu, 25 Sep 2025 22:49:25 +0800 Subject: [PATCH 29/53] fix(targeting): correct armor target acquisition logic --- src/tongji/predictor/live_target.hpp | 89 ++++++++++++++++---- src/tongji/predictor/live_target_manager.cpp | 20 +---- src/util/index.hpp | 15 ++++ 3 files changed, 93 insertions(+), 31 deletions(-) diff --git a/src/tongji/predictor/live_target.hpp b/src/tongji/predictor/live_target.hpp index 102c2d6..c9fece5 100644 --- a/src/tongji/predictor/live_target.hpp +++ b/src/tongji/predictor/live_target.hpp @@ -1,8 +1,10 @@ #pragma once #include +#include +#include #include -#include +#include #include "data/armor_gimbal_control_spacing.hpp" #include "enum/car_id.hpp" @@ -20,6 +22,7 @@ struct TargetStatus { bool lost = false; bool reidentified = false; int last_id = -1; + double lock_id_ = -1; int switch_count = 0; int update_count = 0; int lost_count = 0; @@ -59,20 +62,74 @@ class LiveTarget { const PredictModel& GetModel() const { return model_; } std::time_t GetTimeStamp() const { return last_time_stamp_; } - std::vector GetArmorGimbalControlSpacings() { - std::vector armors; - for (int id = 0; id < model_.GetArmorNum(); id++) { - auto angle = - util::math::clamp_pm_pi(this->ekf_.x[6] + id * 2 * CV_PI / model_.GetArmorNum()); - auto xyz = model_.h_armor_xyz(this->ekf_.x, id); - - data::ArmorGimbalControlSpacing armor; - armor.id = model_.GetID(); - armor.position = xyz; - armor.orientation = util::math::euler_to_quaternion(angle, 15. / 180. * CV_PI, 0); - armors.emplace_back(std::move(armor)); + data::ArmorGimbalControlSpacing GetTargetArmorGimbalControlSpacings() { + const auto& ekf_x = GetEkfX(); + const auto& xyza_list = model_.GetArmorXYZAList(ekf_x); + + Eigen::Vector4d xyza; + data::ArmorGimbalControlSpacing armor; + + if (!status_.jumped) { + // 如果装甲板未发生过跳变，则只有当前装甲板的位置已知 + xyza = xyza_list.at(0); + } + + // 整车旋转中心的球坐标yaw + auto center_yaw = std::atan2(ekf_x[2], ekf_x[0]); + std::vector delta_angle_list; + for (int i = 0; i < model_.GetArmorNum(); i++) { + auto delta_angle = util::math::clamp_pm_pi(xyza_list[i][3] - center_yaw); + delta_angle_list.emplace_back(delta_angle); + } + + // 不考虑小陀螺 + if (std::abs(ekf_x[8]) <= 2 && model_.GetID() != enumeration::CarIDFlag::Outpost) { + // 选择在可射击范围内的装甲板 + std::vector id_list; + for (int i = 0; i < model_.GetArmorNum(); i++) { + if (std::abs(delta_angle_list[i]) > 60 / 57.3) continue; + id_list.push_back(i); + } + + // 锁定模式：防止在两个都呈45度的装甲板之间来回切换 + if (id_list.size() > 1) { + int id0 = id_list[0], id1 = id_list[1]; + + // 未处于锁定模式时，选择delta_angle绝对值较小的装甲板，进入锁定模式 + if (status_.lock_id_ != id0 && status_.lock_id_ != id1) + status_.lock_id_ = + (std::abs(delta_angle_list[id0]) < std::abs(delta_angle_list[id1])) ? id0 + : id1; + xyza = xyza_list.at(status_.lock_id_); + } + + // 只有一个装甲板在可射击范围内时，退出锁定模式 + status_.lock_id_ = -1; + xyza = xyza_list.at(id_list[0]); + } + + double coming_angle, leaving_angle; + if (model_.GetID() == enumeration::CarIDFlag::Outpost) { + coming_angle = 70 / 57.3; + leaving_angle = 30 / 57.3; + } else { + coming_angle = comming_angle_; + leaving_angle = leaving_angle_; + } + + // 在小陀螺时，一侧的装甲板不断出现，另一侧的装甲板不断消失，显然前者被打中的概率更高 + for (int i = 0; i < model_.GetArmorNum(); i++) { + if (std::abs(delta_angle_list[i]) > coming_angle) continue; + if (ekf_x[7] > 0 && delta_angle_list[i] < leaving_angle) xyza = xyza_list[i]; + else if (ekf_x[7] < 0 && delta_angle_list[i] > -leaving_angle) xyza = xyza_list[i]; } - return armors; + + armor.id = model_.GetID(); + armor.position = xyza.head<3>(); + const auto& angle = xyza[3]; + armor.orientation = util::math::euler_to_quaternion(angle, 15. / 180. * CV_PI, 0); + + return armor; } void Update(const double& dt, const Eigen::Vector3d& armor_xyz_in_world, @@ -144,6 +201,8 @@ class LiveTarget { std::time_t last_time_stamp_; util::ExtendedKalmanFilter ekf_; PredictModel model_; -}; + double comming_angle_ = 60 / 57.3; + double leaving_angle_ = 20 / 57.3; +}; } \ No newline at end of file diff --git a/src/tongji/predictor/live_target_manager.cpp b/src/tongji/predictor/live_target_manager.cpp index fda3cb3..8d662af 100644 --- a/src/tongji/predictor/live_target_manager.cpp +++ b/src/tongji/predictor/live_target_manager.cpp @@ -10,6 +10,7 @@ #include "tongji/predictor/in_gimbal_control_armor.hpp" #include "tongji/predictor/live_target.hpp" #include "tongji/predictor/target_snapshot_manager.hpp" +#include "util/index.hpp" namespace world_exe::tongji::predictor { @@ -28,10 +29,10 @@ class LiveTargetManager::Impl { std::unordered_map> result; - for (auto id : ExpandArmorIdFlags(flag)) { + for (auto id : util::enumeration::ExpandArmorIdFlags(flag)) { auto it = targets_.find(id); if (it != targets_.end() && it->second) { - result[id] = it->second->GetArmorGimbalControlSpacings(); + result[id].emplace_back(it->second->GetTargetArmorGimbalControlSpacings()); } } @@ -42,7 +43,7 @@ class LiveTargetManager::Impl { const enumeration::ArmorIdFlag& flag) const { std::unordered_map> snapshot_map; - for (auto id : ExpandArmorIdFlags(flag)) { + for (auto id : util::enumeration::ExpandArmorIdFlags(flag)) { auto it = targets_.find(id); if (it != targets_.end() && it->second) { snapshot_map[id] = it->second; @@ -72,19 +73,6 @@ class LiveTargetManager::Impl { } private: - inline std::vector ExpandArmorIdFlags( - enumeration::ArmorIdFlag flags) const { - std::vector result; - for (int i = 0; i < static_cast(enumeration::ArmorIdFlag::Count); ++i) { - auto single = static_cast( - static_cast(enumeration::ArmorIdFlag::Hero) << i); - if ((static_cast(flags) & static_cast(single)) != 0) { - result.push_back(single); - } - } - return result; - } - std::unordered_map> targets_; }; diff --git a/src/util/index.hpp b/src/util/index.hpp index 350fe26..7a300a8 100644 --- a/src/util/index.hpp +++ b/src/util/index.hpp @@ -1,5 +1,6 @@ #pragma once +#include #define BACKWARD_HAS_DW 1 // #include "backward.hpp" #include "enum/armor_id.hpp" @@ -31,4 +32,18 @@ static const world_exe::enumeration::ArmorIdFlag GetArmorIdFlag(int index) { throw std::out_of_range("Invalid index for ArmorIdFlag conversion."); } } + +static std::vector<::world_exe::enumeration::ArmorIdFlag> ExpandArmorIdFlags( + ::world_exe::enumeration::ArmorIdFlag flags) { + std::vector<::world_exe::enumeration::ArmorIdFlag> result; + for (int i = 0; i < static_cast(::world_exe::enumeration::ArmorIdFlag::Count); ++i) { + auto single = static_cast<::world_exe::enumeration::ArmorIdFlag>( + static_cast(::world_exe::enumeration::ArmorIdFlag::Hero) << i); + if ((static_cast(flags) & static_cast(single)) != 0) { + result.push_back(single); + } + } + return result; +} + } \ No newline at end of file From 30235d25e1cac95fedf574c4a6eddf89f70125ac Mon Sep 17 00:00:00 2001 From: heyeuu <2829004293@qq.com> Date: Fri, 26 Sep 2025 10:18:42 +0800 Subject: [PATCH 30/53] refactor(predict, utils): move Extended Kalman Filter from utils to predict module --- .../predictor}/extended_kalman_filter.hpp | 2 +- src/tongji/predictor/live_target.hpp | 9 ++++----- src/tongji/predictor/target_snapshot.hpp | 4 ++-- 3 files changed, 7 insertions(+), 8 deletions(-) rename src/{util => tongji/predictor}/extended_kalman_filter.hpp (98%) diff --git a/src/util/extended_kalman_filter.hpp b/src/tongji/predictor/extended_kalman_filter.hpp similarity index 98% rename from src/util/extended_kalman_filter.hpp rename to src/tongji/predictor/extended_kalman_filter.hpp index 73fc86b..32450db 100644 --- a/src/util/extended_kalman_filter.hpp +++ b/src/tongji/predictor/extended_kalman_filter.hpp @@ -6,7 +6,7 @@ #include #include -namespace world_exe::util { +namespace world_exe::tongji::predictor { class ExtendedKalmanFilter { public: Eigen::VectorXd x; diff --git a/src/tongji/predictor/live_target.hpp b/src/tongji/predictor/live_target.hpp index c9fece5..162d38e 100644 --- a/src/tongji/predictor/live_target.hpp +++ b/src/tongji/predictor/live_target.hpp @@ -8,8 +8,8 @@ #include "data/armor_gimbal_control_spacing.hpp" #include "enum/car_id.hpp" +#include "extended_kalman_filter.hpp" #include "predict_model.hpp" -#include "util/extended_kalman_filter.hpp" #include "util/math.hpp" namespace world_exe::tongji::predictor { @@ -53,8 +53,7 @@ class LiveTarget { 0, 0; Eigen::MatrixXd P0 = model_.GetP0Dig().asDiagonal(); - ekf_ = util::ExtendedKalmanFilter( - x0, P0, model_.x_add); // 初始化滤波器（预测量、预测量协方差） + ekf_ = ExtendedKalmanFilter(x0, P0, model_.x_add); // 初始化滤波器（预测量、预测量协方差） } Eigen::VectorXd GetEkfX() const { return ekf_.x; } @@ -66,7 +65,7 @@ class LiveTarget { const auto& ekf_x = GetEkfX(); const auto& xyza_list = model_.GetArmorXYZAList(ekf_x); - Eigen::Vector4d xyza; + Eigen::Vector4d xyza = xyza_list.at(0); data::ArmorGimbalControlSpacing armor; if (!status_.jumped) { @@ -199,7 +198,7 @@ class LiveTarget { } std::time_t last_time_stamp_; - util::ExtendedKalmanFilter ekf_; + ExtendedKalmanFilter ekf_; PredictModel model_; double comming_angle_ = 60 / 57.3; diff --git a/src/tongji/predictor/target_snapshot.hpp b/src/tongji/predictor/target_snapshot.hpp index 0dc4a61..fcffdf0 100644 --- a/src/tongji/predictor/target_snapshot.hpp +++ b/src/tongji/predictor/target_snapshot.hpp @@ -1,8 +1,8 @@ #pragma once +#include "extended_kalman_filter.hpp" #include "tongji/predictor/live_target.hpp" #include "tongji/predictor/predict_model.hpp" -#include "util/extended_kalman_filter.hpp" namespace world_exe::tongji::predictor { @@ -30,7 +30,7 @@ class TargetSnapshot { private: PredictModel model_; - util::ExtendedKalmanFilter ekf_; + ExtendedKalmanFilter ekf_; }; } \ No newline at end of file From 1538095ed81b5641cf990339495ffa9206547293 Mon Sep 17 00:00:00 2001 From: heyeuu <2829004293@qq.com> Date: Mon, 29 Sep 2025 01:06:12 +0800 Subject: [PATCH 31/53] feat: add trajectory --- src/tongji/fire_controller/trajectory.hpp | 42 +++++++++++++++++++++++ 1 file changed, 42 insertions(+) create mode 100644 src/tongji/fire_controller/trajectory.hpp diff --git a/src/tongji/fire_controller/trajectory.hpp b/src/tongji/fire_controller/trajectory.hpp new file mode 100644 index 0000000..3799c2c --- /dev/null +++ b/src/tongji/fire_controller/trajectory.hpp @@ -0,0 +1,42 @@ +#pragma once + +#include +namespace world_exe::tongji::fire_control { + +struct TrajectorySolver { + bool solvable = true; + double fly_time; + double pitch; // 抬头为正 + + // 不考虑空气阻力 + // v0 子弹初速度大小，单位：m/s + // d 目标水平距离，单位：m + // h 目标竖直高度，单位：m + // g 重力加速度，单位：m/s^2 + //(g·x²)/(2v₀²)·u² - x·u + (g·x²)/(2v₀²) + y = 0(其中u = tan(θ)) + TrajectorySolver(const double& v0, const double& d, const double& h, const double& g) { + auto a = g * d * d / (2 * v0 * v0); + auto b = -d; + auto c = a + h; + auto delta = b * b - 4 * a * c; + + if (delta < 0) { + solvable = false; + return; + } + + solvable = true; + + auto tan_pitch_1 = (-b + std::sqrt(delta)) / (2 * a); + auto tan_pitch_2 = (-b - std::sqrt(delta)) / (2 * a); + auto pitch_1 = std::atan(tan_pitch_1); + auto pitch_2 = std::atan(tan_pitch_2); + auto t_1 = d / (v0 * std::cos(pitch_1)); + auto t_2 = d / (v0 * std::cos(pitch_2)); + + pitch = (t_1 < t_2) ? pitch_1 : pitch_2; + fly_time = (t_1 < t_2) ? t_1 : t_2; + } +}; + +} \ No newline at end of file From 442f3660590b758ca1c8a0bcedd50c0f21d12ff9 Mon Sep 17 00:00:00 2001 From: alray <1780284652@qq.com> Date: Thu, 2 Oct 2025 00:54:12 +0800 Subject: [PATCH 32/53] =?UTF-8?q?[Add]=20=E6=B7=BB=E5=8A=A0=E7=81=AB?= =?UTF-8?q?=E6=8E=A7=E5=AE=9E=E7=8E=B0=E7=9A=84=E5=A4=B4=E6=96=87=E4=BB=B6?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/tongji/fire_controller/firecontrol.hpp | 14 ++++++++++++ .../fire_controller/firecontrol_impl.cpp | 12 ++++++++++ src/tongji/fire_controller/tracker.hpp | 22 +++++++++++++++++++ src/tongji/predictor/live_target_manager.hpp | 1 - 4 files changed, 48 insertions(+), 1 deletion(-) create mode 100644 src/tongji/fire_controller/firecontrol.hpp create mode 100644 src/tongji/fire_controller/firecontrol_impl.cpp create mode 100644 src/tongji/fire_controller/tracker.hpp diff --git a/src/tongji/fire_controller/firecontrol.hpp b/src/tongji/fire_controller/firecontrol.hpp new file mode 100644 index 0000000..cb81d96 --- /dev/null +++ b/src/tongji/fire_controller/firecontrol.hpp @@ -0,0 +1,14 @@ +#pragma once + +#include "interfaces/fire_controller.hpp" +namespace world_exe::tongji::fire_controller { +class FireControl final : interfaces::IFireControl { + + const data::FireControl CalculateTarget(const std::time_t& time_duration) const override; + + const enumeration::CarIDFlag GetAttackCarId() const override; + + struct Impl; + std::unique_ptr pimpl_; +}; +} diff --git a/src/tongji/fire_controller/firecontrol_impl.cpp b/src/tongji/fire_controller/firecontrol_impl.cpp new file mode 100644 index 0000000..43c55f4 --- /dev/null +++ b/src/tongji/fire_controller/firecontrol_impl.cpp @@ -0,0 +1,12 @@ + +#include "./firecontrol.hpp" +#include "tongji/fire_controller/tracker.hpp" + +using namespace world_exe::tongji::fire_controller; +class FireControl::Impl { + // 后续转依赖注入 +public: + explicit Impl(DefaultTracker tracker) { } + + // impl interface here +}; diff --git a/src/tongji/fire_controller/tracker.hpp b/src/tongji/fire_controller/tracker.hpp new file mode 100644 index 0000000..f2314be --- /dev/null +++ b/src/tongji/fire_controller/tracker.hpp @@ -0,0 +1,22 @@ +#pragma once + +#include "../predictor/target_snapshot.hpp" +#include "../predictor/target_snapshot_manager.hpp" + +class DefaultTracker { + using TargetSnapshotManager = world_exe::tongji::predictor::TargetSnapshotManager; + using TargetSnapshot = world_exe::tongji::predictor::TargetSnapshot; + +public: + DefaultTracker(const std::shared_ptr& snapshot_manager) + : snapshot_manager_(snapshot_manager) {}; + + ~DefaultTracker() = default; + + auto CalculateTarget() noexcept -> TargetSnapshot { + // tongji shooter::tracker impl here + } + +private: + const std::shared_ptr snapshot_manager_; +}; \ No newline at end of file diff --git a/src/tongji/predictor/live_target_manager.hpp b/src/tongji/predictor/live_target_manager.hpp index f78580f..33937ab 100644 --- a/src/tongji/predictor/live_target_manager.hpp +++ b/src/tongji/predictor/live_target_manager.hpp @@ -5,7 +5,6 @@ #include "enum/armor_id.hpp" #include "interfaces/target_predictor.hpp" - namespace world_exe::tongji::predictor { class LiveTargetManager final : public interfaces::ITargetPredictor { From ba1f6fb1d38dc443097d96b3ce74b4b0f7b67e18 Mon Sep 17 00:00:00 2001 From: alray <1780284652@qq.com> Date: Fri, 3 Oct 2025 03:48:53 +0800 Subject: [PATCH 33/53] =?UTF-8?q?[Add]=20=E6=B7=BB=E5=8A=A0Tracker?= =?UTF-8?q?=E5=AE=9E=E7=8E=B0?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/tongji/fire_controller/tracker.hpp | 46 ++++++++++++++++--- src/tongji/identifier/identified_armor.hpp | 26 +++++++++++ .../predictor/target_snapshot_manager.cpp | 14 ++++++ .../predictor/target_snapshot_manager.hpp | 4 ++ 4 files changed, 83 insertions(+), 7 deletions(-) diff --git a/src/tongji/fire_controller/tracker.hpp b/src/tongji/fire_controller/tracker.hpp index f2314be..29091bb 100644 --- a/src/tongji/fire_controller/tracker.hpp +++ b/src/tongji/fire_controller/tracker.hpp @@ -2,21 +2,53 @@ #include "../predictor/target_snapshot.hpp" #include "../predictor/target_snapshot_manager.hpp" +#include "enum/enum_tools.hpp" +#include "interfaces/car_state.hpp" +#include "tongji/identifier/identified_armor.hpp" +#include +#include -class DefaultTracker { +class DefaultTracker final { using TargetSnapshotManager = world_exe::tongji::predictor::TargetSnapshotManager; using TargetSnapshot = world_exe::tongji::predictor::TargetSnapshot; + using ArmorInImage = world_exe::tongji::identifier::IdentifiedArmor; + using EnemiesState = world_exe::interfaces::ICarState; public: - DefaultTracker(const std::shared_ptr& snapshot_manager) - : snapshot_manager_(snapshot_manager) {}; + DefaultTracker() = default; ~DefaultTracker() = default; - auto CalculateTarget() noexcept -> TargetSnapshot { - // tongji shooter::tracker impl here + auto CalculateTarget( // + ArmorInImage& armors, // + const std::shared_ptr& snapshot_manager_ // + ) noexcept -> std::unique_ptr { + + const auto& sq_armor_list = armors.get_sq_armor(); + + sq_armor_list->sort( + [](const std::tuple& a, + const std::tuple& b) { + cv::Point2f img_center(1440.0 / 2, 1080.0 / 2); // TODO + + auto const& [a_, a_center] = a; + auto const& [b_, b_center] = b; + + auto distance_1 = cv::norm(a_center - img_center); + auto distance_2 = cv::norm(b_center - img_center); + return distance_1 < distance_2; + }); + sq_armor_list->sort([](const auto& a, const auto& b) { return a.priority < b.priority; }); + + auto iterator = sq_armor_list->begin(); + auto filter_flag = snapshot_manager_->GetId(); + + while (!world_exe::enumeration::IsFlagContains(filter_flag, iterator->armor.id)) { + ++iterator; + if (iterator == sq_armor_list->end()) return nullptr; + } + return snapshot_manager_->GetSingleSnapshot(iterator->armor.id); } private: - const std::shared_ptr snapshot_manager_; -}; \ No newline at end of file +}; diff --git a/src/tongji/identifier/identified_armor.hpp b/src/tongji/identifier/identified_armor.hpp index b6ac9ff..d7b3d59 100644 --- a/src/tongji/identifier/identified_armor.hpp +++ b/src/tongji/identifier/identified_armor.hpp @@ -4,8 +4,17 @@ #include "interfaces/armor_in_image.hpp" #include "interfaces/time_stamped.hpp" #include "util/index.hpp" +#include +#include namespace world_exe::tongji::identifier { + +struct SPArmor { + const data::ArmorImageSpacing& armor; + const cv::Point2f center; + const int priority = 0; +}; + class IdentifiedArmor final : public interfaces::IArmorInImage, public interfaces::ITimeStamped { public: explicit IdentifiedArmor(const std::vector& armors) { @@ -23,8 +32,25 @@ class IdentifiedArmor final : public interfaces::IArmorInImage, public interface return armors_[util::enumeration::GetIndex(armor_id)]; } + std::shared_ptr> get_sq_armor() { + if (armor_list != nullptr) return armor_list; + + armor_list = std::make_shared>(); + for (const auto& car : armors_) + for (const auto& armor : car) + armor_list->emplace_back( + armor, (armor.image_points[0] + armor.image_points[3]) / 2, 0); + + return armor_list; + } + + static IdentifiedArmor DecorateIArmorInImage(const interfaces::IArmorInImage& armor) { + throw std::runtime_error("Not implemented"); + } + private: std::time_t time_stamp_ { 0 }; std::array, 8> armors_; + std::shared_ptr> armor_list = nullptr; }; } \ No newline at end of file diff --git a/src/tongji/predictor/target_snapshot_manager.cpp b/src/tongji/predictor/target_snapshot_manager.cpp index a660c9b..64b5fa8 100644 --- a/src/tongji/predictor/target_snapshot_manager.cpp +++ b/src/tongji/predictor/target_snapshot_manager.cpp @@ -32,6 +32,15 @@ class TargetSnapshotManager::Impl { return std::make_shared(result, time_stamp); } + const std::unique_ptr GetSingleSnapshot( + const enumeration::ArmorIdFlag& id) const { + auto it = snapshots_.find(id); + if (it != snapshots_.end()) { + return std::make_unique(it->second); + } + return nullptr; + } + private: std::unordered_map snapshots_; enumeration::ArmorIdFlag id_; @@ -44,6 +53,11 @@ TargetSnapshotManager::~TargetSnapshotManager() = default; const enumeration ::ArmorIdFlag& TargetSnapshotManager::GetId() const { return pimpl_->GetId(); } +const std::unique_ptr TargetSnapshotManager::GetSingleSnapshot( + const enumeration::ArmorIdFlag& id) const { + return pimpl_->GetSingleSnapshot(id); +} + std ::shared_ptr TargetSnapshotManager::Predictor( const std ::time_t& time_stamp) const { return pimpl_->Predictor(time_stamp); diff --git a/src/tongji/predictor/target_snapshot_manager.hpp b/src/tongji/predictor/target_snapshot_manager.hpp index 02394fd..1353394 100644 --- a/src/tongji/predictor/target_snapshot_manager.hpp +++ b/src/tongji/predictor/target_snapshot_manager.hpp @@ -6,6 +6,7 @@ #include "enum/armor_id.hpp" #include "interfaces/predictor.hpp" #include "tongji/predictor/live_target.hpp" +#include "tongji/predictor/target_snapshot.hpp" namespace world_exe::tongji::predictor { @@ -17,6 +18,9 @@ class TargetSnapshotManager final : public interfaces::IPredictor { const enumeration ::ArmorIdFlag& GetId() const override; + const std::unique_ptr GetSingleSnapshot( + const enumeration::ArmorIdFlag& id) const; + std ::shared_ptr Predictor( const std ::time_t& time_stamp) const override; From 3e0fe3d491c78b9fdda824700cb807bf2741b25b Mon Sep 17 00:00:00 2001 From: heyeuu <2829004293@qq.com> Date: Fri, 3 Oct 2025 04:34:53 +0800 Subject: [PATCH 34/53] fix(tracker): fix data structure in tracker.hpp --- src/tongji/fire_controller/tracker.hpp | 19 +++++++------------ 1 file changed, 7 insertions(+), 12 deletions(-) diff --git a/src/tongji/fire_controller/tracker.hpp b/src/tongji/fire_controller/tracker.hpp index 29091bb..050593f 100644 --- a/src/tongji/fire_controller/tracker.hpp +++ b/src/tongji/fire_controller/tracker.hpp @@ -26,18 +26,13 @@ class DefaultTracker final { const auto& sq_armor_list = armors.get_sq_armor(); - sq_armor_list->sort( - [](const std::tuple& a, - const std::tuple& b) { - cv::Point2f img_center(1440.0 / 2, 1080.0 / 2); // TODO - - auto const& [a_, a_center] = a; - auto const& [b_, b_center] = b; - - auto distance_1 = cv::norm(a_center - img_center); - auto distance_2 = cv::norm(b_center - img_center); - return distance_1 < distance_2; - }); + sq_armor_list->sort([](const world_exe::tongji::identifier::SPArmor& a, + const world_exe::tongji::identifier::SPArmor& b) { + cv::Point2f img_center(1440.0 / 2, 1080.0 / 2); // TODO + auto distance_1 = cv::norm(a.center - img_center); + auto distance_2 = cv::norm(b.center - img_center); + return distance_1 < distance_2; + }); sq_armor_list->sort([](const auto& a, const auto& b) { return a.priority < b.priority; }); auto iterator = sq_armor_list->begin(); From 64e372a38eb15e2bf27422c138fca809038f2bb9 Mon Sep 17 00:00:00 2001 From: heyeuu <2829004293@qq.com> Date: Fri, 3 Oct 2025 04:35:28 +0800 Subject: [PATCH 35/53] fix(tracker): fix data structure in tracker.hpp --- src/tongji/fire_controller/tracker.hpp | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/src/tongji/fire_controller/tracker.hpp b/src/tongji/fire_controller/tracker.hpp index 050593f..e5ffdfb 100644 --- a/src/tongji/fire_controller/tracker.hpp +++ b/src/tongji/fire_controller/tracker.hpp @@ -1,12 +1,14 @@ #pragma once +#include + +#include + #include "../predictor/target_snapshot.hpp" #include "../predictor/target_snapshot_manager.hpp" #include "enum/enum_tools.hpp" #include "interfaces/car_state.hpp" #include "tongji/identifier/identified_armor.hpp" -#include -#include class DefaultTracker final { using TargetSnapshotManager = world_exe::tongji::predictor::TargetSnapshotManager; From 44a79ff965e86e3d192cd780c0c86c07d21bfd92 Mon Sep 17 00:00:00 2001 From: heyeuu <2829004293@qq.com> Date: Sat, 4 Oct 2025 03:06:25 +0800 Subject: [PATCH 36/53] feat(fire_controller): add aim point selection and trajectory classes --- .../fire_controller/aim_point_chooser.hpp | 87 +++++++++++++++++++ .../fire_controller/fire_control_solver.hpp | 87 +++++++++++++++++++ src/tongji/fire_controller/trajectory.hpp | 20 +++-- src/tongji/predictor/live_target.hpp | 82 +---------------- src/tongji/predictor/live_target_manager.cpp | 14 +-- src/tongji/predictor/target_snapshot.hpp | 22 ++++- 6 files changed, 217 insertions(+), 95 deletions(-) create mode 100644 src/tongji/fire_controller/aim_point_chooser.hpp create mode 100644 src/tongji/fire_controller/fire_control_solver.hpp diff --git a/src/tongji/fire_controller/aim_point_chooser.hpp b/src/tongji/fire_controller/aim_point_chooser.hpp new file mode 100644 index 0000000..4922113 --- /dev/null +++ b/src/tongji/fire_controller/aim_point_chooser.hpp @@ -0,0 +1,87 @@ +#pragma once + +#include "enum/car_id.hpp" +#include "util/math.hpp" + +namespace world_exe::tongji::fire_control { + +class AimPointChooser { +public: + AimPointChooser( + const double& comming_angle = 60 / 57.3, const double& leaving_angle = 20 / 57.3) + : comming_angle_(comming_angle) + , leaving_angle_(leaving_angle) { } + + std::pair ChooseAimArmor(const Eigen::Vector& ekf_x, + const std::vector& xyza_list, const enumeration::CarIDFlag& single_id) { + const auto armor_num = xyza_list.size(); + int chosen_id = -1; + + // 整车旋转中心的球坐标yaw + const auto center_yaw = std::atan2(ekf_x[2], ekf_x[0]); + + // 如果delta_angle为0，则该装甲板中心和整车中心的连线在世界坐标系的xy平面过原点 + std::vector delta_angle_list; + for (int i = 0; i < armor_num; i++) { + auto delta_angle = util::math::clamp_pm_pi(xyza_list[i][3] - center_yaw); + delta_angle_list.emplace_back(delta_angle); + } + + // 不考虑小陀螺 + if (std::abs(ekf_x[8]) <= 2 && single_id != enumeration::CarIDFlag::Outpost) { + // 选择在可射击范围内的装甲板 + std::vector id_list; + for (int i = 0; i < armor_num; i++) { + if (std::abs(delta_angle_list[i]) > 60 / 57.3) continue; + id_list.push_back(i); + } + + if (id_list.size() == 1) { + chosen_id = id_list[0]; + lock_id_ = -1; + } else if (id_list.size() > 1) { + const int id0 = id_list[0], id1 = id_list[1]; + // 未处于锁定模式时，选择delta_angle绝对值较小的装甲板，进入锁定模式 + if (lock_id_ != id0 && lock_id_ != id1) + lock_id_ = (std::abs(delta_angle_list[id0]) < std::abs(delta_angle_list[id1])) + ? id0 + : id1; + chosen_id = lock_id_; + } else { + chosen_id = -1; + } + } else { + // 小陀螺 + double coming_angle = + (single_id == enumeration::CarIDFlag::Outpost) ? 70 / 57.3 : comming_angle_; + double leaving_angle = + (single_id == enumeration::CarIDFlag::Outpost) ? 30 / 57.3 : leaving_angle_; + + // 在小陀螺时，一侧的装甲板不断出现，另一侧的装甲板不断消失，显然前者被打中的概率更高 + for (int i = 0; i < armor_num; i++) { + if (std::abs(delta_angle_list[i]) > coming_angle) continue; + if ((ekf_x[7] > 0 && delta_angle_list[i] < leaving_angle) + || (ekf_x[7] < 0 && delta_angle_list[i] > -leaving_angle)) { + chosen_id = i; + break; + } + } + } + + if (chosen_id == -1) { + return { false, xyza_list[0] }; + } + + return { + true, + xyza_list[chosen_id], + }; + } + +private: + double comming_angle_ = 60 / 57.3; + double leaving_angle_ = 20 / 57.3; + int lock_id_ = -1; +}; + +} diff --git a/src/tongji/fire_controller/fire_control_solver.hpp b/src/tongji/fire_controller/fire_control_solver.hpp new file mode 100644 index 0000000..dfec413 --- /dev/null +++ b/src/tongji/fire_controller/fire_control_solver.hpp @@ -0,0 +1,87 @@ +#pragma once + +#include +#include +#include +#include + +#include "tongji/fire_controller/aim_point_chooser.hpp" +#include "tongji/fire_controller/trajectory.hpp" +#include "tongji/predictor/target_snapshot.hpp" + +namespace world_exe::tongji::fire_control { + +struct ControlCommand { + bool solvable; + double yaw; + double pitch; + double horizon_distance = 0; // 无人机专有 +}; + +class FireControlSolver { +public: + FireControlSolver(std::unique_ptr snapshot, + const double& bullet_speed, const double& yaw_offset, const double& pitch_offset, + const double& gravity = 9.7833) + : snapshot_(std::move(snapshot)) + , aim_point_chooser_(std::make_unique()) + , bullet_speed_(bullet_speed) + , g_(gravity) { } + + ControlCommand GenerateCommand() { + const auto& [valid0, aim_point0] = aim_point_chooser_->ChooseAimArmor( + snapshot_->GetEkfX(), snapshot_->GetXYZAList(0), snapshot_->GetID()); + if (!valid0) return { false, 0, 0 }; + + const auto& xyz0 = aim_point0.head(3); + const auto& d0 = std::sqrt(xyz0.x() * xyz0.x() + xyz0.y() * xyz0.y()); + const auto trajectory0 = TrajectorySolver::SolveTrajectory(bullet_speed_, d0, xyz0[2], g_); + + // 迭代求解飞行时间 (最多10次，收敛条件：相邻两次fly_time差 <0.001) + bool converged = false; + double prev_fly_time = trajectory0.fly_time; + + // 预测目标在 future + prev_fly_time 时刻的位置 + Eigen::Vector4d final_aim_point; + auto current_trajectory = trajectory0; + + for (int i = 0; i < 10; ++i) { + auto dt = prev_fly_time; + const auto& [valid, aim_point] = aim_point_chooser_->ChooseAimArmor( + snapshot_->Predict(dt), snapshot_->GetXYZAList(dt), snapshot_->GetID()); + + if (!valid) return { false, 0., 0. }; + + const auto& xyz = aim_point.head(3); + const auto& d = std::sqrt(xyz.x() * xyz.x() + xyz.y() * xyz.y()); + current_trajectory = TrajectorySolver::SolveTrajectory(bullet_speed_, d, xyz.z(), g_); + + if (!current_trajectory.solvable) return { false, 0., 0. }; + + // 检查收敛条件 + if (std::abs(current_trajectory.fly_time - prev_fly_time) < 0.001) { + converged = true; + final_aim_point = aim_point; + break; + } + prev_fly_time = current_trajectory.fly_time; + } + + if (!converged) return { false, 0., 0. }; + + // 计算最终角度 + Eigen::Vector3d final_xyz = final_aim_point.head(3); + const double yaw = std::atan2(final_xyz.y(), final_xyz.x()) + yaw_offset_; + const double pitch = + -(current_trajectory.pitch + pitch_offset_); // 世界坐标系下pitch向上为负 + return { true, yaw, pitch }; + } + +private: + double bullet_speed_; + double yaw_offset_, pitch_offset_; + const double g_; + std::unique_ptr snapshot_; + std::unique_ptr aim_point_chooser_; +}; +} \ No newline at end of file diff --git a/src/tongji/fire_controller/trajectory.hpp b/src/tongji/fire_controller/trajectory.hpp index 3799c2c..b5d9586 100644 --- a/src/tongji/fire_controller/trajectory.hpp +++ b/src/tongji/fire_controller/trajectory.hpp @@ -3,39 +3,41 @@ #include namespace world_exe::tongji::fire_control { -struct TrajectorySolver { +struct TrajectoryResult { bool solvable = true; double fly_time; double pitch; // 抬头为正 +}; +struct TrajectorySolver { // 不考虑空气阻力 // v0 子弹初速度大小，单位：m/s // d 目标水平距离，单位：m // h 目标竖直高度，单位：m // g 重力加速度，单位：m/s^2 //(g·x²)/(2v₀²)·u² - x·u + (g·x²)/(2v₀²) + y = 0(其中u = tan(θ)) - TrajectorySolver(const double& v0, const double& d, const double& h, const double& g) { + static auto SolveTrajectory(const double& v0, const double& d, const double& h, const double& g) + -> TrajectoryResult const { auto a = g * d * d / (2 * v0 * v0); auto b = -d; auto c = a + h; auto delta = b * b - 4 * a * c; if (delta < 0) { - solvable = false; - return; + return { .solvable = false, .fly_time = 0, .pitch = 0 }; } - solvable = true; - auto tan_pitch_1 = (-b + std::sqrt(delta)) / (2 * a); auto tan_pitch_2 = (-b - std::sqrt(delta)) / (2 * a); auto pitch_1 = std::atan(tan_pitch_1); auto pitch_2 = std::atan(tan_pitch_2); auto t_1 = d / (v0 * std::cos(pitch_1)); auto t_2 = d / (v0 * std::cos(pitch_2)); - - pitch = (t_1 < t_2) ? pitch_1 : pitch_2; - fly_time = (t_1 < t_2) ? t_1 : t_2; + return { + .solvable = true, + .fly_time = (t_1 < t_2) ? t_1 : t_2, + .pitch = (t_1 < t_2) ? pitch_1 : pitch_2, + }; } }; diff --git a/src/tongji/predictor/live_target.hpp b/src/tongji/predictor/live_target.hpp index c9fece5..cff0951 100644 --- a/src/tongji/predictor/live_target.hpp +++ b/src/tongji/predictor/live_target.hpp @@ -4,13 +4,10 @@ #include #include #include -#include -#include "data/armor_gimbal_control_spacing.hpp" #include "enum/car_id.hpp" #include "predict_model.hpp" #include "util/extended_kalman_filter.hpp" -#include "util/math.hpp" namespace world_exe::tongji::predictor { @@ -34,7 +31,7 @@ class LiveTarget { LiveTarget(const Eigen::Vector3d& armor_xyz_in_world, const Eigen::Vector3d& armor_ypr_in_world, const enumeration::CarIDFlag& car_id, const std::time_t& t) - : last_time_stamp_(t) + : time_stamp_(t) , model_(car_id) { // x vx y vy z vz a w r l h @@ -60,77 +57,7 @@ class LiveTarget { Eigen::VectorXd GetEkfX() const { return ekf_.x; } Eigen::VectorXd GetP0Dig() const { return model_.GetP0Dig(); } const PredictModel& GetModel() const { return model_; } - std::time_t GetTimeStamp() const { return last_time_stamp_; } - - data::ArmorGimbalControlSpacing GetTargetArmorGimbalControlSpacings() { - const auto& ekf_x = GetEkfX(); - const auto& xyza_list = model_.GetArmorXYZAList(ekf_x); - - Eigen::Vector4d xyza; - data::ArmorGimbalControlSpacing armor; - - if (!status_.jumped) { - // 如果装甲板未发生过跳变，则只有当前装甲板的位置已知 - xyza = xyza_list.at(0); - } - - // 整车旋转中心的球坐标yaw - auto center_yaw = std::atan2(ekf_x[2], ekf_x[0]); - std::vector delta_angle_list; - for (int i = 0; i < model_.GetArmorNum(); i++) { - auto delta_angle = util::math::clamp_pm_pi(xyza_list[i][3] - center_yaw); - delta_angle_list.emplace_back(delta_angle); - } - - // 不考虑小陀螺 - if (std::abs(ekf_x[8]) <= 2 && model_.GetID() != enumeration::CarIDFlag::Outpost) { - // 选择在可射击范围内的装甲板 - std::vector id_list; - for (int i = 0; i < model_.GetArmorNum(); i++) { - if (std::abs(delta_angle_list[i]) > 60 / 57.3) continue; - id_list.push_back(i); - } - - // 锁定模式：防止在两个都呈45度的装甲板之间来回切换 - if (id_list.size() > 1) { - int id0 = id_list[0], id1 = id_list[1]; - - // 未处于锁定模式时，选择delta_angle绝对值较小的装甲板，进入锁定模式 - if (status_.lock_id_ != id0 && status_.lock_id_ != id1) - status_.lock_id_ = - (std::abs(delta_angle_list[id0]) < std::abs(delta_angle_list[id1])) ? id0 - : id1; - xyza = xyza_list.at(status_.lock_id_); - } - - // 只有一个装甲板在可射击范围内时，退出锁定模式 - status_.lock_id_ = -1; - xyza = xyza_list.at(id_list[0]); - } - - double coming_angle, leaving_angle; - if (model_.GetID() == enumeration::CarIDFlag::Outpost) { - coming_angle = 70 / 57.3; - leaving_angle = 30 / 57.3; - } else { - coming_angle = comming_angle_; - leaving_angle = leaving_angle_; - } - - // 在小陀螺时，一侧的装甲板不断出现，另一侧的装甲板不断消失，显然前者被打中的概率更高 - for (int i = 0; i < model_.GetArmorNum(); i++) { - if (std::abs(delta_angle_list[i]) > coming_angle) continue; - if (ekf_x[7] > 0 && delta_angle_list[i] < leaving_angle) xyza = xyza_list[i]; - else if (ekf_x[7] < 0 && delta_angle_list[i] > -leaving_angle) xyza = xyza_list[i]; - } - - armor.id = model_.GetID(); - armor.position = xyza.head<3>(); - const auto& angle = xyza[3]; - armor.orientation = util::math::euler_to_quaternion(angle, 15. / 180. * CV_PI, 0); - - return armor; - } + std::time_t GetTimeStamp() const { return time_stamp_; } void Update(const double& dt, const Eigen::Vector3d& armor_xyz_in_world, const Eigen::Vector3d& armor_ypr_in_world, const Eigen::Vector3d& armor_ypd_in_world) { @@ -198,11 +125,8 @@ class LiveTarget { this->ekf_.x[7] = this->ekf_.x[7] > 0 ? 2.51 : -2.51; } - std::time_t last_time_stamp_; + std::time_t time_stamp_; util::ExtendedKalmanFilter ekf_; PredictModel model_; - - double comming_angle_ = 60 / 57.3; - double leaving_angle_ = 20 / 57.3; }; } \ No newline at end of file diff --git a/src/tongji/predictor/live_target_manager.cpp b/src/tongji/predictor/live_target_manager.cpp index 8d662af..f2e1972 100644 --- a/src/tongji/predictor/live_target_manager.cpp +++ b/src/tongji/predictor/live_target_manager.cpp @@ -29,12 +29,14 @@ class LiveTargetManager::Impl { std::unordered_map> result; - for (auto id : util::enumeration::ExpandArmorIdFlags(flag)) { - auto it = targets_.find(id); - if (it != targets_.end() && it->second) { - result[id].emplace_back(it->second->GetTargetArmorGimbalControlSpacings()); - } - } + // TODO + + // for (auto id : util::enumeration::ExpandArmorIdFlags(flag)) { + // auto it = targets_.find(id); + // if (it != targets_.end() && it->second) { + // result[id].emplace_back(it->second->GetArmorGimbalControlSpacings()); + // } + // } return std::make_shared(result, time_stamp); } diff --git a/src/tongji/predictor/target_snapshot.hpp b/src/tongji/predictor/target_snapshot.hpp index 0dc4a61..a38c705 100644 --- a/src/tongji/predictor/target_snapshot.hpp +++ b/src/tongji/predictor/target_snapshot.hpp @@ -1,5 +1,8 @@ #pragma once +#include + +#include "data/armor_gimbal_control_spacing.hpp" #include "tongji/predictor/live_target.hpp" #include "tongji/predictor/predict_model.hpp" #include "util/extended_kalman_filter.hpp" @@ -10,7 +13,8 @@ class TargetSnapshot { public: TargetSnapshot(const LiveTarget& target) : model_(target.GetModel()) - , ekf_(target.GetEkfX(), target.GetP0Dig().asDiagonal(), model_.x_add) { } + , ekf_(target.GetEkfX(), target.GetP0Dig().asDiagonal(), model_.x_add) + , time_stamp_(target.GetTimeStamp()) { } std::vector GetArmorGimbalControlSpacings() const { std::vector armors; @@ -28,9 +32,25 @@ class TargetSnapshot { return armors; } + auto GetXYZAList(const double& dt) -> std::vector const { + return model_.GetArmorXYZAList(this->Predict(dt)); + } + + auto GetTimeStamp() const { return time_stamp_; } + + auto GetID() const { return model_.GetID(); } + + auto GetEkfX() const { return ekf_.x; } + + auto Predict(const double& dt) -> Eigen::Vector const { + auto predicted_x = model_.f(ekf_.x, dt); + return predicted_x; + } + private: PredictModel model_; util::ExtendedKalmanFilter ekf_; + const std::time_t time_stamp_; }; } \ No newline at end of file From 2dc6cd82b37ba01d9c1b89d10577a43f8e9ca890 Mon Sep 17 00:00:00 2001 From: heyeuu <2829004293@qq.com> Date: Sat, 4 Oct 2025 05:17:37 +0800 Subject: [PATCH 37/53] feat(fire_control): add fire command validation class fire_judge --- .../fire_controller/aim_point_chooser.hpp | 4 +- src/tongji/fire_controller/fire_judge.hpp | 49 +++++++++++++++++++ src/tongji/fire_controller/firecontrol.hpp | 14 ------ .../fire_controller/firecontrol_impl.cpp | 12 ----- ...rol_solver.hpp => gimbal_angle_solver.hpp} | 26 +++++----- 5 files changed, 66 insertions(+), 39 deletions(-) create mode 100644 src/tongji/fire_controller/fire_judge.hpp delete mode 100644 src/tongji/fire_controller/firecontrol.hpp delete mode 100644 src/tongji/fire_controller/firecontrol_impl.cpp rename src/tongji/fire_controller/{fire_control_solver.hpp => gimbal_angle_solver.hpp} (84%) diff --git a/src/tongji/fire_controller/aim_point_chooser.hpp b/src/tongji/fire_controller/aim_point_chooser.hpp index 4922113..b457a76 100644 --- a/src/tongji/fire_controller/aim_point_chooser.hpp +++ b/src/tongji/fire_controller/aim_point_chooser.hpp @@ -79,8 +79,8 @@ class AimPointChooser { } private: - double comming_angle_ = 60 / 57.3; - double leaving_angle_ = 20 / 57.3; + double comming_angle_ = 60 / 57.3; // degree + double leaving_angle_ = 20 / 57.3; // degree int lock_id_ = -1; }; diff --git a/src/tongji/fire_controller/fire_judge.hpp b/src/tongji/fire_controller/fire_judge.hpp new file mode 100644 index 0000000..9776c5f --- /dev/null +++ b/src/tongji/fire_controller/fire_judge.hpp @@ -0,0 +1,49 @@ +#pragma once + +#include +#include + +#include "tongji/fire_controller/gimbal_angle_solver.hpp" + +namespace world_exe::tongji::fire_control { + +class FireJudge { +public: + explicit FireJudge(const bool& auto_fire, const double& first_tolerance = 5, + const double& second_tolerance = 2, const double& judge_distance = 3) + : auto_fire_(auto_fire) + , last_command_ { false, 0, 0 } + , first_tolerance_(first_tolerance) + , second_tolerance_(second_tolerance) + , judge_distance_(judge_distance) { } + + bool ShouldFire(const GimbalCommand& command, const Eigen::Vector3d& valid_target_pos, + const Eigen::Vector3d& gimbal_pos) { + + if (!command.valid || !auto_fire_) return false; + const auto& tolerance = std::sqrt(valid_target_pos.x() * valid_target_pos.x() + + valid_target_pos.y() * valid_target_pos.y()) + > judge_distance_ + ? second_tolerance_ + : first_tolerance_; + + if (std::abs(last_command_.yaw - command.yaw) + < tolerance * 2 // 此时认为command突变不应该射击 + && std::abs(gimbal_pos[0] - last_command_.yaw) < tolerance && command.valid) { + last_command_ = command; + return true; + } + last_command_ = command; + return false; + } + +private: + bool auto_fire_; + GimbalCommand last_command_; + + double first_tolerance_ { 5 }; // 近距离射击容差，degree + double second_tolerance_ { 2 }; // 远距离射击容差，degree + double judge_distance_ { 3 }; // 距离判断阈值 +}; + +} \ No newline at end of file diff --git a/src/tongji/fire_controller/firecontrol.hpp b/src/tongji/fire_controller/firecontrol.hpp deleted file mode 100644 index cb81d96..0000000 --- a/src/tongji/fire_controller/firecontrol.hpp +++ /dev/null @@ -1,14 +0,0 @@ -#pragma once - -#include "interfaces/fire_controller.hpp" -namespace world_exe::tongji::fire_controller { -class FireControl final : interfaces::IFireControl { - - const data::FireControl CalculateTarget(const std::time_t& time_duration) const override; - - const enumeration::CarIDFlag GetAttackCarId() const override; - - struct Impl; - std::unique_ptr pimpl_; -}; -} diff --git a/src/tongji/fire_controller/firecontrol_impl.cpp b/src/tongji/fire_controller/firecontrol_impl.cpp deleted file mode 100644 index 43c55f4..0000000 --- a/src/tongji/fire_controller/firecontrol_impl.cpp +++ /dev/null @@ -1,12 +0,0 @@ - -#include "./firecontrol.hpp" -#include "tongji/fire_controller/tracker.hpp" - -using namespace world_exe::tongji::fire_controller; -class FireControl::Impl { - // 后续转依赖注入 -public: - explicit Impl(DefaultTracker tracker) { } - - // impl interface here -}; diff --git a/src/tongji/fire_controller/fire_control_solver.hpp b/src/tongji/fire_controller/gimbal_angle_solver.hpp similarity index 84% rename from src/tongji/fire_controller/fire_control_solver.hpp rename to src/tongji/fire_controller/gimbal_angle_solver.hpp index dfec413..d8bca8f 100644 --- a/src/tongji/fire_controller/fire_control_solver.hpp +++ b/src/tongji/fire_controller/gimbal_angle_solver.hpp @@ -11,16 +11,16 @@ namespace world_exe::tongji::fire_control { -struct ControlCommand { - bool solvable; +struct GimbalCommand { + bool valid; double yaw; double pitch; double horizon_distance = 0; // 无人机专有 }; -class FireControlSolver { +class GimbleAngleSolver { public: - FireControlSolver(std::unique_ptr snapshot, + GimbleAngleSolver(std::unique_ptr snapshot, const double& bullet_speed, const double& yaw_offset, const double& pitch_offset, const double& gravity = 9.7833) : snapshot_(std::move(snapshot)) @@ -28,7 +28,7 @@ class FireControlSolver { , bullet_speed_(bullet_speed) , g_(gravity) { } - ControlCommand GenerateCommand() { + GimbalCommand Solve() { const auto& [valid0, aim_point0] = aim_point_chooser_->ChooseAimArmor( snapshot_->GetEkfX(), snapshot_->GetXYZAList(0), snapshot_->GetID()); if (!valid0) return { false, 0, 0 }; @@ -42,7 +42,7 @@ class FireControlSolver { double prev_fly_time = trajectory0.fly_time; // 预测目标在 future + prev_fly_time 时刻的位置 - Eigen::Vector4d final_aim_point; + auto current_trajectory = trajectory0; for (int i = 0; i < 10; ++i) { @@ -60,8 +60,8 @@ class FireControlSolver { // 检查收敛条件 if (std::abs(current_trajectory.fly_time - prev_fly_time) < 0.001) { - converged = true; - final_aim_point = aim_point; + converged = true; + final_aim_point_ = aim_point; break; } prev_fly_time = current_trajectory.fly_time; @@ -70,17 +70,21 @@ class FireControlSolver { if (!converged) return { false, 0., 0. }; // 计算最终角度 - Eigen::Vector3d final_xyz = final_aim_point.head(3); + Eigen::Vector3d final_xyz = final_aim_point_.head(3); const double yaw = std::atan2(final_xyz.y(), final_xyz.x()) + yaw_offset_; const double pitch = -(current_trajectory.pitch + pitch_offset_); // 世界坐标系下pitch向上为负 return { true, yaw, pitch }; } + auto GetAimPoint() -> Eigen::Vector4d const { return final_aim_point_; } + private: - double bullet_speed_; - double yaw_offset_, pitch_offset_; + double bullet_speed_; // m/s + double yaw_offset_, pitch_offset_; // degree const double g_; + Eigen::Vector4d final_aim_point_; + std::unique_ptr snapshot_; std::unique_ptr aim_point_chooser_; }; From c44607aa7ad81341a6caa61589049326f3687076 Mon Sep 17 00:00:00 2001 From: heyeuu <2829004293@qq.com> Date: Sun, 5 Oct 2025 03:50:54 +0800 Subject: [PATCH 38/53] refactor(control): rename classes and refine fire logic MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit - fire_judge → fire_decision - gimble_angle_solver → aim_solver - Improve fire validation and decision process --- src/tongji/fire_controller/aim_solver.hpp | 92 +++++++++++++++++++ .../{fire_judge.hpp => fire_decision.hpp} | 22 ++--- .../fire_controller/gimbal_angle_solver.hpp | 91 ------------------ src/tongji/fire_controller/trajectory.hpp | 2 +- src/tongji/predictor/target_snapshot.hpp | 2 +- 5 files changed, 105 insertions(+), 104 deletions(-) create mode 100644 src/tongji/fire_controller/aim_solver.hpp rename src/tongji/fire_controller/{fire_judge.hpp => fire_decision.hpp} (61%) delete mode 100644 src/tongji/fire_controller/gimbal_angle_solver.hpp diff --git a/src/tongji/fire_controller/aim_solver.hpp b/src/tongji/fire_controller/aim_solver.hpp new file mode 100644 index 0000000..d33d529 --- /dev/null +++ b/src/tongji/fire_controller/aim_solver.hpp @@ -0,0 +1,92 @@ +#pragma once + +#include +#include +#include +#include +#include + +#include "tongji/fire_controller/aim_point_chooser.hpp" +#include "tongji/fire_controller/trajectory.hpp" +#include "tongji/predictor/target_snapshot.hpp" + +namespace world_exe::tongji::fire_control { + +struct AimSolution { + bool valid; + double yaw; + double pitch; + Eigen::Vector4d aim_point; // 最终瞄准点（世界坐标 + 装甲板yaw） + double horizon_distance = 0; // 无人机专有 +}; + +class AimingSolver { +public: + AimingSolver(std::unique_ptr snapshot, const double& bullet_speed, + const double& yaw_offset, const double& pitch_offset, const double& gravity = 9.7833) + : snapshot_(std::move(snapshot)) + , aim_point_chooser_(std::make_unique()) + , bullet_speed_(bullet_speed) + , yaw_offset_(yaw_offset / 57.3) // degree to rad + , pitch_offset_(pitch_offset / 57.3) // degree to rad + , g_(gravity) { } + + AimSolution SolveAimSolution(const double& time_delay) { + if (!snapshot_) return { false, 0, 0, {}, 0 }; + + // 迭代求解飞行时间 (最多10次，收敛条件：相邻两次fly_time差 <0.001) + double prev_fly_time = 0; + Eigen::Vector4d final_aim_point; + TrajectoryResult final_trajectory; + bool converged = false; + + // 预测目标在未来 dt时间后的位置 + for (int i = 0; i < 10; ++i) { + double dt = time_delay + prev_fly_time; + const auto aim = SelectPredictedAim(dt); + if (!aim.has_value()) return { false, 0, 0, {}, 0 }; // failed: no valid aim point + + const auto traj = SolveTrajectory(aim->head(3)); + if (!traj.has_value()) return { false, 0, 0, {}, 0 }; // failed: trajectory unsolvable + + if (i > 0 && std::abs(traj->fly_time - prev_fly_time) < 0.001) { + final_aim_point = *aim; + final_trajectory = *traj; + converged = true; + break; + } + prev_fly_time = traj->fly_time; + } + if (!converged) return { false, 0, 0, {}, 0 }; // failed: trajectory did not converge + + const auto xyz = final_aim_point.head(3); + const double yaw = std::atan2(xyz.y(), xyz.x()) + yaw_offset_; + const double pitch = -(final_trajectory.pitch + pitch_offset_); + return { true, yaw, pitch, final_aim_point }; + } + + void UpdateSnapshot(std::unique_ptr snapshot) { + snapshot_ = std::move(snapshot); + } + +private: + std::optional SelectPredictedAim(double dt) const { + const auto& [selectable, aim_point] = aim_point_chooser_->ChooseAimArmor( + snapshot_->Predict(dt), snapshot_->GetPredictedXYZAList(dt), snapshot_->GetID()); + return selectable ? std::optional { aim_point } : std::nullopt; + } + + std::optional SolveTrajectory(const Eigen::Vector3d& xyz) const { + double d = std::hypot(xyz.x(), xyz.y()); + auto result = TrajectorySolver::SolveTrajectory(bullet_speed_, d, xyz.z(), g_); + return result.solvable ? std::optional { result } : std::nullopt; + } + + double bullet_speed_; // m/s + double yaw_offset_, pitch_offset_; + const double g_; + + std::unique_ptr aim_point_chooser_; + std::unique_ptr snapshot_; +}; +} \ No newline at end of file diff --git a/src/tongji/fire_controller/fire_judge.hpp b/src/tongji/fire_controller/fire_decision.hpp similarity index 61% rename from src/tongji/fire_controller/fire_judge.hpp rename to src/tongji/fire_controller/fire_decision.hpp index 9776c5f..8dda984 100644 --- a/src/tongji/fire_controller/fire_judge.hpp +++ b/src/tongji/fire_controller/fire_decision.hpp @@ -3,43 +3,43 @@ #include #include -#include "tongji/fire_controller/gimbal_angle_solver.hpp" +#include "tongji/fire_controller/aim_solver.hpp" namespace world_exe::tongji::fire_control { -class FireJudge { +class FireDecision { public: - explicit FireJudge(const bool& auto_fire, const double& first_tolerance = 5, + explicit FireDecision(const bool& auto_fire, const double& first_tolerance = 5, const double& second_tolerance = 2, const double& judge_distance = 3) : auto_fire_(auto_fire) - , last_command_ { false, 0, 0 } + , last_aim_solution_ { false, 0, 0 } , first_tolerance_(first_tolerance) , second_tolerance_(second_tolerance) , judge_distance_(judge_distance) { } - bool ShouldFire(const GimbalCommand& command, const Eigen::Vector3d& valid_target_pos, + bool ShouldFire(const AimSolution& aimsolution, const Eigen::Vector3d& valid_target_pos, const Eigen::Vector3d& gimbal_pos) { - if (!command.valid || !auto_fire_) return false; + if (!aimsolution.valid || !auto_fire_) return false; const auto& tolerance = std::sqrt(valid_target_pos.x() * valid_target_pos.x() + valid_target_pos.y() * valid_target_pos.y()) > judge_distance_ ? second_tolerance_ : first_tolerance_; - if (std::abs(last_command_.yaw - command.yaw) + if (std::abs(last_aim_solution_.yaw - aimsolution.yaw) < tolerance * 2 // 此时认为command突变不应该射击 - && std::abs(gimbal_pos[0] - last_command_.yaw) < tolerance && command.valid) { - last_command_ = command; + && std::abs(gimbal_pos[0] - last_aim_solution_.yaw) < tolerance && last_aim_solution_.valid) { + last_aim_solution_ = aimsolution; return true; } - last_command_ = command; + last_aim_solution_ = aimsolution; return false; } private: bool auto_fire_; - GimbalCommand last_command_; + AimSolution last_aim_solution_; double first_tolerance_ { 5 }; // 近距离射击容差，degree double second_tolerance_ { 2 }; // 远距离射击容差，degree diff --git a/src/tongji/fire_controller/gimbal_angle_solver.hpp b/src/tongji/fire_controller/gimbal_angle_solver.hpp deleted file mode 100644 index d8bca8f..0000000 --- a/src/tongji/fire_controller/gimbal_angle_solver.hpp +++ /dev/null @@ -1,91 +0,0 @@ -#pragma once - -#include -#include -#include -#include - -#include "tongji/fire_controller/aim_point_chooser.hpp" -#include "tongji/fire_controller/trajectory.hpp" -#include "tongji/predictor/target_snapshot.hpp" - -namespace world_exe::tongji::fire_control { - -struct GimbalCommand { - bool valid; - double yaw; - double pitch; - double horizon_distance = 0; // 无人机专有 -}; - -class GimbleAngleSolver { -public: - GimbleAngleSolver(std::unique_ptr snapshot, - const double& bullet_speed, const double& yaw_offset, const double& pitch_offset, - const double& gravity = 9.7833) - : snapshot_(std::move(snapshot)) - , aim_point_chooser_(std::make_unique()) - , bullet_speed_(bullet_speed) - , g_(gravity) { } - - GimbalCommand Solve() { - const auto& [valid0, aim_point0] = aim_point_chooser_->ChooseAimArmor( - snapshot_->GetEkfX(), snapshot_->GetXYZAList(0), snapshot_->GetID()); - if (!valid0) return { false, 0, 0 }; - - const auto& xyz0 = aim_point0.head(3); - const auto& d0 = std::sqrt(xyz0.x() * xyz0.x() + xyz0.y() * xyz0.y()); - const auto trajectory0 = TrajectorySolver::SolveTrajectory(bullet_speed_, d0, xyz0[2], g_); - - // 迭代求解飞行时间 (最多10次，收敛条件：相邻两次fly_time差 <0.001) - bool converged = false; - double prev_fly_time = trajectory0.fly_time; - - // 预测目标在 future + prev_fly_time 时刻的位置 - - auto current_trajectory = trajectory0; - - for (int i = 0; i < 10; ++i) { - auto dt = prev_fly_time; - const auto& [valid, aim_point] = aim_point_chooser_->ChooseAimArmor( - snapshot_->Predict(dt), snapshot_->GetXYZAList(dt), snapshot_->GetID()); - - if (!valid) return { false, 0., 0. }; - - const auto& xyz = aim_point.head(3); - const auto& d = std::sqrt(xyz.x() * xyz.x() + xyz.y() * xyz.y()); - current_trajectory = TrajectorySolver::SolveTrajectory(bullet_speed_, d, xyz.z(), g_); - - if (!current_trajectory.solvable) return { false, 0., 0. }; - - // 检查收敛条件 - if (std::abs(current_trajectory.fly_time - prev_fly_time) < 0.001) { - converged = true; - final_aim_point_ = aim_point; - break; - } - prev_fly_time = current_trajectory.fly_time; - } - - if (!converged) return { false, 0., 0. }; - - // 计算最终角度 - Eigen::Vector3d final_xyz = final_aim_point_.head(3); - const double yaw = std::atan2(final_xyz.y(), final_xyz.x()) + yaw_offset_; - const double pitch = - -(current_trajectory.pitch + pitch_offset_); // 世界坐标系下pitch向上为负 - return { true, yaw, pitch }; - } - - auto GetAimPoint() -> Eigen::Vector4d const { return final_aim_point_; } - -private: - double bullet_speed_; // m/s - double yaw_offset_, pitch_offset_; // degree - const double g_; - Eigen::Vector4d final_aim_point_; - - std::unique_ptr snapshot_; - std::unique_ptr aim_point_chooser_; -}; -} \ No newline at end of file diff --git a/src/tongji/fire_controller/trajectory.hpp b/src/tongji/fire_controller/trajectory.hpp index b5d9586..2b9c5e1 100644 --- a/src/tongji/fire_controller/trajectory.hpp +++ b/src/tongji/fire_controller/trajectory.hpp @@ -6,7 +6,7 @@ namespace world_exe::tongji::fire_control { struct TrajectoryResult { bool solvable = true; double fly_time; - double pitch; // 抬头为正 + double pitch; // 抬头为正,rad }; struct TrajectorySolver { diff --git a/src/tongji/predictor/target_snapshot.hpp b/src/tongji/predictor/target_snapshot.hpp index a38c705..75a5e96 100644 --- a/src/tongji/predictor/target_snapshot.hpp +++ b/src/tongji/predictor/target_snapshot.hpp @@ -32,7 +32,7 @@ class TargetSnapshot { return armors; } - auto GetXYZAList(const double& dt) -> std::vector const { + auto GetPredictedXYZAList(const double& dt) -> std::vector const { return model_.GetArmorXYZAList(this->Predict(dt)); } From 3aa3c9d5f36b5119a121828a709e77f1e8dc6a26 Mon Sep 17 00:00:00 2001 From: heyeuu <2829004293@qq.com> Date: Sun, 5 Oct 2025 04:41:23 +0800 Subject: [PATCH 39/53] feat(fire_control): implement basic module interfaces --- .../fire_controller/fire_controller.cpp | 78 +++++++++++++++++++ .../fire_controller/fire_controller.hpp | 22 ++++++ src/tongji/fire_controller/tracker.hpp | 9 ++- 3 files changed, 108 insertions(+), 1 deletion(-) create mode 100644 src/tongji/fire_controller/fire_controller.cpp create mode 100644 src/tongji/fire_controller/fire_controller.hpp diff --git a/src/tongji/fire_controller/fire_controller.cpp b/src/tongji/fire_controller/fire_controller.cpp new file mode 100644 index 0000000..b9eba13 --- /dev/null +++ b/src/tongji/fire_controller/fire_controller.cpp @@ -0,0 +1,78 @@ +#include "fire_controller.hpp" + +#include +#include +#include +#include + +#include + +#include "enum/car_id.hpp" +#include "tongji/fire_controller/aim_solver.hpp" +#include "tongji/fire_controller/fire_decision.hpp" +#include "tongji/fire_controller/tracker.hpp" +#include "tongji/identifier/identified_armor.hpp" +#include "tongji/predictor/target_snapshot_manager.hpp" +#include "tongji/predictor/time_stamp.hpp" + +namespace world_exe::tongji::fire_control { +class FireController::Impl { +public: + Impl(const double& control_delay_in_second, const double& bullet_speed) + : control_delay_(control_delay_in_second) + , bullet_speed_(bullet_speed) { } + + // TODO:std::time_t + const data ::FireControl CalculateTarget(const std ::time_t& time_duration) const { + if (!identified_armors_ || !tracker_ || !predictor_ || !aim_solver_ || !fire_decision_) + return { .fire_allowance = false }; + + auto snapshot = tracker_->CalculateTarget(*identified_armors_, predictor_); + if (!snapshot) return { .fire_allowance = false }; + + aim_solver_->UpdateSnapshot(std::move(snapshot)); + const auto& aim_solution = + aim_solver_->SolveAimSolution(static_cast(time_duration)); + if (!aim_solution.valid) return { .fire_allowance = false }; + + const Eigen::Vector3d target_pos = aim_solution.aim_point.head<3>(); + const bool fireable = fire_decision_->ShouldFire(aim_solution, target_pos, gimbal_pos_); + return { .time_stamp = time_stamp_.GetTimeStamp(), + .gimbal_dir = Eigen::Vector3d(aim_solution.yaw, aim_solution.pitch, 0), + .fire_allowance = fireable }; + } + + const enumeration ::CarIDFlag GetAttackCarId() const { + if (!tracker_) return enumeration::CarIDFlag::Unknow; + return tracker_->GetCurrentTargetID(); + } + + void UpdateArmors(const identifier::IdentifiedArmor& armors) { identified_armors_ = armors; } + void UpdateGimbalPosition(const Eigen::Vector3d& gimbal_pos) { gimbal_pos_ = gimbal_pos; }; + void SetTimeStamp(const std::time_t& time_stamp) { time_stamp_.SetTimeStamp(time_stamp); } + +private: + Eigen::Vector3d gimbal_pos_ { Eigen::Vector3d::Zero() }; + double control_delay_; + double bullet_speed_; + + mutable std::optional identified_armors_; + std::unique_ptr tracker_; + std::shared_ptr predictor_; + std::unique_ptr aim_solver_; + std::unique_ptr fire_decision_; + predictor::TimeStamp time_stamp_ { 0 }; +}; + +FireController::FireController() + : pimpl_() { } + +const data ::FireControl FireController::CalculateTarget(const std ::time_t& time_duration) const { + return pimpl_->CalculateTarget(time_duration); +} + +const enumeration ::CarIDFlag FireController::GetAttackCarId() const { + return pimpl_->GetAttackCarId(); +} + +} \ No newline at end of file diff --git a/src/tongji/fire_controller/fire_controller.hpp b/src/tongji/fire_controller/fire_controller.hpp new file mode 100644 index 0000000..8b2e658 --- /dev/null +++ b/src/tongji/fire_controller/fire_controller.hpp @@ -0,0 +1,22 @@ +#pragma once + +#include + +#include "interfaces/fire_controller.hpp" + +namespace world_exe::tongji::fire_control { + +class FireController final : public interfaces::IFireControl { +public: + FireController(); + + const data ::FireControl CalculateTarget(const std ::time_t& time_duration) const override; + + const enumeration ::CarIDFlag GetAttackCarId() const override; + +private: + class Impl; + std::unique_ptr pimpl_; +}; + +} \ No newline at end of file diff --git a/src/tongji/fire_controller/tracker.hpp b/src/tongji/fire_controller/tracker.hpp index e5ffdfb..b065bfd 100644 --- a/src/tongji/fire_controller/tracker.hpp +++ b/src/tongji/fire_controller/tracker.hpp @@ -6,6 +6,7 @@ #include "../predictor/target_snapshot.hpp" #include "../predictor/target_snapshot_manager.hpp" +#include "enum/car_id.hpp" #include "enum/enum_tools.hpp" #include "interfaces/car_state.hpp" #include "tongji/identifier/identified_armor.hpp" @@ -44,8 +45,14 @@ class DefaultTracker final { ++iterator; if (iterator == sq_armor_list->end()) return nullptr; } - return snapshot_manager_->GetSingleSnapshot(iterator->armor.id); + auto snapshot = snapshot_manager_->GetSingleSnapshot(iterator->armor.id); + if (!snapshot) return nullptr; + tracking_car_id_ = snapshot->GetID(); + return snapshot; } + world_exe::enumeration::CarIDFlag GetCurrentTargetID() const { return tracking_car_id_; } + private: + world_exe::enumeration::CarIDFlag tracking_car_id_; }; From 890a40a7f5fffcdd8c21d9366a59f5f672668a43 Mon Sep 17 00:00:00 2001 From: heyeuu <2829004293@qq.com> Date: Mon, 6 Oct 2025 23:58:13 +0800 Subject: [PATCH 40/53] fix(tracking): improve live_target convergence logic --- src/tongji/predictor/live_target.hpp | 72 ++++++++++++++-------------- 1 file changed, 37 insertions(+), 35 deletions(-) diff --git a/src/tongji/predictor/live_target.hpp b/src/tongji/predictor/live_target.hpp index cff0951..3edc46b 100644 --- a/src/tongji/predictor/live_target.hpp +++ b/src/tongji/predictor/live_target.hpp @@ -4,6 +4,7 @@ #include #include #include +#include #include "enum/car_id.hpp" #include "predict_model.hpp" @@ -11,23 +12,23 @@ namespace world_exe::tongji::predictor { -struct TargetStatus { - bool jumped = false; - bool switched = false; - bool converged = false; - bool diverged = false; - bool lost = false; - bool reidentified = false; - int last_id = -1; - double lock_id_ = -1; - int switch_count = 0; - int update_count = 0; - int lost_count = 0; -}; +// struct TargetStatus { +// bool jumped = false; +// bool switched = false; +// bool converged = false; +// bool diverged = false; +// bool lost = false; +// bool reidentified = false; +// int last_id = -1; +// double lock_id_ = -1; +// int switch_count = 0; +// int update_count = 0; +// int lost_count = 0; +// }; class LiveTarget { public: - TargetStatus status_; + // TargetStatus status_; LiveTarget(const Eigen::Vector3d& armor_xyz_in_world, const Eigen::Vector3d& armor_ypr_in_world, const enumeration::CarIDFlag& car_id, const std::time_t& t) @@ -61,35 +62,30 @@ class LiveTarget { void Update(const double& dt, const Eigen::Vector3d& armor_xyz_in_world, const Eigen::Vector3d& armor_ypr_in_world, const Eigen::Vector3d& armor_ypd_in_world) { - // 装甲板匹配 int id = model_.MatchArmor(ekf_.x, armor_xyz_in_world, armor_ypr_in_world, armor_ypd_in_world); - if (id != 0) status_.jumped = true; - status_.switched = (id != status_.last_id); - if (status_.switched) status_.switch_count++; + last_id = id; + update_count++; - status_.last_id = id; Update_ypda(armor_xyz_in_world, armor_ypr_in_world, armor_ypd_in_world, id, dt); - status_.update_count++; } private: - bool Converged() { - if (model_.GetID() != enumeration::CarIDFlag::Outpost && status_.update_count > 3 - && !this->Diverged()) { - status_.converged = true; - } - + bool EvaluateConvergence() { // 前哨站特殊判断 - if (model_.GetID() == enumeration::CarIDFlag::Outpost && status_.update_count > 10 - && !this->Diverged()) { - status_.converged = true; - } - return status_.converged; + const int required_count = (model_.GetID() == enumeration::CarIDFlag::Outpost) ? 10 : 3; + if (update_count < required_count) return false; + if (EvaluateDivergence()) return false; + + auto nis_failures = + std::accumulate(ekf_.recent_nis_failures.begin(), ekf_.recent_nis_failures.end(), 0); + if (nis_failures > 0.4 * ekf_.window_size) return false; + + return true; } - bool Diverged() const { + bool EvaluateDivergence() const { auto r_ok = ekf_.x[8] > 0.05 && ekf_.x[8] < 0.5; auto l_ok = ekf_.x[8] + ekf_.x[9] > 0.05 && ekf_.x[8] + ekf_.x[9] < 0.5; @@ -120,13 +116,19 @@ class LiveTarget { ekf_.Update(dt, A, Q, f, z, H, R, h, z_subtract); // 前哨站转速特判 - if (this->Converged() && model_.GetID() == enumeration::CarIDFlag::Outpost - && std::abs(this->ekf_.x[7]) > 2) - this->ekf_.x[7] = this->ekf_.x[7] > 0 ? 2.51 : -2.51; + // TODO + // if (this->Converged() && model_.GetID() == enumeration::CarIDFlag::Outpost + // && std::abs(this->ekf_.x[7]) > 2) + // this->ekf_.x[7] = this->ekf_.x[7] > 0 ? 2.51 : -2.51; } std::time_t time_stamp_; util::ExtendedKalmanFilter ekf_; PredictModel model_; + + int last_id = -1; + int update_count = 0; + + // bool converged_ { false }; }; } \ No newline at end of file From e599cb4f60738f4bc5c63a27fe821590ae86df55 Mon Sep 17 00:00:00 2001 From: heyeuu <2829004293@qq.com> Date: Tue, 7 Oct 2025 05:13:58 +0800 Subject: [PATCH 41/53] feat: complete core logic across multiple modules - State Machine: add convergence detection logic - LiveTargetManager: implement add/remove operations for LiveTarget - IdentifiedArmor: add GetDetectedIDs() method for ID retrieval - Tracker: complete state update logic implementation - FireControl: add reset function for SnapshotManager --- src/tongji/fire_controller/aim_solver.hpp | 13 +- .../fire_controller/fire_controller.cpp | 43 +++++- .../fire_controller/fire_controller.hpp | 3 + src/tongji/fire_controller/tracker.hpp | 142 ++++++++++++++++-- src/tongji/identifier/identified_armor.hpp | 13 ++ src/tongji/predictor/live_target.hpp | 45 +++--- src/tongji/predictor/live_target_manager.cpp | 43 +++++- src/tongji/predictor/predict_model.hpp | 5 + src/tongji/predictor/target_snapshot.hpp | 8 +- .../state_machine/car_state_manager.hpp | 58 +++---- src/tongji/state_machine/state_machine.cpp | 46 ++++-- src/tongji/state_machine/state_machine.hpp | 5 +- 12 files changed, 320 insertions(+), 104 deletions(-) diff --git a/src/tongji/fire_controller/aim_solver.hpp b/src/tongji/fire_controller/aim_solver.hpp index d33d529..12fec38 100644 --- a/src/tongji/fire_controller/aim_solver.hpp +++ b/src/tongji/fire_controller/aim_solver.hpp @@ -22,15 +22,18 @@ struct AimSolution { class AimingSolver { public: - AimingSolver(std::unique_ptr snapshot, const double& bullet_speed, + AimingSolver( const double& bullet_speed, const double& yaw_offset, const double& pitch_offset, const double& gravity = 9.7833) - : snapshot_(std::move(snapshot)) - , aim_point_chooser_(std::make_unique()) + : aim_point_chooser_(std::make_unique()) , bullet_speed_(bullet_speed) , yaw_offset_(yaw_offset / 57.3) // degree to rad , pitch_offset_(pitch_offset / 57.3) // degree to rad , g_(gravity) { } + void UpdateSnapshot(std::unique_ptr snapshot) { + snapshot_ = std::move(snapshot); + } + AimSolution SolveAimSolution(const double& time_delay) { if (!snapshot_) return { false, 0, 0, {}, 0 }; @@ -65,10 +68,6 @@ class AimingSolver { return { true, yaw, pitch, final_aim_point }; } - void UpdateSnapshot(std::unique_ptr snapshot) { - snapshot_ = std::move(snapshot); - } - private: std::optional SelectPredictedAim(double dt) const { const auto& [selectable, aim_point] = aim_point_chooser_->ChooseAimArmor( diff --git a/src/tongji/fire_controller/fire_controller.cpp b/src/tongji/fire_controller/fire_controller.cpp index b9eba13..114de5d 100644 --- a/src/tongji/fire_controller/fire_controller.cpp +++ b/src/tongji/fire_controller/fire_controller.cpp @@ -14,39 +14,64 @@ #include "tongji/identifier/identified_armor.hpp" #include "tongji/predictor/target_snapshot_manager.hpp" #include "tongji/predictor/time_stamp.hpp" +#include "tongji/state_machine/state_machine.hpp" namespace world_exe::tongji::fire_control { + +using TargetSnapshotManager = predictor::TargetSnapshotManager; + class FireController::Impl { public: - Impl(const double& control_delay_in_second, const double& bullet_speed) + Impl(bool auto_fire, const double& control_delay_in_second, const double& bullet_speed, + double yaw_offset, double pitch_offset) : control_delay_(control_delay_in_second) - , bullet_speed_(bullet_speed) { } + , bullet_speed_(bullet_speed) + , tracker_(std::make_unique()) + , predictor_(nullptr) // TODO + , aim_solver_(std::make_unique(bullet_speed, yaw_offset, pitch_offset)) + , fire_decision_(std::make_unique(auto_fire)) + , state_machine_(std::make_unique()) { } // TODO:std::time_t const data ::FireControl CalculateTarget(const std ::time_t& time_duration) const { if (!identified_armors_ || !tracker_ || !predictor_ || !aim_solver_ || !fire_decision_) return { .fire_allowance = false }; - auto snapshot = tracker_->CalculateTarget(*identified_armors_, predictor_); - if (!snapshot) return { .fire_allowance = false }; + auto snapshot = tracker_->SelectTrackingTarget(*identified_armors_, predictor_); + bool found = (snapshot != nullptr); + tracker_->UpdateState(found); + if (!found) return { .fire_allowance = false }; aim_solver_->UpdateSnapshot(std::move(snapshot)); + const auto& aim_solution = - aim_solver_->SolveAimSolution(static_cast(time_duration)); + aim_solver_->SolveAimSolution(static_cast(time_duration)); // TODO if (!aim_solution.valid) return { .fire_allowance = false }; + state_machine_->Update(identified_armors_->GetDetectedIDs(), time_stamp_.GetTimeStamp()); + + const auto allowed_id = state_machine_->GetAllowdToFires(); // TODO + const auto current_id = tracker_->GetCurrentTargetID(); + const Eigen::Vector3d target_pos = aim_solution.aim_point.head<3>(); - const bool fireable = fire_decision_->ShouldFire(aim_solution, target_pos, gimbal_pos_); + const bool fireable = fire_decision_->ShouldFire(aim_solution, target_pos, gimbal_pos_) + && state_machine_->IsAllowedToFire(current_id); + return { .time_stamp = time_stamp_.GetTimeStamp(), .gimbal_dir = Eigen::Vector3d(aim_solution.yaw, aim_solution.pitch, 0), .fire_allowance = fireable }; } + // TODO const enumeration ::CarIDFlag GetAttackCarId() const { if (!tracker_) return enumeration::CarIDFlag::Unknow; return tracker_->GetCurrentTargetID(); } + void SetSnapshotManager(std::shared_ptr manager) { + predictor_ = std::move(manager); + } + void UpdateArmors(const identifier::IdentifiedArmor& armors) { identified_armors_ = armors; } void UpdateGimbalPosition(const Eigen::Vector3d& gimbal_pos) { gimbal_pos_ = gimbal_pos; }; void SetTimeStamp(const std::time_t& time_stamp) { time_stamp_.SetTimeStamp(time_stamp); } @@ -61,6 +86,8 @@ class FireController::Impl { std::shared_ptr predictor_; std::unique_ptr aim_solver_; std::unique_ptr fire_decision_; + std::unique_ptr state_machine_; + predictor::TimeStamp time_stamp_ { 0 }; }; @@ -71,6 +98,10 @@ const data ::FireControl FireController::CalculateTarget(const std ::time_t& tim return pimpl_->CalculateTarget(time_duration); } +void FireController::SetSnapshotManager(std::shared_ptr manager) { + pimpl_->SetSnapshotManager(std::move(manager)); +} + const enumeration ::CarIDFlag FireController::GetAttackCarId() const { return pimpl_->GetAttackCarId(); } diff --git a/src/tongji/fire_controller/fire_controller.hpp b/src/tongji/fire_controller/fire_controller.hpp index 8b2e658..8458a07 100644 --- a/src/tongji/fire_controller/fire_controller.hpp +++ b/src/tongji/fire_controller/fire_controller.hpp @@ -3,6 +3,7 @@ #include #include "interfaces/fire_controller.hpp" +#include "tongji/predictor/target_snapshot_manager.hpp" namespace world_exe::tongji::fire_control { @@ -14,6 +15,8 @@ class FireController final : public interfaces::IFireControl { const enumeration ::CarIDFlag GetAttackCarId() const override; + void SetSnapshotManager(std::shared_ptr manager); + private: class Impl; std::unique_ptr pimpl_; diff --git a/src/tongji/fire_controller/tracker.hpp b/src/tongji/fire_controller/tracker.hpp index b065bfd..9e871e0 100644 --- a/src/tongji/fire_controller/tracker.hpp +++ b/src/tongji/fire_controller/tracker.hpp @@ -6,10 +6,22 @@ #include "../predictor/target_snapshot.hpp" #include "../predictor/target_snapshot_manager.hpp" +#include "enum/armor_id.hpp" #include "enum/car_id.hpp" #include "enum/enum_tools.hpp" #include "interfaces/car_state.hpp" #include "tongji/identifier/identified_armor.hpp" +#include "tongji/predictor/time_stamp.hpp" + +namespace world_exe::tongji::fire_control { + +enum class TrackState { + Lost, // + Detecting, // + Tracking, // + TempLost, // + Switching // +}; class DefaultTracker final { using TargetSnapshotManager = world_exe::tongji::predictor::TargetSnapshotManager; @@ -18,11 +30,12 @@ class DefaultTracker final { using EnemiesState = world_exe::interfaces::ICarState; public: - DefaultTracker() = default; + DefaultTracker() + : last_timestamp_(0) { } ~DefaultTracker() = default; - auto CalculateTarget( // + auto SelectTrackingTarget( // ArmorInImage& armors, // const std::shared_ptr& snapshot_manager_ // ) noexcept -> std::unique_ptr { @@ -38,21 +51,128 @@ class DefaultTracker final { }); sq_armor_list->sort([](const auto& a, const auto& b) { return a.priority < b.priority; }); - auto iterator = sq_armor_list->begin(); + // auto iterator = sq_armor_list->begin(); auto filter_flag = snapshot_manager_->GetId(); + for (const auto& armor : *sq_armor_list) { + if (!enumeration::IsFlagContains(filter_flag, armor.armor.id)) continue; + + auto snapshot = snapshot_manager_->GetSingleSnapshot(armor.armor.id); + if (!snapshot) continue; - while (!world_exe::enumeration::IsFlagContains(filter_flag, iterator->armor.id)) { - ++iterator; - if (iterator == sq_armor_list->end()) return nullptr; + if (state_ == TrackState::Tracking && snapshot->GetPriority() < current_priority_) { + SetState(TrackState::Switching); + temp_lost_count_ = 0; + } + + auto now = predictor::TimeStamp(std::time(nullptr)); + if (state_ != TrackState::Lost && now.SecondsSince(last_timestamp_) > 0.1) { + SetState(TrackState::Lost); + ResetTracking(); + return nullptr; + } + + last_timestamp_ = now; + tracking_car_id_ = snapshot->GetID(); + current_priority_ = snapshot->GetPriority(); + return snapshot; } - auto snapshot = snapshot_manager_->GetSingleSnapshot(iterator->armor.id); - if (!snapshot) return nullptr; - tracking_car_id_ = snapshot->GetID(); - return snapshot; + ResetTracking(); + return nullptr; } world_exe::enumeration::CarIDFlag GetCurrentTargetID() const { return tracking_car_id_; } + void UpdateState(bool found) { + switch (state_) { + case TrackState::Lost: { + if (found) { + SetState(TrackState::Detecting); + detect_count_++; + } else { + ResetTracking(); + } + break; + } + + case TrackState::Detecting: { + if (found) { + detect_count_++; + if (detect_count_ >= min_detect_count_) SetState(TrackState::Tracking); + } else { + detect_count_ = 0; + SetState((pre_state_ == TrackState::Switching) ? TrackState::Switching + : TrackState::Lost); + } + break; + } + + case TrackState::Tracking: { + if (!found) { + temp_lost_count_ = 1; + SetState(TrackState::TempLost); + } + break; + } + + case TrackState::Switching: { + if (found) { + SetState(TrackState::Detecting); + } else { + temp_lost_count_++; + if (temp_lost_count_ > max_switch_count_) { + SetState(TrackState::Lost); + ResetTracking(); + }; + } + break; + } + + case TrackState::TempLost: { + if (found) { + SetState(TrackState::Tracking); + } else { + temp_lost_count_++; + max_temp_lost_count_ = (tracking_car_id_ == enumeration::ArmorIdFlag::Outpost) + ? outpost_max_temp_lost_count_ + : normal_max_temp_lost_count_; + + if (temp_lost_count_ > max_temp_lost_count_) { + SetState(TrackState::Lost); + ResetTracking(); + }; + } + break; + } + } + } + + TrackState GetState() const { return state_; } + private: - world_exe::enumeration::CarIDFlag tracking_car_id_; + void SetState(TrackState new_state) { + pre_state_ = state_; + state_ = new_state; + } + + void ResetTracking() { + tracking_car_id_ = enumeration::CarIDFlag::None; + current_priority_ = 100; + } + + int current_priority_ = 100; + world_exe::enumeration::CarIDFlag tracking_car_id_ { enumeration::CarIDFlag::None }; + TrackState state_ = TrackState::Lost; + TrackState pre_state_ = TrackState::Lost; + + int detect_count_ = 0; + int temp_lost_count_ = 0; + int max_temp_lost_count_ = 15; + const int min_detect_count_ = 5; + const int outpost_max_temp_lost_count_ = 75; + const int normal_max_temp_lost_count_ = max_temp_lost_count_; + const int max_switch_count_ = 200; + + predictor::TimeStamp last_timestamp_; }; + +} \ No newline at end of file diff --git a/src/tongji/identifier/identified_armor.hpp b/src/tongji/identifier/identified_armor.hpp index d7b3d59..950fb32 100644 --- a/src/tongji/identifier/identified_armor.hpp +++ b/src/tongji/identifier/identified_armor.hpp @@ -1,9 +1,11 @@ #pragma once #include "enum/armor_id.hpp" +#include "enum/car_id.hpp" #include "interfaces/armor_in_image.hpp" #include "interfaces/time_stamped.hpp" #include "util/index.hpp" +#include #include #include @@ -48,6 +50,17 @@ class IdentifiedArmor final : public interfaces::IArmorInImage, public interface throw std::runtime_error("Not implemented"); } + auto GetDetectedIDs() { + enumeration::CarIDFlag result; + for (int i = 0; i < 8; i++) { + if (!armors_[i].empty()) { + result = static_cast( + static_cast(result) | static_cast(armors_[i][0].id)); + } + } + return result; + } + private: std::time_t time_stamp_ { 0 }; std::array, 8> armors_; diff --git a/src/tongji/predictor/live_target.hpp b/src/tongji/predictor/live_target.hpp index 3edc46b..2f5eed1 100644 --- a/src/tongji/predictor/live_target.hpp +++ b/src/tongji/predictor/live_target.hpp @@ -8,31 +8,16 @@ #include "enum/car_id.hpp" #include "predict_model.hpp" +#include "tongji/state_machine/car_state_manager.hpp" #include "util/extended_kalman_filter.hpp" namespace world_exe::tongji::predictor { -// struct TargetStatus { -// bool jumped = false; -// bool switched = false; -// bool converged = false; -// bool diverged = false; -// bool lost = false; -// bool reidentified = false; -// int last_id = -1; -// double lock_id_ = -1; -// int switch_count = 0; -// int update_count = 0; -// int lost_count = 0; -// }; - class LiveTarget { public: - // TargetStatus status_; - LiveTarget(const Eigen::Vector3d& armor_xyz_in_world, const Eigen::Vector3d& armor_ypr_in_world, - const enumeration::CarIDFlag& car_id, const std::time_t& t) - : time_stamp_(t) + const enumeration::CarIDFlag& car_id) + : last_see_time_stamp_(std::time(nullptr)) , model_(car_id) { // x vx y vy z vz a w r l h @@ -52,13 +37,13 @@ class LiveTarget { Eigen::MatrixXd P0 = model_.GetP0Dig().asDiagonal(); ekf_ = util::ExtendedKalmanFilter( - x0, P0, model_.x_add); // 初始化滤波器（预测量、预测量协方差） + x0, P0, model_.x_add); // 初始化滤波器（预测量、预测量协方差） } Eigen::VectorXd GetEkfX() const { return ekf_.x; } Eigen::VectorXd GetP0Dig() const { return model_.GetP0Dig(); } const PredictModel& GetModel() const { return model_; } - std::time_t GetTimeStamp() const { return time_stamp_; } + predictor::TimeStamp LastSeen() const { return predictor::TimeStamp(last_see_time_stamp_); } void Update(const double& dt, const Eigen::Vector3d& armor_xyz_in_world, const Eigen::Vector3d& armor_ypr_in_world, const Eigen::Vector3d& armor_ypd_in_world) { @@ -69,10 +54,14 @@ class LiveTarget { update_count++; Update_ypda(armor_xyz_in_world, armor_ypr_in_world, armor_ypd_in_world, id, dt); + + last_see_time_stamp_ = std::time(nullptr); } + bool IsConverged() const { return EvaluateConvergence(); } + private: - bool EvaluateConvergence() { + bool EvaluateConvergence() const { // 前哨站特殊判断 const int required_count = (model_.GetID() == enumeration::CarIDFlag::Outpost) ? 10 : 3; if (update_count < required_count) return false; @@ -116,19 +105,19 @@ class LiveTarget { ekf_.Update(dt, A, Q, f, z, H, R, h, z_subtract); // 前哨站转速特判 - // TODO - // if (this->Converged() && model_.GetID() == enumeration::CarIDFlag::Outpost - // && std::abs(this->ekf_.x[7]) > 2) - // this->ekf_.x[7] = this->ekf_.x[7] > 0 ? 2.51 : -2.51; + if (model_.GetID() == enumeration::CarIDFlag::Outpost && EvaluateConvergence()) { + constexpr double max_outpost_w = 2.51; + if (std::abs(ekf_.x[7]) > 2.0) { + ekf_.x[7] = ekf_.x[7] > 0 ? max_outpost_w : -max_outpost_w; + } + } } - std::time_t time_stamp_; + std::time_t last_see_time_stamp_; util::ExtendedKalmanFilter ekf_; PredictModel model_; int last_id = -1; int update_count = 0; - - // bool converged_ { false }; }; } \ No newline at end of file diff --git a/src/tongji/predictor/live_target_manager.cpp b/src/tongji/predictor/live_target_manager.cpp index f2e1972..698d9a2 100644 --- a/src/tongji/predictor/live_target_manager.cpp +++ b/src/tongji/predictor/live_target_manager.cpp @@ -1,10 +1,12 @@ #include "live_target_manager.hpp" #include +#include #include #include #include "enum/armor_id.hpp" +#include "enum/car_id.hpp" #include "enum/enum_tools.hpp" #include "interfaces/predictor_update_package.hpp" #include "tongji/predictor/in_gimbal_control_armor.hpp" @@ -16,24 +18,30 @@ namespace world_exe::tongji::predictor { class LiveTargetManager::Impl { public: - Impl() = default; + Impl(double timeout_sec = 0.1) + : timeout_sec_(timeout_sec) { } void RegisterTarget(enumeration::ArmorIdFlag id, std::shared_ptr target) { targets_[id] = std::move(target); } void RemoveTarget(enumeration::ArmorIdFlag id) { targets_.erase(id); } + bool HasTarget(enumeration::ArmorIdFlag id) const { return targets_.count(id) > 0; } + std::shared_ptr GetTarget(enumeration::ArmorIdFlag id) const { + auto it = targets_.find(id); + return (it != targets_.end()) ? it->second : nullptr; + } std::shared_ptr Predict( const enumeration::ArmorIdFlag& flag, const std::time_t& time_stamp) { std::unordered_map> result; + throw std::runtime_error("Not implemented"); // TODO - // for (auto id : util::enumeration::ExpandArmorIdFlags(flag)) { // auto it = targets_.find(id); - // if (it != targets_.end() && it->second) { + // if (it != targets_.end() && it->second && it->second->IsConverged()) { // result[id].emplace_back(it->second->GetArmorGimbalControlSpacings()); // } // } @@ -47,7 +55,7 @@ class LiveTargetManager::Impl { for (auto id : util::enumeration::ExpandArmorIdFlags(flag)) { auto it = targets_.find(id); - if (it != targets_.end() && it->second) { + if (it != targets_.end() && it->second && it->second->IsConverged()) { snapshot_map[id] = it->second; } } @@ -55,7 +63,15 @@ class LiveTargetManager::Impl { return std::make_shared(flag, snapshot_map); } - void Update(std::shared_ptr data, const double& dt) { + void Update(std::shared_ptr data, double dt) { + const auto now = predictor::TimeStamp(std::time(nullptr)); + RemoveLostTargets(now); + UpdateTargets(data, dt); + } + +private: + void UpdateTargets( + const std::shared_ptr& data, double dt) { const auto& transform = data->GetTransform(); const auto& rotation_transform = Eigen::Quaterniond(transform.linear()); @@ -74,8 +90,23 @@ class LiveTargetManager::Impl { } } -private: + void RemoveLostTargets(const predictor::TimeStamp& now) { + for (auto it = targets_.begin(); it != targets_.end();) { + if (IsTargetLost(it->second, now)) { + it = targets_.erase(it); + } else { + ++it; + } + } + } + + bool IsTargetLost( + const std::shared_ptr& target, const predictor::TimeStamp& now) const { + return now.SecondsSince(target->LastSeen()) > timeout_sec_; + } + std::unordered_map> targets_; + const double timeout_sec_ { 0.1 }; }; LiveTargetManager::LiveTargetManager() = default; diff --git a/src/tongji/predictor/predict_model.hpp b/src/tongji/predictor/predict_model.hpp index 8fe4b43..6697f3b 100644 --- a/src/tongji/predictor/predict_model.hpp +++ b/src/tongji/predictor/predict_model.hpp @@ -46,11 +46,14 @@ class PredictModel { } else { armor_num_ = 4; } + + // TODO :add priority } auto GetP0Dig() const { return P0_dig_; } auto GetRadius() const { return radius_; } auto GetID() const { return car_id_; } + auto GetPriority() const { return priority_; } int GetArmorNum() const { return armor_num_; } // 防止夹角求和出现异常值 @@ -69,6 +72,7 @@ class PredictModel { return x_prior; }; + //TODO int MatchArmor(const Eigen::VectorXd& x, const Eigen::Vector3d& armor_xyz_in_world, const Eigen::Vector3d& armor_ypr_in_world, const Eigen::Vector3d& armor_ypd_in_world) const { @@ -248,6 +252,7 @@ class PredictModel { } private: + int priority_; int armor_num_; enumeration::CarIDFlag car_id_; diff --git a/src/tongji/predictor/target_snapshot.hpp b/src/tongji/predictor/target_snapshot.hpp index 75a5e96..b390a36 100644 --- a/src/tongji/predictor/target_snapshot.hpp +++ b/src/tongji/predictor/target_snapshot.hpp @@ -5,6 +5,7 @@ #include "data/armor_gimbal_control_spacing.hpp" #include "tongji/predictor/live_target.hpp" #include "tongji/predictor/predict_model.hpp" +#include "tongji/predictor/time_stamp.hpp" #include "util/extended_kalman_filter.hpp" namespace world_exe::tongji::predictor { @@ -14,7 +15,7 @@ class TargetSnapshot { TargetSnapshot(const LiveTarget& target) : model_(target.GetModel()) , ekf_(target.GetEkfX(), target.GetP0Dig().asDiagonal(), model_.x_add) - , time_stamp_(target.GetTimeStamp()) { } + , time_stamp_(target.LastSeen()) { } std::vector GetArmorGimbalControlSpacings() const { std::vector armors; @@ -37,10 +38,9 @@ class TargetSnapshot { } auto GetTimeStamp() const { return time_stamp_; } - auto GetID() const { return model_.GetID(); } - auto GetEkfX() const { return ekf_.x; } + auto GetPriority() const { return model_.GetPriority(); } auto Predict(const double& dt) -> Eigen::Vector const { auto predicted_x = model_.f(ekf_.x, dt); @@ -50,7 +50,7 @@ class TargetSnapshot { private: PredictModel model_; util::ExtendedKalmanFilter ekf_; - const std::time_t time_stamp_; + TimeStamp time_stamp_; }; } \ No newline at end of file diff --git a/src/tongji/state_machine/car_state_manager.hpp b/src/tongji/state_machine/car_state_manager.hpp index 219e457..4e940bb 100644 --- a/src/tongji/state_machine/car_state_manager.hpp +++ b/src/tongji/state_machine/car_state_manager.hpp @@ -6,57 +6,61 @@ namespace world_exe::tongji::car_state { +using TimeStamp = predictor::TimeStamp; class CarStateManager { public: CarStateManager(int switch_threshold = 5) - : switch_threshold_(switch_threshold) { } + : switch_threshold_(switch_threshold) + , last_seen_(std::time(nullptr)) { } - void Update(bool detected, std::time_t now_raw) { + void Update(bool detected, const TimeStamp& now) { if (detected) { count_ = std::min(count_ + 1, switch_threshold_); - auto now = predictor::TimeStamp(now_raw); last_seen_ = now; + update_count_++; + if (update_count_ >= converge_threshold_ && is_diverged_) is_converged_ = true; } else { count_ = std::max(count_ - 1, 0); } is_locked_ = (count_ >= switch_threshold_); } - bool IsLost(const predictor::TimeStamp& now, double timeout_sec) const { - return now.SecondsSince(last_seen_) > timeout_sec; - } + bool IsConverged() const { return is_converged_ && !is_diverged_; } - bool IsLocked() const { return is_locked_; } - bool IsConverged() const { return is_converged_; } - bool IsDiverged() const { return is_diverged_; } + bool IsLost(const TimeStamp& now) const { + return is_locked_ && now.SecondsSince(last_seen_) > timeout_sec_; + } - void SetPriority(int p); - void SetThreshold(const int& value) { switch_threshold_ = value; } - void SetDiverged(bool diverged) { - is_diverged_ = diverged; - if (diverged) is_converged_ = false; + void Reset() { + count_ = 0; + update_count_ = 0; + is_locked_ = false; + is_converged_ = false; + is_diverged_ = true; + priority_ = default_priority_; + last_seen_ = predictor::TimeStamp(0); } + void SetPriority(const int& p) { priority_ = p; } int GetPriority() const { return priority_; } - predictor::TimeStamp LastSeen() const { return last_seen_; } + void SetThreshold(const int& value) { switch_threshold_ = value; } - void IncrementUpdateCount() { - update_count_++; - if (update_count_ > 3 && !is_diverged_) { - is_converged_ = true; - } - } + predictor::TimeStamp LastSeen() const { return last_seen_; } private: int count_ = 0; int switch_threshold_; - int update_count_ = 0; + int converge_threshold_ = 3; + int update_count_ = 0; + double timeout_sec_; + + bool is_locked_ = false; + bool is_converged_ = false; + bool is_diverged_ = true; + int priority_ = 100; + int default_priority_ = 100; - bool is_locked_ = false; - bool is_converged_ = false; - bool is_diverged_ = false; - int priority_ = 100; // 默认最低优先级 - predictor::TimeStamp last_seen_ { 0 }; + predictor::TimeStamp last_seen_; }; } \ No newline at end of file diff --git a/src/tongji/state_machine/state_machine.cpp b/src/tongji/state_machine/state_machine.cpp index 442148d..3afd297 100644 --- a/src/tongji/state_machine/state_machine.cpp +++ b/src/tongji/state_machine/state_machine.cpp @@ -1,36 +1,56 @@ #include "state_machine.hpp" +#include + #include "enum/car_id.hpp" +#include "tongji/state_machine/car_state_manager.hpp" +#include "util/index.hpp" namespace world_exe::tongji::state_machine { +using TimeStamp = car_state::TimeStamp; + StateMachine::StateMachine(int switch_threshold) - : switch_threshold_(switch_threshold) { - for (auto& manager : car_states_) { - manager = car_state::CarStateManager(switch_threshold_); - } + : switch_threshold_(switch_threshold) + , car_states_ {} { + std::generate(car_states_.begin(), car_states_.end(), + [threshold = switch_threshold]() { return car_state::CarStateManager(threshold); }); } const enumeration::CarIDFlag& StateMachine::GetAllowdToFires() const { return tracing_state_; } +bool StateMachine::IsAllowedToFire(enumeration::CarIDFlag id) const { return tracing_state_ == id; } + +const car_state::CarStateManager& StateMachine::GetState(enumeration::CarIDFlag single_id) const { + return car_states_[util::enumeration::GetIndex(single_id)]; +} + const interfaces::ICarState& StateMachine::Update( - const enumeration::CarIDFlag& car_detected, std::time_t now) { - tracing_state_ = enumeration::CarIDFlag::None; + const enumeration::CarIDFlag& car_detected, const std::time_t& now) { + tracing_state_ = enumeration::CarIDFlag::None; + int best_priority = std::numeric_limits::max(); for (int i = 0; i < static_cast(enumeration::CarIDFlag::Count); ++i) { auto id = static_cast(1 << i); bool detected = (static_cast(car_detected) >> i) & 0x01; - car_states_[i].Update(detected, now); + auto& state = car_states_[i]; + state.Update(detected, now); + + if (!state.IsConverged()) { + state.Reset(); + continue; + } - if (detected) { - car_states_[i].IncrementUpdateCount(); + if (state.IsLost(now)) { + state.Reset(); + continue; } - if (car_states_[i].IsLocked() && car_states_[i].IsConverged() - && !car_states_[i].IsDiverged()) { - tracing_state_ = - static_cast(static_cast(tracing_state_) | (1 << i)); + int p = state.GetPriority(); + if (p < best_priority) { + best_priority = p; + tracing_state_ = id; } } diff --git a/src/tongji/state_machine/state_machine.hpp b/src/tongji/state_machine/state_machine.hpp index 8fb9872..a10d1eb 100644 --- a/src/tongji/state_machine/state_machine.hpp +++ b/src/tongji/state_machine/state_machine.hpp @@ -12,9 +12,10 @@ class StateMachine final : public interfaces::ICarState { StateMachine(int switch_threshold = 5); const enumeration::CarIDFlag& GetAllowdToFires() const override; - + bool IsAllowedToFire(enumeration::CarIDFlag id) const; + const car_state::CarStateManager& GetState(enumeration::CarIDFlag single_id) const; const interfaces::ICarState& Update( - const enumeration::CarIDFlag& car_detected, std::time_t now); + const enumeration::CarIDFlag& car_detected, const std::time_t& now); private: enumeration::CarIDFlag tracing_state_ = enumeration::CarIDFlag::None; From b11971e3d19cae7ad5fdc48372ee713accba50f2 Mon Sep 17 00:00:00 2001 From: heyeuu <2829004293@qq.com> Date: Wed, 8 Oct 2025 02:12:52 +0800 Subject: [PATCH 42/53] chore(state_machine):refactor StateMachine to PIMPL pattern and do some unimportant cleanup --- .../fire_controller/aim_point_chooser.hpp | 10 +- src/tongji/fire_controller/aim_solver.hpp | 18 ++-- .../fire_controller/fire_controller.cpp | 35 ++++--- src/tongji/predictor/live_target_manager.cpp | 1 - src/tongji/state_machine/state_machine.cpp | 96 ++++++++++++------- src/tongji/state_machine/state_machine.hpp | 15 +-- 6 files changed, 104 insertions(+), 71 deletions(-) diff --git a/src/tongji/fire_controller/aim_point_chooser.hpp b/src/tongji/fire_controller/aim_point_chooser.hpp index b457a76..5ba8b72 100644 --- a/src/tongji/fire_controller/aim_point_chooser.hpp +++ b/src/tongji/fire_controller/aim_point_chooser.hpp @@ -5,6 +5,8 @@ namespace world_exe::tongji::fire_control { +using CarIDFlag = enumeration::CarIDFlag; + class AimPointChooser { public: AimPointChooser( @@ -13,7 +15,7 @@ class AimPointChooser { , leaving_angle_(leaving_angle) { } std::pair ChooseAimArmor(const Eigen::Vector& ekf_x, - const std::vector& xyza_list, const enumeration::CarIDFlag& single_id) { + const std::vector& xyza_list, const CarIDFlag& single_id) { const auto armor_num = xyza_list.size(); int chosen_id = -1; @@ -28,7 +30,7 @@ class AimPointChooser { } // 不考虑小陀螺 - if (std::abs(ekf_x[8]) <= 2 && single_id != enumeration::CarIDFlag::Outpost) { + if (std::abs(ekf_x[8]) <= 2 && single_id != CarIDFlag::Outpost) { // 选择在可射击范围内的装甲板 std::vector id_list; for (int i = 0; i < armor_num; i++) { @@ -53,9 +55,9 @@ class AimPointChooser { } else { // 小陀螺 double coming_angle = - (single_id == enumeration::CarIDFlag::Outpost) ? 70 / 57.3 : comming_angle_; + (single_id == CarIDFlag::Outpost) ? 70 / 57.3 : comming_angle_; double leaving_angle = - (single_id == enumeration::CarIDFlag::Outpost) ? 30 / 57.3 : leaving_angle_; + (single_id == CarIDFlag::Outpost) ? 30 / 57.3 : leaving_angle_; // 在小陀螺时，一侧的装甲板不断出现，另一侧的装甲板不断消失，显然前者被打中的概率更高 for (int i = 0; i < armor_num; i++) { diff --git a/src/tongji/fire_controller/aim_solver.hpp b/src/tongji/fire_controller/aim_solver.hpp index 12fec38..c462bf1 100644 --- a/src/tongji/fire_controller/aim_solver.hpp +++ b/src/tongji/fire_controller/aim_solver.hpp @@ -12,6 +12,8 @@ namespace world_exe::tongji::fire_control { +using TargetSnapshot = predictor::TargetSnapshot; + struct AimSolution { bool valid; double yaw; @@ -22,20 +24,20 @@ struct AimSolution { class AimingSolver { public: - AimingSolver( const double& bullet_speed, - const double& yaw_offset, const double& pitch_offset, const double& gravity = 9.7833) + AimingSolver(const double& bullet_speed, const double& yaw_offset, const double& pitch_offset, + const double& gravity = 9.7833) : aim_point_chooser_(std::make_unique()) , bullet_speed_(bullet_speed) , yaw_offset_(yaw_offset / 57.3) // degree to rad , pitch_offset_(pitch_offset / 57.3) // degree to rad , g_(gravity) { } - void UpdateSnapshot(std::unique_ptr snapshot) { + void UpdateSnapshot(std::unique_ptr snapshot) { snapshot_ = std::move(snapshot); } AimSolution SolveAimSolution(const double& time_delay) { - if (!snapshot_) return { false, 0, 0, {}, 0 }; + if (!snapshot_) return { false, 0, 0, { }, 0 }; // 迭代求解飞行时间 (最多10次，收敛条件：相邻两次fly_time差 <0.001) double prev_fly_time = 0; @@ -47,10 +49,10 @@ class AimingSolver { for (int i = 0; i < 10; ++i) { double dt = time_delay + prev_fly_time; const auto aim = SelectPredictedAim(dt); - if (!aim.has_value()) return { false, 0, 0, {}, 0 }; // failed: no valid aim point + if (!aim.has_value()) return { false, 0, 0, { }, 0 }; // failed: no valid aim point const auto traj = SolveTrajectory(aim->head(3)); - if (!traj.has_value()) return { false, 0, 0, {}, 0 }; // failed: trajectory unsolvable + if (!traj.has_value()) return { false, 0, 0, { }, 0 }; // failed: trajectory unsolvable if (i > 0 && std::abs(traj->fly_time - prev_fly_time) < 0.001) { final_aim_point = *aim; @@ -60,7 +62,7 @@ class AimingSolver { } prev_fly_time = traj->fly_time; } - if (!converged) return { false, 0, 0, {}, 0 }; // failed: trajectory did not converge + if (!converged) return { false, 0, 0, { }, 0 }; // failed: trajectory did not converge const auto xyz = final_aim_point.head(3); const double yaw = std::atan2(xyz.y(), xyz.x()) + yaw_offset_; @@ -86,6 +88,6 @@ class AimingSolver { const double g_; std::unique_ptr aim_point_chooser_; - std::unique_ptr snapshot_; + std::unique_ptr snapshot_; }; } \ No newline at end of file diff --git a/src/tongji/fire_controller/fire_controller.cpp b/src/tongji/fire_controller/fire_controller.cpp index 114de5d..7639411 100644 --- a/src/tongji/fire_controller/fire_controller.cpp +++ b/src/tongji/fire_controller/fire_controller.cpp @@ -19,6 +19,9 @@ namespace world_exe::tongji::fire_control { using TargetSnapshotManager = predictor::TargetSnapshotManager; +using StateMachine = state_machine::StateMachine; +using IdentifiedArmor = identifier::IdentifiedArmor; +using CarIDFlag = enumeration::CarIDFlag; class FireController::Impl { public: @@ -27,17 +30,19 @@ class FireController::Impl { : control_delay_(control_delay_in_second) , bullet_speed_(bullet_speed) , tracker_(std::make_unique()) - , predictor_(nullptr) // TODO + , snapshot_manager_(nullptr) // TODO:A new snapshot manager needs to be created based on the + // input of each frame of the camera , aim_solver_(std::make_unique(bullet_speed, yaw_offset, pitch_offset)) , fire_decision_(std::make_unique(auto_fire)) - , state_machine_(std::make_unique()) { } + , state_machine_(std::make_unique()) { } // TODO:std::time_t const data ::FireControl CalculateTarget(const std ::time_t& time_duration) const { - if (!identified_armors_ || !tracker_ || !predictor_ || !aim_solver_ || !fire_decision_) + if (!identified_armors_ || !tracker_ || !snapshot_manager_ || !aim_solver_ + || !fire_decision_) return { .fire_allowance = false }; - auto snapshot = tracker_->SelectTrackingTarget(*identified_armors_, predictor_); + auto snapshot = tracker_->SelectTrackingTarget(*identified_armors_, snapshot_manager_); bool found = (snapshot != nullptr); tracker_->UpdateState(found); if (!found) return { .fire_allowance = false }; @@ -56,23 +61,23 @@ class FireController::Impl { const Eigen::Vector3d target_pos = aim_solution.aim_point.head<3>(); const bool fireable = fire_decision_->ShouldFire(aim_solution, target_pos, gimbal_pos_) && state_machine_->IsAllowedToFire(current_id); - + return { .time_stamp = time_stamp_.GetTimeStamp(), .gimbal_dir = Eigen::Vector3d(aim_solution.yaw, aim_solution.pitch, 0), .fire_allowance = fireable }; } // TODO - const enumeration ::CarIDFlag GetAttackCarId() const { - if (!tracker_) return enumeration::CarIDFlag::Unknow; + const CarIDFlag GetAttackCarId() const { + if (!tracker_) return CarIDFlag::Unknow; return tracker_->GetCurrentTargetID(); } - void SetSnapshotManager(std::shared_ptr manager) { - predictor_ = std::move(manager); + void SetSnapshotManager(std::shared_ptr manager) { + snapshot_manager_ = std::move(manager); } - void UpdateArmors(const identifier::IdentifiedArmor& armors) { identified_armors_ = armors; } + void UpdateArmors(const IdentifiedArmor& armors) { identified_armors_ = armors; } void UpdateGimbalPosition(const Eigen::Vector3d& gimbal_pos) { gimbal_pos_ = gimbal_pos; }; void SetTimeStamp(const std::time_t& time_stamp) { time_stamp_.SetTimeStamp(time_stamp); } @@ -81,12 +86,12 @@ class FireController::Impl { double control_delay_; double bullet_speed_; - mutable std::optional identified_armors_; + mutable std::optional identified_armors_; std::unique_ptr tracker_; - std::shared_ptr predictor_; + std::shared_ptr snapshot_manager_; std::unique_ptr aim_solver_; std::unique_ptr fire_decision_; - std::unique_ptr state_machine_; + std::unique_ptr state_machine_; predictor::TimeStamp time_stamp_ { 0 }; }; @@ -98,11 +103,11 @@ const data ::FireControl FireController::CalculateTarget(const std ::time_t& tim return pimpl_->CalculateTarget(time_duration); } -void FireController::SetSnapshotManager(std::shared_ptr manager) { +void FireController::SetSnapshotManager(std::shared_ptr manager) { pimpl_->SetSnapshotManager(std::move(manager)); } -const enumeration ::CarIDFlag FireController::GetAttackCarId() const { +const CarIDFlag FireController::GetAttackCarId() const { return pimpl_->GetAttackCarId(); } diff --git a/src/tongji/predictor/live_target_manager.cpp b/src/tongji/predictor/live_target_manager.cpp index 698d9a2..7bdc12c 100644 --- a/src/tongji/predictor/live_target_manager.cpp +++ b/src/tongji/predictor/live_target_manager.cpp @@ -6,7 +6,6 @@ #include #include "enum/armor_id.hpp" -#include "enum/car_id.hpp" #include "enum/enum_tools.hpp" #include "interfaces/predictor_update_package.hpp" #include "tongji/predictor/in_gimbal_control_armor.hpp" diff --git a/src/tongji/state_machine/state_machine.cpp b/src/tongji/state_machine/state_machine.cpp index 3afd297..c72ffa6 100644 --- a/src/tongji/state_machine/state_machine.cpp +++ b/src/tongji/state_machine/state_machine.cpp @@ -1,59 +1,83 @@ #include "state_machine.hpp" -#include +#include #include "enum/car_id.hpp" #include "tongji/state_machine/car_state_manager.hpp" #include "util/index.hpp" namespace world_exe::tongji::state_machine { - using TimeStamp = car_state::TimeStamp; -StateMachine::StateMachine(int switch_threshold) - : switch_threshold_(switch_threshold) - , car_states_ {} { - std::generate(car_states_.begin(), car_states_.end(), - [threshold = switch_threshold]() { return car_state::CarStateManager(threshold); }); -} +class StateMachine::Impl { +public: + Impl() + : car_states_ { } { + std::generate( + car_states_.begin(), car_states_.end(), []() { return car_state::CarStateManager(); }); + } -const enumeration::CarIDFlag& StateMachine::GetAllowdToFires() const { return tracing_state_; } + const enumeration::CarIDFlag& GetAllowdToFires() const { return tracing_state_; } -bool StateMachine::IsAllowedToFire(enumeration::CarIDFlag id) const { return tracing_state_ == id; } + bool IsAllowedToFire(enumeration::CarIDFlag id) const { return tracing_state_ == id; } -const car_state::CarStateManager& StateMachine::GetState(enumeration::CarIDFlag single_id) const { - return car_states_[util::enumeration::GetIndex(single_id)]; -} + const car_state::CarStateManager& GetState(enumeration::CarIDFlag single_id) const { + return car_states_[util::enumeration::GetIndex(single_id)]; + } + void Update(const enumeration::CarIDFlag& car_detected, const std::time_t& now) { + tracing_state_ = enumeration::CarIDFlag::None; + int best_priority = std::numeric_limits::max(); + + for (int i = 0; i < static_cast(enumeration::CarIDFlag::Count); ++i) { + auto id = static_cast(1 << i); + bool detected = (static_cast(car_detected) >> i) & 0x01; + + auto& state = car_states_[i]; + state.Update(detected, now); + + if (!state.IsConverged()) { + state.Reset(); + continue; + } + + if (state.IsLost(now)) { + state.Reset(); + continue; + } + + int p = state.GetPriority(); + if (p < best_priority) { + best_priority = p; + tracing_state_ = id; + } + } + } -const interfaces::ICarState& StateMachine::Update( - const enumeration::CarIDFlag& car_detected, const std::time_t& now) { - tracing_state_ = enumeration::CarIDFlag::None; - int best_priority = std::numeric_limits::max(); +private: + enumeration::CarIDFlag tracing_state_ = enumeration::CarIDFlag::None; + std::array car_states_; +}; - for (int i = 0; i < static_cast(enumeration::CarIDFlag::Count); ++i) { - auto id = static_cast(1 << i); - bool detected = (static_cast(car_detected) >> i) & 0x01; +StateMachine::StateMachine() + : pimpl_(std::make_unique()) { } - auto& state = car_states_[i]; - state.Update(detected, now); +StateMachine::~StateMachine() = default; - if (!state.IsConverged()) { - state.Reset(); - continue; - } +const enumeration::CarIDFlag& StateMachine::GetAllowdToFires() const { + return pimpl_->GetAllowdToFires(); +} - if (state.IsLost(now)) { - state.Reset(); - continue; - } +bool StateMachine::IsAllowedToFire(enumeration::CarIDFlag id) const { + return pimpl_->IsAllowedToFire(id); +} - int p = state.GetPriority(); - if (p < best_priority) { - best_priority = p; - tracing_state_ = id; - } - } +const car_state::CarStateManager& StateMachine::GetState(enumeration::CarIDFlag single_id) const { + return pimpl_->GetState(single_id); +} +const interfaces::ICarState& StateMachine::Update( + const enumeration::CarIDFlag& car_detected, const std::time_t& now) { + pimpl_->Update(car_detected, now); return *this; } diff --git a/src/tongji/state_machine/state_machine.hpp b/src/tongji/state_machine/state_machine.hpp index a10d1eb..4b6fa86 100644 --- a/src/tongji/state_machine/state_machine.hpp +++ b/src/tongji/state_machine/state_machine.hpp @@ -1,25 +1,26 @@ #pragma once #include +#include -#include "car_state_manager.hpp" #include "enum/car_id.hpp" #include "interfaces/car_state.hpp" +#include "tongji/state_machine/car_state_manager.hpp" namespace world_exe::tongji::state_machine { class StateMachine final : public interfaces::ICarState { public: - StateMachine(int switch_threshold = 5); + StateMachine(); + ~StateMachine(); - const enumeration::CarIDFlag& GetAllowdToFires() const override; - bool IsAllowedToFire(enumeration::CarIDFlag id) const; + const enumeration ::CarIDFlag& GetAllowdToFires() const override; const car_state::CarStateManager& GetState(enumeration::CarIDFlag single_id) const; + bool IsAllowedToFire(enumeration::CarIDFlag id) const; const interfaces::ICarState& Update( const enumeration::CarIDFlag& car_detected, const std::time_t& now); private: - enumeration::CarIDFlag tracing_state_ = enumeration::CarIDFlag::None; - std::array car_states_; - int switch_threshold_; + class Impl; + std::unique_ptr pimpl_; }; } \ No newline at end of file From faaa5b1629de59e8344d7933f366adfa40596790 Mon Sep 17 00:00:00 2001 From: heyeuu <2829004293@qq.com> Date: Wed, 8 Oct 2025 05:47:31 +0800 Subject: [PATCH 43/53] feat(tracking): complete LiveTargetManager update logic --- .../fire_controller/fire_controller.cpp | 20 ++---- src/tongji/predictor/live_target.hpp | 21 +++++- src/tongji/predictor/live_target_manager.cpp | 64 +++++++++++++------ src/tongji/predictor/predict_model.hpp | 3 +- src/tongji/state_machine/state_machine.cpp | 19 ++---- src/tongji/state_machine/state_machine.hpp | 1 - 6 files changed, 76 insertions(+), 52 deletions(-) diff --git a/src/tongji/fire_controller/fire_controller.cpp b/src/tongji/fire_controller/fire_controller.cpp index 7639411..43f859b 100644 --- a/src/tongji/fire_controller/fire_controller.cpp +++ b/src/tongji/fire_controller/fire_controller.cpp @@ -55,23 +55,17 @@ class FireController::Impl { state_machine_->Update(identified_armors_->GetDetectedIDs(), time_stamp_.GetTimeStamp()); - const auto allowed_id = state_machine_->GetAllowdToFires(); // TODO - const auto current_id = tracker_->GetCurrentTargetID(); - const Eigen::Vector3d target_pos = aim_solution.aim_point.head<3>(); - const bool fireable = fire_decision_->ShouldFire(aim_solution, target_pos, gimbal_pos_) - && state_machine_->IsAllowedToFire(current_id); + const bool fireable = fire_decision_->ShouldFire(aim_solution, target_pos, gimbal_pos_); + + if (fireable) attacked_cars_ = snapshot->GetID(); return { .time_stamp = time_stamp_.GetTimeStamp(), .gimbal_dir = Eigen::Vector3d(aim_solution.yaw, aim_solution.pitch, 0), .fire_allowance = fireable }; } - // TODO - const CarIDFlag GetAttackCarId() const { - if (!tracker_) return CarIDFlag::Unknow; - return tracker_->GetCurrentTargetID(); - } + const CarIDFlag GetAttackCarId() const { return attacked_cars_; } void SetSnapshotManager(std::shared_ptr manager) { snapshot_manager_ = std::move(manager); @@ -86,6 +80,8 @@ class FireController::Impl { double control_delay_; double bullet_speed_; + mutable CarIDFlag attacked_cars_ { CarIDFlag::None }; // TODO + mutable std::optional identified_armors_; std::unique_ptr tracker_; std::shared_ptr snapshot_manager_; @@ -107,8 +103,6 @@ void FireController::SetSnapshotManager(std::shared_ptr m pimpl_->SetSnapshotManager(std::move(manager)); } -const CarIDFlag FireController::GetAttackCarId() const { - return pimpl_->GetAttackCarId(); -} +const CarIDFlag FireController::GetAttackCarId() const { return pimpl_->GetAttackCarId(); } } \ No newline at end of file diff --git a/src/tongji/predictor/live_target.hpp b/src/tongji/predictor/live_target.hpp index 2f5eed1..e0dc168 100644 --- a/src/tongji/predictor/live_target.hpp +++ b/src/tongji/predictor/live_target.hpp @@ -6,9 +6,10 @@ #include #include +#include "data/armor_gimbal_control_spacing.hpp" #include "enum/car_id.hpp" #include "predict_model.hpp" -#include "tongji/state_machine/car_state_manager.hpp" +#include "tongji/predictor/time_stamp.hpp" #include "util/extended_kalman_filter.hpp" namespace world_exe::tongji::predictor { @@ -37,7 +38,7 @@ class LiveTarget { Eigen::MatrixXd P0 = model_.GetP0Dig().asDiagonal(); ekf_ = util::ExtendedKalmanFilter( - x0, P0, model_.x_add); // 初始化滤波器（预测量、预测量协方差） + x0, P0, model_.x_add); // 初始化滤波器（预测量、预测量协方差） } Eigen::VectorXd GetEkfX() const { return ekf_.x; } @@ -45,6 +46,22 @@ class LiveTarget { const PredictModel& GetModel() const { return model_; } predictor::TimeStamp LastSeen() const { return predictor::TimeStamp(last_see_time_stamp_); } + std::vector GetArmorGimbalControlSpacings() const { + std::vector armors; + for (int id = 0; id < model_.GetArmorNum(); id++) { + auto angle = + util::math::clamp_pm_pi(this->ekf_.x[6] + id * 2 * CV_PI / model_.GetArmorNum()); + auto xyz = model_.h_armor_xyz(this->ekf_.x, id); + + data::ArmorGimbalControlSpacing armor; + armor.id = model_.GetID(); + armor.position = xyz; + armor.orientation = util::math::euler_to_quaternion(angle, 15. / 180. * CV_PI, 0); + armors.emplace_back(std::move(armor)); + } + return armors; + } + void Update(const double& dt, const Eigen::Vector3d& armor_xyz_in_world, const Eigen::Vector3d& armor_ypr_in_world, const Eigen::Vector3d& armor_ypd_in_world) { // 装甲板匹配 diff --git a/src/tongji/predictor/live_target_manager.cpp b/src/tongji/predictor/live_target_manager.cpp index 7bdc12c..504f828 100644 --- a/src/tongji/predictor/live_target_manager.cpp +++ b/src/tongji/predictor/live_target_manager.cpp @@ -1,11 +1,12 @@ #include "live_target_manager.hpp" +#include #include -#include #include #include #include "enum/armor_id.hpp" +#include "enum/car_id.hpp" #include "enum/enum_tools.hpp" #include "interfaces/predictor_update_package.hpp" #include "tongji/predictor/in_gimbal_control_armor.hpp" @@ -36,14 +37,13 @@ class LiveTargetManager::Impl { std::unordered_map> result; - throw std::runtime_error("Not implemented"); - // TODO - // for (auto id : util::enumeration::ExpandArmorIdFlags(flag)) { - // auto it = targets_.find(id); - // if (it != targets_.end() && it->second && it->second->IsConverged()) { - // result[id].emplace_back(it->second->GetArmorGimbalControlSpacings()); - // } - // } + for (auto id : util::enumeration::ExpandArmorIdFlags(flag)) { + auto it = targets_.find(id); + if (it != targets_.end() && it->second && it->second->IsConverged()) { + auto spacings = it->second->GetArmorGimbalControlSpacings(); + result[id] = std::move(spacings); + } + } return std::make_shared(result, time_stamp); } @@ -68,23 +68,48 @@ class LiveTargetManager::Impl { UpdateTargets(data, dt); } + auto GetLiveTargetIDs() -> enumeration::CarIDFlag const { + enumeration::CarIDFlag result = enumeration::CarIDFlag::None; + for (const auto& [id, target] : targets_) { + if (target && target->IsConverged()) { + result = static_cast( + static_cast(result) | static_cast(id)); + } + } + return result; + } + private: void UpdateTargets( const std::shared_ptr& data, double dt) { - const auto& transform = data->GetTransform(); - const auto& rotation_transform = Eigen::Quaterniond(transform.linear()); + const Eigen::Affine3d transform = data->GetTransform(); + const Eigen::Matrix3d rotation_matrix = transform.linear(); + const Eigen::Quaterniond rotation_quat(rotation_matrix); - for (const auto& [id, target] : targets_) { - const auto armors = data->GetArmors()->GetArmors(id); + const auto& armors_interface = data->GetArmors(); + for (int i; i < 8; i++) { + auto id = static_cast( + static_cast(enumeration::CarIDFlag::Hero) << i); + const auto& armors = armors_interface->GetArmors(id); if (armors.empty()) continue; - for (auto armor : armors) { - const auto& armor_in_world_position = transform * armor.position; - const auto& armor_in_world_oritaiton = rotation_transform * armor.orientation; - target->Update(dt, armor_in_world_position, - util::math::quaternion_to_euler(armor_in_world_oritaiton, 2, 1, 0), - util::math::xyz2ypd(armor_in_world_position)); + if (!HasTarget(id)) { + const auto& armor = armors.front(); + const Eigen::Vector3d position_in_world = transform * armor.position; + const Eigen::Vector3d ypr_in_world = rotation_matrix.eulerAngles(2, 1, 0); // ZYX + + auto target = std::make_shared(position_in_world, ypr_in_world, id); + RegisterTarget(id, target); + } + + auto target = GetTarget(id); + for (const auto& armor : armors) { + const Eigen::Vector3d position_in_world = transform * armor.position; + const Eigen::Vector3d ypr = + util::math::quaternion_to_euler(Eigen::Quaterniond(rotation_matrix), 2, 1, 0); + + target->Update(dt, position_in_world, ypr, util::math::xyz2ypd(position_in_world)); } } } @@ -120,5 +145,4 @@ std ::shared_ptr LiveTargetManager::GetPredictor( const enumeration ::ArmorIdFlag& id) const { return pimpl_->GetPredictor(id); } - } \ No newline at end of file diff --git a/src/tongji/predictor/predict_model.hpp b/src/tongji/predictor/predict_model.hpp index 6697f3b..109fee0 100644 --- a/src/tongji/predictor/predict_model.hpp +++ b/src/tongji/predictor/predict_model.hpp @@ -47,7 +47,7 @@ class PredictModel { armor_num_ = 4; } - // TODO :add priority + // TODO :init priority } auto GetP0Dig() const { return P0_dig_; } @@ -72,7 +72,6 @@ class PredictModel { return x_prior; }; - //TODO int MatchArmor(const Eigen::VectorXd& x, const Eigen::Vector3d& armor_xyz_in_world, const Eigen::Vector3d& armor_ypr_in_world, const Eigen::Vector3d& armor_ypd_in_world) const { diff --git a/src/tongji/state_machine/state_machine.cpp b/src/tongji/state_machine/state_machine.cpp index c72ffa6..2ce500e 100644 --- a/src/tongji/state_machine/state_machine.cpp +++ b/src/tongji/state_machine/state_machine.cpp @@ -1,6 +1,7 @@ #include "state_machine.hpp" #include +#include #include "enum/car_id.hpp" #include "tongji/state_machine/car_state_manager.hpp" @@ -16,17 +17,14 @@ class StateMachine::Impl { std::generate( car_states_.begin(), car_states_.end(), []() { return car_state::CarStateManager(); }); } - const enumeration::CarIDFlag& GetAllowdToFires() const { return tracing_state_; } - bool IsAllowedToFire(enumeration::CarIDFlag id) const { return tracing_state_ == id; } - const car_state::CarStateManager& GetState(enumeration::CarIDFlag single_id) const { return car_states_[util::enumeration::GetIndex(single_id)]; } + void Update(const enumeration::CarIDFlag& car_detected, const std::time_t& now) { - tracing_state_ = enumeration::CarIDFlag::None; - int best_priority = std::numeric_limits::max(); + tracing_state_ = enumeration::CarIDFlag::None; for (int i = 0; i < static_cast(enumeration::CarIDFlag::Count); ++i) { auto id = static_cast(1 << i); @@ -45,11 +43,8 @@ class StateMachine::Impl { continue; } - int p = state.GetPriority(); - if (p < best_priority) { - best_priority = p; - tracing_state_ = id; - } + tracing_state_ = static_cast( + static_cast(tracing_state_) | static_cast(id)); } } @@ -67,10 +62,6 @@ const enumeration::CarIDFlag& StateMachine::GetAllowdToFires() const { return pimpl_->GetAllowdToFires(); } -bool StateMachine::IsAllowedToFire(enumeration::CarIDFlag id) const { - return pimpl_->IsAllowedToFire(id); -} - const car_state::CarStateManager& StateMachine::GetState(enumeration::CarIDFlag single_id) const { return pimpl_->GetState(single_id); } diff --git a/src/tongji/state_machine/state_machine.hpp b/src/tongji/state_machine/state_machine.hpp index 4b6fa86..780fe07 100644 --- a/src/tongji/state_machine/state_machine.hpp +++ b/src/tongji/state_machine/state_machine.hpp @@ -15,7 +15,6 @@ class StateMachine final : public interfaces::ICarState { const enumeration ::CarIDFlag& GetAllowdToFires() const override; const car_state::CarStateManager& GetState(enumeration::CarIDFlag single_id) const; - bool IsAllowedToFire(enumeration::CarIDFlag id) const; const interfaces::ICarState& Update( const enumeration::CarIDFlag& car_detected, const std::time_t& now); From 08b7ebd390dbd4584d05135950ba03d8e2ceb40f Mon Sep 17 00:00:00 2001 From: heyeuu <2829004293@qq.com> Date: Wed, 8 Oct 2025 10:11:49 +0800 Subject: [PATCH 44/53] feat(fire_control):enhance CalculateTarget logic and add miss public interfaces declaration --- .../fire_controller/fire_controller.cpp | 56 ++++++++++--------- .../fire_controller/fire_controller.hpp | 13 +++-- src/tongji/fire_controller/tracker.hpp | 1 - src/tongji/predictor/live_target_manager.cpp | 32 +++++++---- src/tongji/predictor/live_target_manager.hpp | 6 ++ 5 files changed, 67 insertions(+), 41 deletions(-) diff --git a/src/tongji/fire_controller/fire_controller.cpp b/src/tongji/fire_controller/fire_controller.cpp index 43f859b..75d3b21 100644 --- a/src/tongji/fire_controller/fire_controller.cpp +++ b/src/tongji/fire_controller/fire_controller.cpp @@ -12,6 +12,7 @@ #include "tongji/fire_controller/fire_decision.hpp" #include "tongji/fire_controller/tracker.hpp" #include "tongji/identifier/identified_armor.hpp" +#include "tongji/predictor/live_target_manager.hpp" #include "tongji/predictor/target_snapshot_manager.hpp" #include "tongji/predictor/time_stamp.hpp" #include "tongji/state_machine/state_machine.hpp" @@ -22,6 +23,8 @@ using TargetSnapshotManager = predictor::TargetSnapshotManager; using StateMachine = state_machine::StateMachine; using IdentifiedArmor = identifier::IdentifiedArmor; using CarIDFlag = enumeration::CarIDFlag; +using LiveTargetManager = predictor::LiveTargetManager; +using TimeStamp = predictor::TimeStamp; class FireController::Impl { public: @@ -30,31 +33,30 @@ class FireController::Impl { : control_delay_(control_delay_in_second) , bullet_speed_(bullet_speed) , tracker_(std::make_unique()) - , snapshot_manager_(nullptr) // TODO:A new snapshot manager needs to be created based on the - // input of each frame of the camera , aim_solver_(std::make_unique(bullet_speed, yaw_offset, pitch_offset)) , fire_decision_(std::make_unique(auto_fire)) - , state_machine_(std::make_unique()) { } + , live_target_manager_(std::make_shared()) { } // TODO:std::time_t const data ::FireControl CalculateTarget(const std ::time_t& time_duration) const { - if (!identified_armors_ || !tracker_ || !snapshot_manager_ || !aim_solver_ - || !fire_decision_) + if (!identified_armors_ || !tracker_ || !aim_solver_ || !fire_decision_ + || !live_target_manager_) return { .fire_allowance = false }; + + auto snapshot_manager = std::dynamic_pointer_cast( + live_target_manager_->GetPredictor(live_target_manager_->GetLiveTargetIDs())); + auto snapshot = tracker_->SelectTrackingTarget(*identified_armors_, snapshot_manager); - auto snapshot = tracker_->SelectTrackingTarget(*identified_armors_, snapshot_manager_); - bool found = (snapshot != nullptr); + bool found = (snapshot != nullptr); tracker_->UpdateState(found); if (!found) return { .fire_allowance = false }; aim_solver_->UpdateSnapshot(std::move(snapshot)); - const auto& aim_solution = - aim_solver_->SolveAimSolution(static_cast(time_duration)); // TODO + const auto& aim_solution = aim_solver_->SolveAimSolution(static_cast( + time_duration)); // TODO:std::time_t should not be converted directly to double type if (!aim_solution.valid) return { .fire_allowance = false }; - state_machine_->Update(identified_armors_->GetDetectedIDs(), time_stamp_.GetTimeStamp()); - const Eigen::Vector3d target_pos = aim_solution.aim_point.head<3>(); const bool fireable = fire_decision_->ShouldFire(aim_solution, target_pos, gimbal_pos_); @@ -67,42 +69,46 @@ class FireController::Impl { const CarIDFlag GetAttackCarId() const { return attacked_cars_; } - void SetSnapshotManager(std::shared_ptr manager) { - snapshot_manager_ = std::move(manager); - } - - void UpdateArmors(const IdentifiedArmor& armors) { identified_armors_ = armors; } + void UpdateIdentifiedArmors(const IdentifiedArmor& armors) { identified_armors_ = armors; } void UpdateGimbalPosition(const Eigen::Vector3d& gimbal_pos) { gimbal_pos_ = gimbal_pos; }; void SetTimeStamp(const std::time_t& time_stamp) { time_stamp_.SetTimeStamp(time_stamp); } + TimeStamp GetTimeStamp() const { return time_stamp_; } private: Eigen::Vector3d gimbal_pos_ { Eigen::Vector3d::Zero() }; double control_delay_; double bullet_speed_; - mutable CarIDFlag attacked_cars_ { CarIDFlag::None }; // TODO + mutable CarIDFlag attacked_cars_ { + CarIDFlag::None + }; // TODO:Mutable should not be used to break const encapsulation mutable std::optional identified_armors_; std::unique_ptr tracker_; - std::shared_ptr snapshot_manager_; + std::shared_ptr live_target_manager_; std::unique_ptr aim_solver_; std::unique_ptr fire_decision_; - std::unique_ptr state_machine_; - predictor::TimeStamp time_stamp_ { 0 }; + predictor::TimeStamp time_stamp_ { std::time(nullptr) }; }; -FireController::FireController() - : pimpl_() { } +FireController::FireController(bool auto_fire, const double& control_delay_in_second, + const double& bullet_speed, double yaw_offset, double pitch_offset) + : pimpl_(std::make_unique( + auto_fire, control_delay_in_second, bullet_speed, yaw_offset, pitch_offset)) { } const data ::FireControl FireController::CalculateTarget(const std ::time_t& time_duration) const { return pimpl_->CalculateTarget(time_duration); } -void FireController::SetSnapshotManager(std::shared_ptr manager) { - pimpl_->SetSnapshotManager(std::move(manager)); +const CarIDFlag FireController::GetAttackCarId() const { return pimpl_->GetAttackCarId(); } + +void FireController::UpdateIdentifiedArmors(const IdentifiedArmor& armors) { + return pimpl_->UpdateIdentifiedArmors(armors); } -const CarIDFlag FireController::GetAttackCarId() const { return pimpl_->GetAttackCarId(); } +void FireController::UpdateGimbalPosition(const Eigen::Vector3d& gimbal_pos) { + return pimpl_->UpdateGimbalPosition(gimbal_pos); +}; } \ No newline at end of file diff --git a/src/tongji/fire_controller/fire_controller.hpp b/src/tongji/fire_controller/fire_controller.hpp index 8458a07..c5fedd4 100644 --- a/src/tongji/fire_controller/fire_controller.hpp +++ b/src/tongji/fire_controller/fire_controller.hpp @@ -3,19 +3,24 @@ #include #include "interfaces/fire_controller.hpp" -#include "tongji/predictor/target_snapshot_manager.hpp" +#include "tongji/identifier/identified_armor.hpp" +#include "tongji/predictor/time_stamp.hpp" namespace world_exe::tongji::fire_control { class FireController final : public interfaces::IFireControl { public: - FireController(); + FireController(bool auto_fire, const double& control_delay_in_second, + const double& bullet_speed, double yaw_offset, double pitch_offset); const data ::FireControl CalculateTarget(const std ::time_t& time_duration) const override; - const enumeration ::CarIDFlag GetAttackCarId() const override; - void SetSnapshotManager(std::shared_ptr manager); + void UpdateIdentifiedArmors(const identifier::IdentifiedArmor& armors); + void UpdateGimbalPosition(const Eigen::Vector3d& gimbal_pos); + + void SetTimeStamp(const std::time_t& time_stamp); + predictor::TimeStamp GetTimeStamp() const; private: class Impl; diff --git a/src/tongji/fire_controller/tracker.hpp b/src/tongji/fire_controller/tracker.hpp index 9e871e0..361a3da 100644 --- a/src/tongji/fire_controller/tracker.hpp +++ b/src/tongji/fire_controller/tracker.hpp @@ -51,7 +51,6 @@ class DefaultTracker final { }); sq_armor_list->sort([](const auto& a, const auto& b) { return a.priority < b.priority; }); - // auto iterator = sq_armor_list->begin(); auto filter_flag = snapshot_manager_->GetId(); for (const auto& armor : *sq_armor_list) { if (!enumeration::IsFlagContains(filter_flag, armor.armor.id)) continue; diff --git a/src/tongji/predictor/live_target_manager.cpp b/src/tongji/predictor/live_target_manager.cpp index 504f828..aaae1be 100644 --- a/src/tongji/predictor/live_target_manager.cpp +++ b/src/tongji/predictor/live_target_manager.cpp @@ -21,17 +21,6 @@ class LiveTargetManager::Impl { Impl(double timeout_sec = 0.1) : timeout_sec_(timeout_sec) { } - void RegisterTarget(enumeration::ArmorIdFlag id, std::shared_ptr target) { - targets_[id] = std::move(target); - } - - void RemoveTarget(enumeration::ArmorIdFlag id) { targets_.erase(id); } - bool HasTarget(enumeration::ArmorIdFlag id) const { return targets_.count(id) > 0; } - std::shared_ptr GetTarget(enumeration::ArmorIdFlag id) const { - auto it = targets_.find(id); - return (it != targets_.end()) ? it->second : nullptr; - } - std::shared_ptr Predict( const enumeration::ArmorIdFlag& flag, const std::time_t& time_stamp) { std::unordered_map> @@ -66,6 +55,7 @@ class LiveTargetManager::Impl { const auto now = predictor::TimeStamp(std::time(nullptr)); RemoveLostTargets(now); UpdateTargets(data, dt); + // TODO:update the state_machine } auto GetLiveTargetIDs() -> enumeration::CarIDFlag const { @@ -80,6 +70,16 @@ class LiveTargetManager::Impl { } private: + void RegisterTarget(enumeration::ArmorIdFlag id, std::shared_ptr target) { + targets_[id] = std::move(target); + } + void RemoveTarget(enumeration::ArmorIdFlag id) { targets_.erase(id); } + bool HasTarget(enumeration::ArmorIdFlag id) const { return targets_.count(id) > 0; } + std::shared_ptr GetTarget(enumeration::ArmorIdFlag id) const { + auto it = targets_.find(id); + return (it != targets_.end()) ? it->second : nullptr; + } + void UpdateTargets( const std::shared_ptr& data, double dt) { const Eigen::Affine3d transform = data->GetTransform(); @@ -145,4 +145,14 @@ std ::shared_ptr LiveTargetManager::GetPredictor( const enumeration ::ArmorIdFlag& id) const { return pimpl_->GetPredictor(id); } + +auto LiveTargetManager::GetLiveTargetIDs() -> enumeration::CarIDFlag const { + return pimpl_->GetLiveTargetIDs(); +} + +void LiveTargetManager::Update( + std::shared_ptr data, double dt) { + return pimpl_->Update(data, dt); +} + } \ No newline at end of file diff --git a/src/tongji/predictor/live_target_manager.hpp b/src/tongji/predictor/live_target_manager.hpp index 33937ab..f64af9f 100644 --- a/src/tongji/predictor/live_target_manager.hpp +++ b/src/tongji/predictor/live_target_manager.hpp @@ -3,6 +3,8 @@ #include #include "enum/armor_id.hpp" +#include "enum/car_id.hpp" +#include "interfaces/predictor_update_package.hpp" #include "interfaces/target_predictor.hpp" namespace world_exe::tongji::predictor { @@ -18,6 +20,10 @@ class LiveTargetManager final : public interfaces::ITargetPredictor { std ::shared_ptr GetPredictor( const enumeration ::ArmorIdFlag& id) const override; + auto GetLiveTargetIDs() -> enumeration::CarIDFlag const; + + void Update(std::shared_ptr data, double dt) ; + private: class Impl; std::unique_ptr pimpl_; From 7e3b7437316cf879a9998e1bf85ae5448cbdb5ce Mon Sep 17 00:00:00 2001 From: heyeuu <2829004293@qq.com> Date: Wed, 8 Oct 2025 11:50:49 +0800 Subject: [PATCH 45/53] refactor(state_machine): complete state machine update logic --- .../fire_controller/fire_controller.cpp | 18 ++++++++------- .../fire_controller/fire_controller.hpp | 6 +++-- src/tongji/predictor/live_target_manager.cpp | 23 +++++++++++++++---- src/tongji/predictor/live_target_manager.hpp | 6 +++-- 4 files changed, 36 insertions(+), 17 deletions(-) diff --git a/src/tongji/fire_controller/fire_controller.cpp b/src/tongji/fire_controller/fire_controller.cpp index 75d3b21..e065cb6 100644 --- a/src/tongji/fire_controller/fire_controller.cpp +++ b/src/tongji/fire_controller/fire_controller.cpp @@ -28,21 +28,22 @@ using TimeStamp = predictor::TimeStamp; class FireController::Impl { public: - Impl(bool auto_fire, const double& control_delay_in_second, const double& bullet_speed, - double yaw_offset, double pitch_offset) + Impl(std::shared_ptr state_machine, bool auto_fire, + const double& control_delay_in_second, const double& bullet_speed, double yaw_offset, + double pitch_offset) : control_delay_(control_delay_in_second) , bullet_speed_(bullet_speed) , tracker_(std::make_unique()) , aim_solver_(std::make_unique(bullet_speed, yaw_offset, pitch_offset)) , fire_decision_(std::make_unique(auto_fire)) - , live_target_manager_(std::make_shared()) { } + , live_target_manager_(std::make_shared(state_machine)) { } // TODO:std::time_t const data ::FireControl CalculateTarget(const std ::time_t& time_duration) const { if (!identified_armors_ || !tracker_ || !aim_solver_ || !fire_decision_ || !live_target_manager_) return { .fire_allowance = false }; - + auto snapshot_manager = std::dynamic_pointer_cast( live_target_manager_->GetPredictor(live_target_manager_->GetLiveTargetIDs())); auto snapshot = tracker_->SelectTrackingTarget(*identified_armors_, snapshot_manager); @@ -92,10 +93,11 @@ class FireController::Impl { predictor::TimeStamp time_stamp_ { std::time(nullptr) }; }; -FireController::FireController(bool auto_fire, const double& control_delay_in_second, - const double& bullet_speed, double yaw_offset, double pitch_offset) - : pimpl_(std::make_unique( - auto_fire, control_delay_in_second, bullet_speed, yaw_offset, pitch_offset)) { } +FireController::FireController(std::shared_ptr state_machine, bool auto_fire, + const double& control_delay_in_second, const double& bullet_speed, double yaw_offset, + double pitch_offset) + : pimpl_(std::make_unique(state_machine, auto_fire, control_delay_in_second, bullet_speed, + yaw_offset, pitch_offset)) { } const data ::FireControl FireController::CalculateTarget(const std ::time_t& time_duration) const { return pimpl_->CalculateTarget(time_duration); diff --git a/src/tongji/fire_controller/fire_controller.hpp b/src/tongji/fire_controller/fire_controller.hpp index c5fedd4..6258923 100644 --- a/src/tongji/fire_controller/fire_controller.hpp +++ b/src/tongji/fire_controller/fire_controller.hpp @@ -5,13 +5,15 @@ #include "interfaces/fire_controller.hpp" #include "tongji/identifier/identified_armor.hpp" #include "tongji/predictor/time_stamp.hpp" +#include "tongji/state_machine/state_machine.hpp" namespace world_exe::tongji::fire_control { class FireController final : public interfaces::IFireControl { public: - FireController(bool auto_fire, const double& control_delay_in_second, - const double& bullet_speed, double yaw_offset, double pitch_offset); + FireController(std::shared_ptr state_machine, bool auto_fire, + const double& control_delay_in_second, const double& bullet_speed, double yaw_offset, + double pitch_offset); const data ::FireControl CalculateTarget(const std ::time_t& time_duration) const override; const enumeration ::CarIDFlag GetAttackCarId() const override; diff --git a/src/tongji/predictor/live_target_manager.cpp b/src/tongji/predictor/live_target_manager.cpp index aaae1be..c5e6ba2 100644 --- a/src/tongji/predictor/live_target_manager.cpp +++ b/src/tongji/predictor/live_target_manager.cpp @@ -12,14 +12,17 @@ #include "tongji/predictor/in_gimbal_control_armor.hpp" #include "tongji/predictor/live_target.hpp" #include "tongji/predictor/target_snapshot_manager.hpp" +#include "tongji/predictor/time_stamp.hpp" +#include "tongji/state_machine/state_machine.hpp" #include "util/index.hpp" namespace world_exe::tongji::predictor { class LiveTargetManager::Impl { public: - Impl(double timeout_sec = 0.1) - : timeout_sec_(timeout_sec) { } + Impl(std::shared_ptr state_machine, double timeout_sec = 0.1) + : timeout_sec_(timeout_sec) + , state_machine_(state_machine) { } std::shared_ptr Predict( const enumeration::ArmorIdFlag& flag, const std::time_t& time_stamp) { @@ -54,7 +57,7 @@ class LiveTargetManager::Impl { void Update(std::shared_ptr data, double dt) { const auto now = predictor::TimeStamp(std::time(nullptr)); RemoveLostTargets(now); - UpdateTargets(data, dt); + AddNewTargets(data, dt); // TODO:update the state_machine } @@ -80,7 +83,7 @@ class LiveTargetManager::Impl { return (it != targets_.end()) ? it->second : nullptr; } - void UpdateTargets( + void AddNewTargets( const std::shared_ptr& data, double dt) { const Eigen::Affine3d transform = data->GetTransform(); const Eigen::Matrix3d rotation_matrix = transform.linear(); @@ -94,6 +97,8 @@ class LiveTargetManager::Impl { if (armors.empty()) continue; + UpdateStateMachine(id, std::time(nullptr)); + if (!HasTarget(id)) { const auto& armor = armors.front(); const Eigen::Vector3d position_in_world = transform * armor.position; @@ -101,6 +106,7 @@ class LiveTargetManager::Impl { auto target = std::make_shared(position_in_world, ypr_in_world, id); RegisterTarget(id, target); + continue; } auto target = GetTarget(id); @@ -124,16 +130,23 @@ class LiveTargetManager::Impl { } } + void UpdateStateMachine(enumeration::CarIDFlag cars_detected, TimeStamp now) { + state_machine_->Update(cars_detected, now.GetTimeStamp()); + } + bool IsTargetLost( const std::shared_ptr& target, const predictor::TimeStamp& now) const { return now.SecondsSince(target->LastSeen()) > timeout_sec_; } std::unordered_map> targets_; + std::shared_ptr state_machine_; const double timeout_sec_ { 0.1 }; }; -LiveTargetManager::LiveTargetManager() = default; +LiveTargetManager::LiveTargetManager( + std::shared_ptr state_machine, double timeout_sec) + : pimpl_(std::make_unique(state_machine, timeout_sec)) { } LiveTargetManager::~LiveTargetManager() = default; std ::shared_ptr LiveTargetManager::Predict( diff --git a/src/tongji/predictor/live_target_manager.hpp b/src/tongji/predictor/live_target_manager.hpp index f64af9f..7a1c978 100644 --- a/src/tongji/predictor/live_target_manager.hpp +++ b/src/tongji/predictor/live_target_manager.hpp @@ -6,12 +6,14 @@ #include "enum/car_id.hpp" #include "interfaces/predictor_update_package.hpp" #include "interfaces/target_predictor.hpp" +#include "tongji/state_machine/state_machine.hpp" namespace world_exe::tongji::predictor { class LiveTargetManager final : public interfaces::ITargetPredictor { public: - LiveTargetManager(); + LiveTargetManager( + std::shared_ptr state_machine, double timeout_sec = 0.1); ~LiveTargetManager(); std ::shared_ptr Predict( @@ -22,7 +24,7 @@ class LiveTargetManager final : public interfaces::ITargetPredictor { auto GetLiveTargetIDs() -> enumeration::CarIDFlag const; - void Update(std::shared_ptr data, double dt) ; + void Update(std::shared_ptr data, double dt); private: class Impl; From 010b46631be6d204256fccf670a762d617957887 Mon Sep 17 00:00:00 2001 From: heyeuu <2829004293@qq.com> Date: Wed, 8 Oct 2025 14:13:20 +0800 Subject: [PATCH 46/53] perf(filter): make Kalman filter type inference compile-time Replace runtime type checks with templates for better performance. --- .../predictor/extended_kalman_filter.hpp | 39 ++++--- src/tongji/predictor/live_target.hpp | 33 +++--- src/tongji/predictor/predict_model.hpp | 91 +++++++++------- src/tongji/predictor/target_snapshot.hpp | 4 +- src/util/extended_kalman_filter.hpp | 101 ------------------ 5 files changed, 96 insertions(+), 172 deletions(-) delete mode 100644 src/util/extended_kalman_filter.hpp diff --git a/src/tongji/predictor/extended_kalman_filter.hpp b/src/tongji/predictor/extended_kalman_filter.hpp index 32450db..f016491 100644 --- a/src/tongji/predictor/extended_kalman_filter.hpp +++ b/src/tongji/predictor/extended_kalman_filter.hpp @@ -7,17 +7,29 @@ #include namespace world_exe::tongji::predictor { +template // class ExtendedKalmanFilter { public: - Eigen::VectorXd x; - Eigen::MatrixXd P; + using XVec = Eigen::Matrix; + using ZVec = Eigen::Matrix; + using AMat = Eigen::Matrix; + using PMat = Eigen::Matrix; + using PDig = Eigen::Matrix; + using RMat = Eigen::Matrix; + using RDig = Eigen::Matrix; + using QMat = Eigen::Matrix; + using HMat = Eigen::Matrix; + + XVec x; + PMat P; ExtendedKalmanFilter() = default; ExtendedKalmanFilter( - const Eigen::VectorXd& x0, const Eigen::MatrixXd& P0, - std::function x_add = - [](const Eigen::VectorXd& a, const Eigen::VectorXd& b) { return a + b; }) { + const XVec& x0, const PMat& P0, + std::function x_add = [](const XVec& a, const XVec& b) { + return a + b; + }) { data["residual_yaw"] = 0.0; data["residual_pitch"] = 0.0; data["residual_distance"] = 0.0; @@ -29,13 +41,12 @@ class ExtendedKalmanFilter { data["recent_nis_failures"] = 0.0; } - Eigen::VectorXd Update( - const double& dt, const Eigen::MatrixXd& A, const Eigen::MatrixXd& Q, - std::function f, - const Eigen::VectorXd& z, const Eigen::MatrixXd& H, const Eigen::MatrixXd& R, - std::function h, - std::function z_subtract = - [](const Eigen::VectorXd& a, const Eigen::VectorXd& b) { return a - b; }) { + XVec Update( + const double& dt, const AMat& A, const QMat& Q, + std::function f, const ZVec& z, const HMat& H, + const RMat& R, std::function h, + std::function z_subtract = + [](const ZVec& a, const ZVec& b) { return a - b; }) { auto x_n = f(x, dt); @@ -90,8 +101,8 @@ class ExtendedKalmanFilter { double last_nis; private: - Eigen::MatrixXd I; - std::function x_add; + Eigen::Matrix I; + std::function x_add; int nees_count_ = 0; int nis_count_ = 0; diff --git a/src/tongji/predictor/live_target.hpp b/src/tongji/predictor/live_target.hpp index e0dc168..d25bfdb 100644 --- a/src/tongji/predictor/live_target.hpp +++ b/src/tongji/predictor/live_target.hpp @@ -1,16 +1,15 @@ #pragma once #include -#include #include #include #include #include "data/armor_gimbal_control_spacing.hpp" #include "enum/car_id.hpp" +#include "extended_kalman_filter.hpp" #include "predict_model.hpp" #include "tongji/predictor/time_stamp.hpp" -#include "util/extended_kalman_filter.hpp" namespace world_exe::tongji::predictor { @@ -32,17 +31,17 @@ class LiveTarget { armor_xyz_in_world[1] + model_.GetRadius() * std::sin(armor_ypr_in_world[0]); auto center_z = armor_xyz_in_world[2]; - Eigen::VectorXd x0(11); + ExtendedKalmanFilter<11, 4>::XVec x0; x0 << center_x, 0, center_y, 0, center_z, 0, armor_ypr_in_world[0], 0, model_.GetRadius(), 0, 0; - Eigen::MatrixXd P0 = model_.GetP0Dig().asDiagonal(); - ekf_ = util::ExtendedKalmanFilter( + ExtendedKalmanFilter<11, 4>::PMat P0 = model_.GetP0Dig().asDiagonal(); + ekf_ = ExtendedKalmanFilter<11, 4>( x0, P0, model_.x_add); // 初始化滤波器（预测量、预测量协方差） } - Eigen::VectorXd GetEkfX() const { return ekf_.x; } - Eigen::VectorXd GetP0Dig() const { return model_.GetP0Dig(); } + ExtendedKalmanFilter<11, 4>::XVec GetEkfX() const { return ekf_.x; } + ExtendedKalmanFilter<11, 4>::PDig GetP0Dig() const { return model_.GetP0Dig(); } const PredictModel& GetModel() const { return model_; } predictor::TimeStamp LastSeen() const { return predictor::TimeStamp(last_see_time_stamp_); } @@ -104,19 +103,19 @@ class LiveTarget { const Eigen::Vector3d& armor_ypr_in_world, const Eigen::Vector3d& armor_ypd_in_world, const int& id, const double& dt) { // 观测jacobi - auto H = model_.H(ekf_.x, id); - auto R = model_.R(armor_xyz_in_world, armor_ypr_in_world, armor_ypd_in_world, id); - auto A = model_.A(dt); - auto Q = model_.Q(dt); - auto f = model_.f; - auto h = [this, id](const Eigen::VectorXd& x) { return model_.h(x, id); }; + auto H = model_.H(ekf_.x, id); + auto R = model_.R(armor_xyz_in_world, armor_ypr_in_world, armor_ypd_in_world, id); + auto A = model_.A(dt); + auto Q = model_.Q(dt); + auto f = model_.f; + auto h = [this, id](const ExtendedKalmanFilter<11, 4>::XVec& x) { return model_.h(x, id); }; auto z_subtract = model_.z_subtract; - const Eigen::VectorXd& ypd = armor_ypd_in_world; - const Eigen::VectorXd& ypr = armor_ypr_in_world; + const Eigen::Vector3d& ypd = armor_ypd_in_world; + const Eigen::Vector3d& ypr = armor_ypr_in_world; // 获得观测量 - Eigen::VectorXd z(4); + ExtendedKalmanFilter<11, 4>::ZVec z(4); z << ypd[0], ypd[1], ypd[2], ypr[0]; ekf_.Update(dt, A, Q, f, z, H, R, h, z_subtract); @@ -131,7 +130,7 @@ class LiveTarget { } std::time_t last_see_time_stamp_; - util::ExtendedKalmanFilter ekf_; + ExtendedKalmanFilter<11, 4> ekf_; PredictModel model_; int last_id = -1; diff --git a/src/tongji/predictor/predict_model.hpp b/src/tongji/predictor/predict_model.hpp index 109fee0..a9942a4 100644 --- a/src/tongji/predictor/predict_model.hpp +++ b/src/tongji/predictor/predict_model.hpp @@ -2,10 +2,12 @@ #include +#include #include #include #include "enum/car_id.hpp" +#include "tongji/predictor/extended_kalman_filter.hpp" #include "util/math.hpp" namespace world_exe::tongji::predictor { @@ -57,23 +59,27 @@ class PredictModel { int GetArmorNum() const { return armor_num_; } // 防止夹角求和出现异常值 - std::function x_add = - [](const Eigen::VectorXd& a, const Eigen::VectorXd& b) { - Eigen::VectorXd c = a + b; - c(6) = util::math::clamp_pm_pi(c(6)); + std::function::XVec( + const ExtendedKalmanFilter<11, 4>::XVec&, const ExtendedKalmanFilter<11, 4>::XVec&)> + x_add = [](const ExtendedKalmanFilter<11, 4>::XVec& a, + const ExtendedKalmanFilter<11, 4>::XVec& b) { + ExtendedKalmanFilter<11, 4>::XVec c = a + b; + c(6) = util::math::clamp_pm_pi(c(6)); return c; }; // 防止夹角求和出现异常值 - std::function f = - [=, this](const Eigen::VectorXd& x, const double& dt) -> Eigen::VectorXd { - Eigen::VectorXd x_prior = A(dt) * x; - x_prior(6) = util::math::clamp_pm_pi(x_prior(6)); + std::function::XVec( + const ExtendedKalmanFilter<11, 4>::XVec& x, const double& dt)> + f = [this](const ExtendedKalmanFilter<11, 4>::XVec& x, + const double& dt) -> ExtendedKalmanFilter<11, 4>::XVec { + ExtendedKalmanFilter<11, 4>::XVec x_prior = A(dt) * x; + x_prior(6) = util::math::clamp_pm_pi(x_prior(6)); return x_prior; }; - int MatchArmor(const Eigen::VectorXd& x, const Eigen::Vector3d& armor_xyz_in_world, - const Eigen::Vector3d& armor_ypr_in_world, + int MatchArmor(const ExtendedKalmanFilter<11, 4>::XVec& x, + const Eigen::Vector3d& armor_xyz_in_world, const Eigen::Vector3d& armor_ypr_in_world, const Eigen::Vector3d& armor_ypd_in_world) const { const auto& xyza_list = GetArmorXYZAList(x); @@ -107,7 +113,7 @@ class PredictModel { } // 计算出装甲板中心的坐标（考虑长短轴） - Eigen::Vector3d h_armor_xyz(const Eigen::VectorXd& x, int id) const { + Eigen::Vector3d h_armor_xyz(const ExtendedKalmanFilter<11, 4>::XVec& x, int id) const { auto angle = util::math::clamp_pm_pi(x(6) + id * 2 * CV_PI / armor_num_); auto use_l_h = (armor_num_ == 4) && (id == 1 || id == 3); @@ -119,7 +125,7 @@ class PredictModel { return { armor_x, armor_y, armor_z }; } - Eigen::MatrixXd H(const Eigen::VectorXd& x, int id) const { + ExtendedKalmanFilter<11, 4>::HMat H(const ExtendedKalmanFilter<11, 4>::XVec& x, int id) const { auto angle = util::math::clamp_pm_pi(x(6) + id * 2 * CV_PI / armor_num_); auto use_l_h = (armor_num_ == 4) && (id == 1 || id == 3); @@ -135,7 +141,7 @@ class PredictModel { auto dz_dh = (use_l_h) ? 1.0 : 0.0; // clang-format off - Eigen::MatrixXd H_armor_xyza{ + Eigen::MatrixXd H_armor_xyza{ {1, 0, 0, 0, 0, 0, dx_da, 0, dx_dr, dx_dl, 0}, {0, 0, 1, 0, 0, 0, dy_da, 0, dy_dr, dy_dl, 0}, {0, 0, 0, 0, 1, 0, 0, 0, 0, 0, dz_dh}, @@ -146,7 +152,7 @@ class PredictModel { Eigen::VectorXd armor_xyz = h_armor_xyz(x, id); Eigen::MatrixXd H_armor_ypd = util::math::xyz2ypd_jacobian(armor_xyz); // clang-format off - Eigen::MatrixXd H_armor_ypda{ + Eigen::Matrix H_armor_ypda{ {H_armor_ypd(0, 0), H_armor_ypd(0, 1), H_armor_ypd(0, 2), 0}, {H_armor_ypd(1, 0), H_armor_ypd(1, 1), H_armor_ypd(1, 2), 0}, {H_armor_ypd(2, 0), H_armor_ypd(2, 1), H_armor_ypd(2, 2), 0}, @@ -157,13 +163,13 @@ class PredictModel { return H_armor_ypda * H_armor_xyza; } - Eigen::MatrixXd R(const Eigen::Vector3d& armor_xyz_in_world, + ExtendedKalmanFilter<11, 4>::RMat R(const Eigen::Vector3d& armor_xyz_in_world, const Eigen::Vector3d& armor_ypr_in_world, const Eigen::Vector3d& armor_ypd_in_world, int id) { // Eigen::VectorXd R_dig{{4e-3, 4e-3, 1, 9e-2}}; auto center_yaw = std::atan2(armor_xyz_in_world(1), armor_xyz_in_world(0)); auto delta_angle = util::math::clamp_pm_pi(armor_ypr_in_world(0) - center_yaw); - Eigen::VectorXd R_dig(4); + ExtendedKalmanFilter<11, 4>::RDig R_dig(4); R_dig << 4e-3, 4e-3, log(std::abs(delta_angle) + 1) + 1, log(std::abs(armor_ypd_in_world(2)) + 1) / 200 + 9e-2; @@ -171,10 +177,11 @@ class PredictModel { return R_dig.asDiagonal(); } - Eigen::MatrixXd A(double dt) const { + ExtendedKalmanFilter<11, 4>::AMat A(double dt) const { // 状态转移矩阵 + ExtendedKalmanFilter<11, 4>::AMat _A; // clang-format off - return (Eigen::MatrixXd(11, 11) << + _A<< 1, dt, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, dt, 0, 0, 0, 0, 0, 0, 0, @@ -185,11 +192,13 @@ class PredictModel { 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, - 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1).finished(); + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1; + //clang-format on + return _A; } - //clang-format on + - Eigen::MatrixXd Q(double dt) const { +ExtendedKalmanFilter<11, 4>::QMat Q(double dt) const { // Piecewise White Noise Model // https://github.com/rlabbe/Kalman-and-Bayesian-Filters-in-Python/blob/master/07-Kalman-Filter-Math.ipynb double v1, v2; @@ -205,8 +214,10 @@ class PredictModel { auto c = dt * dt; // 预测过程噪声偏差的方差 + Eigen::Matrix _Q; // clang-format off - return (Eigen::MatrixXd(11, 11) << + _Q + << a * v1, b * v1, 0, 0, 0, 0, 0, 0, 0, 0, 0, b * v1, c * v1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, a * v1, b * v1, 0, 0, 0, 0, 0, 0, 0, @@ -217,29 +228,33 @@ class PredictModel { 0, 0, 0, 0, 0, 0, b * v2, c * v2, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, - 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0).finished(); + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0; // clang-format on + return _Q; } - std::function h = [this](const Eigen::VectorXd& x, - int id) { - Eigen::Vector3d xyz = this->h_armor_xyz(x, id); - Eigen::Vector3d ypd = util::math::xyz2ypd(xyz); - double angle = util::math::clamp_pm_pi(x(6) + id * 2 * CV_PI / this->armor_num_); - return Eigen::Vector4d { ypd(0), ypd(1), ypd(2), angle }; - }; + std::function::ZVec(const ExtendedKalmanFilter<11, 4>::XVec&, int)> + h = [this](const ExtendedKalmanFilter<11, 4>::XVec& x, int id) { + Eigen::Vector3d xyz = this->h_armor_xyz(x, id); + Eigen::Vector3d ypd = util::math::xyz2ypd(xyz); + double angle = util::math::clamp_pm_pi(x(6) + id * 2 * CV_PI / this->armor_num_); + return Eigen::Vector4d { ypd(0), ypd(1), ypd(2), angle }; + }; // 防止夹角求差出现异常值 - std::function z_subtract = - [](const Eigen::VectorXd& a, const Eigen::VectorXd& b) { - Eigen::VectorXd c = a - b; - c(0) = util::math::clamp_pm_pi(c(0)); - c(1) = util::math::clamp_pm_pi(c(1)); - c(3) = util::math::clamp_pm_pi(c(3)); + std::function::ZVec( + const ExtendedKalmanFilter<11, 4>::ZVec&, const ExtendedKalmanFilter<11, 4>::ZVec&)> + z_subtract = [](const ExtendedKalmanFilter<11, 4>::ZVec& a, + const ExtendedKalmanFilter<11, 4>::ZVec& b) { + ExtendedKalmanFilter<11, 4>::ZVec c = a - b; + c(0) = util::math::clamp_pm_pi(c(0)); + c(1) = util::math::clamp_pm_pi(c(1)); + c(3) = util::math::clamp_pm_pi(c(3)); return c; }; - std::vector GetArmorXYZAList(const Eigen::VectorXd& ekf_x) const { + std::vector GetArmorXYZAList( + const ExtendedKalmanFilter<11, 4>::XVec& ekf_x) const { std::vector _armor_xyza_list; for (int i = 0; i < armor_num_; i++) { @@ -255,7 +270,7 @@ class PredictModel { int armor_num_; enumeration::CarIDFlag car_id_; - Eigen::VectorXd P0_dig_; + ExtendedKalmanFilter<11, 4>::PDig P0_dig_; double radius_; }; diff --git a/src/tongji/predictor/target_snapshot.hpp b/src/tongji/predictor/target_snapshot.hpp index b390a36..ffe37b6 100644 --- a/src/tongji/predictor/target_snapshot.hpp +++ b/src/tongji/predictor/target_snapshot.hpp @@ -3,10 +3,10 @@ #include #include "data/armor_gimbal_control_spacing.hpp" +#include "extended_kalman_filter.hpp" #include "tongji/predictor/live_target.hpp" #include "tongji/predictor/predict_model.hpp" #include "tongji/predictor/time_stamp.hpp" -#include "util/extended_kalman_filter.hpp" namespace world_exe::tongji::predictor { @@ -49,7 +49,7 @@ class TargetSnapshot { private: PredictModel model_; - util::ExtendedKalmanFilter ekf_; + ExtendedKalmanFilter<11, 4> ekf_; TimeStamp time_stamp_; }; diff --git a/src/util/extended_kalman_filter.hpp b/src/util/extended_kalman_filter.hpp deleted file mode 100644 index 73fc86b..0000000 --- a/src/util/extended_kalman_filter.hpp +++ /dev/null @@ -1,101 +0,0 @@ -#pragma once - -#include -#include -#include -#include -#include - -namespace world_exe::util { -class ExtendedKalmanFilter { -public: - Eigen::VectorXd x; - Eigen::MatrixXd P; - - ExtendedKalmanFilter() = default; - - ExtendedKalmanFilter( - const Eigen::VectorXd& x0, const Eigen::MatrixXd& P0, - std::function x_add = - [](const Eigen::VectorXd& a, const Eigen::VectorXd& b) { return a + b; }) { - data["residual_yaw"] = 0.0; - data["residual_pitch"] = 0.0; - data["residual_distance"] = 0.0; - data["residual_angle"] = 0.0; - data["nis"] = 0.0; - data["nees"] = 0.0; - data["nis_fail"] = 0.0; - data["nees_fail"] = 0.0; - data["recent_nis_failures"] = 0.0; - } - - Eigen::VectorXd Update( - const double& dt, const Eigen::MatrixXd& A, const Eigen::MatrixXd& Q, - std::function f, - const Eigen::VectorXd& z, const Eigen::MatrixXd& H, const Eigen::MatrixXd& R, - std::function h, - std::function z_subtract = - [](const Eigen::VectorXd& a, const Eigen::VectorXd& b) { return a - b; }) { - - auto x_n = f(x, dt); - - auto P_n = A * P * A.transpose() + Q; - - auto residual = z_subtract(z, h(x_n)); - - auto S = H * P * H.transpose() + R; - - auto K = P_n * H.transpose() * S.inverse(); - - x = x_add(x, K * residual); - - // Stable Compution of the Posterior Covariance - // https://github.com/rlabbe/Kalman-and-Bayesian-Filters-in-Python/blob/master/07-Kalman-Filter-Math.ipynb - P = (I - K * H) * P * (I - K * H).transpose() + K * R * K.transpose(); - - /// 卡方检验 - // 新增检验 - double nis = residual.transpose() * S.inverse() * residual; - - // 卡方检验阈值（自由度=4，取置信水平95%） - constexpr double nis_threshold = 0.711; - - if (nis > nis_threshold) nis_count_++, data["nis_fail"] = 1; - total_count_++; - last_nis = nis; - - recent_nis_failures.push_back(nis > nis_threshold ? 1 : 0); - - if (recent_nis_failures.size() > window_size) { - recent_nis_failures.pop_front(); - } - - int recent_failures = - std::accumulate(recent_nis_failures.begin(), recent_nis_failures.end(), 0); - double recent_rate = static_cast(recent_failures) / recent_nis_failures.size(); - - data["residual_yaw"] = residual[0]; - data["residual_pitch"] = residual[1]; - data["residual_distance"] = residual[2]; - data["residual_angle"] = residual[3]; - data["nis"] = nis; - data["recent_nis_failures"] = recent_rate; - - return x; - } - - std::map data; // 卡方检验数据 - std::deque recent_nis_failures { 0 }; - size_t window_size = 100; - double last_nis; - -private: - Eigen::MatrixXd I; - std::function x_add; - - int nees_count_ = 0; - int nis_count_ = 0; - int total_count_ = 0; -}; - -} // namespace tools From ab6f47abac2572dc1caf8db014727119bc8a85d6 Mon Sep 17 00:00:00 2001 From: heyeuu <2829004293@qq.com> Date: Thu, 9 Oct 2025 23:09:13 +0800 Subject: [PATCH 47/53] feat(decider): implement priority assessment logic --- src/tongji/decider/armor_info.hpp | 26 ++++ src/tongji/decider/decider.cpp | 131 ++++++++++++++++++ src/tongji/decider/decider.hpp | 30 ++++ .../fire_controller/fire_controller.cpp | 15 +- .../fire_controller/fire_controller.hpp | 4 +- src/tongji/fire_controller/tracker.hpp | 98 +++++++------ src/tongji/identifier/identified_armor.hpp | 22 +-- src/tongji/predictor/predict_model.hpp | 4 - src/tongji/predictor/target_snapshot.hpp | 1 - .../state_machine/car_state_manager.hpp | 13 +- 10 files changed, 265 insertions(+), 79 deletions(-) create mode 100644 src/tongji/decider/armor_info.hpp create mode 100644 src/tongji/decider/decider.cpp create mode 100644 src/tongji/decider/decider.hpp diff --git a/src/tongji/decider/armor_info.hpp b/src/tongji/decider/armor_info.hpp new file mode 100644 index 0000000..a5abb07 --- /dev/null +++ b/src/tongji/decider/armor_info.hpp @@ -0,0 +1,26 @@ + + +#pragma once + +#include "data/armor_image_spaceing.hpp" + +namespace world_exe::tongji::decider { + +enum class ArmorPriority { + First = 1, // + Second, // + Third, // + Forth, // + Fifth // +}; + +struct ArmorInfo { + ArmorInfo(const data::ArmorImageSpacing& armor_spacing, + const ArmorPriority& priority = ArmorPriority::Fifth) + : armor_spacing(armor_spacing) + , priority(priority) { } + + data::ArmorImageSpacing armor_spacing; + ArmorPriority priority; +}; +} \ No newline at end of file diff --git a/src/tongji/decider/decider.cpp b/src/tongji/decider/decider.cpp new file mode 100644 index 0000000..14e338d --- /dev/null +++ b/src/tongji/decider/decider.cpp @@ -0,0 +1,131 @@ +#include "decider.hpp" + +#include +#include +#include +#include +#include +#include + +#include "enum/armor_id.hpp" +#include "enum/car_id.hpp" +#include "tongji/decider/armor_info.hpp" +#include "util/index.hpp" + +namespace world_exe::tongji::decider { + +class Decider::Impl { +public: + explicit Impl(PriorityMode mode = PriorityMode::MODE_ONE) + : mode_(mode) { } + + void SetInvincibleArmors(const enumeration::ArmorIdFlag& armors) { + invincible_armor_.clear(); + for (const auto id : util::enumeration::ExpandArmorIdFlags(armors)) { + invincible_armor_.emplace(std::move(armors)); + } + } + + void SetPriority(const std::vector& detected_result) const { + const PriorityMap& priority_map = (mode_ == PriorityMode::MODE_ONE) ? mode1 : mode2; + if (!detected_result.empty()) { + for (auto armor : detected_result) { + armor.priority = priority_map.at(armor.armor_spacing.id); + } + } + } + + enumeration::ArmorIdFlag GetSortedArmor(std::vector& armors) const { + if (armors.empty()) return enumeration::ArmorIdFlag::None; + + // 对每个 DetectionResult 调用 armor_filter 和 set_priority + // ArmorFilter(armors); + // SetPriority(armors); + + // 对每个 DetectionResult 中的 armors 进行排序 + cv::Point2d img_center(1440.0 / 2, 1080.0 / 2); // TODO + std::sort(armors.begin(), armors.end(), [&](const auto& a, const auto& b) { + auto center_a = + (a.armor_spacing.image_points[0] + a.armor_spacing.image_points[3]) * 0.5f; + auto center_b = + (b.armor_spacing.image_points[0] + b.armor_spacing.image_points[3]) * 0.5f; + return cv::norm(center_a - img_center) < cv::norm(center_b - img_center); + }); + + std::sort(armors.begin(), armors.end(), + [](const auto& a, const auto& b) { return a.priority < b.priority; }); + return armors.front().armor_spacing.id; + } + + bool ArmorFilter(std::vector& armors) const { + if (armors.empty()) return true; + + // 25赛季没有5号装甲板 + armors.erase(std::remove_if(armors.begin(), armors.end(), + [](const auto& armor) { return armor.armor_spacing.id == ArmorId::InfantryV; })); + // 不打前哨站 + armors.erase(std::remove_if(armors.begin(), armors.end(), + [](const auto& armor) { return armor.armor_spacing.id == ArmorId::Outpost; })); + // 过滤掉刚复活无敌的装甲板 + armors.erase( + std::remove_if(armors.begin(), armors.end(), + [&](const auto& armor) { return invincible_armor_.count(armor.armor_spacing.id); }), + armors.end()); + + return armors.empty(); + } + +private: + using ArmorPriority = world_exe::tongji::decider::ArmorPriority; + using ArmorId = world_exe::enumeration::ArmorIdFlag; + using ArmorInfo = world_exe::tongji::decider::ArmorInfo; + + using PriorityMap = std::unordered_map; + + const PriorityMap mode1 = { + { ArmorId::Hero, ArmorPriority::Second }, // + { ArmorId::Engineer, ArmorPriority::Forth }, // + { ArmorId::InfantryIII, ArmorPriority::First }, // + { ArmorId::InfantryIV, ArmorPriority::First }, // + { ArmorId::InfantryV, ArmorPriority::Third }, // + { ArmorId::Sentry, ArmorPriority::Third }, // + { ArmorId::Outpost, ArmorPriority::Fifth }, // + { ArmorId::Base, ArmorPriority::Fifth }, // + { ArmorId::Unknow, ArmorPriority::Fifth } // + }; + + const PriorityMap mode2 = { + { ArmorId::Hero, ArmorPriority::Second }, // + { ArmorId::Engineer, ArmorPriority::Forth }, // + { ArmorId::InfantryIII, ArmorPriority::First }, // + { ArmorId::InfantryIV, ArmorPriority::First }, // + { ArmorId::InfantryV, ArmorPriority::Third }, // + { ArmorId::Sentry, ArmorPriority::Third }, // + { ArmorId::Outpost, ArmorPriority::Fifth }, // + { ArmorId::Base, ArmorPriority::Fifth }, // + { ArmorId::Unknow, ArmorPriority::Fifth } // + }; + + std::unordered_set invincible_armor_; + PriorityMode mode_; +}; + +Decider::Decider(PriorityMode mode) + : pimpl_(std::make_unique(mode)) { } + +void Decider::SetInvincibleArmors(const enumeration::ArmorIdFlag& armors) { + return pimpl_->SetInvincibleArmors(armors); +} + +void Decider::SetPriority(const std::vector& detected_result) const { + return pimpl_->SetPriority(detected_result); +} + +bool Decider::ArmorFilter(std::vector& armors) { return pimpl_->ArmorFilter(armors); } + +enumeration::CarIDFlag Decider::GetSortedArmor(std::vector& armors) const { + return pimpl_->GetSortedArmor(armors); +} + +Decider::~Decider() = default; +} diff --git a/src/tongji/decider/decider.hpp b/src/tongji/decider/decider.hpp new file mode 100644 index 0000000..9717d04 --- /dev/null +++ b/src/tongji/decider/decider.hpp @@ -0,0 +1,30 @@ + + +#pragma once + +#include +#include + +#include "enum/armor_id.hpp" +#include "tongji/decider/armor_info.hpp" + +namespace world_exe::tongji::decider { + +enum PriorityMode { MODE_ONE = 1, MODE_TWO }; + +class Decider { +public: + Decider(PriorityMode mode = PriorityMode::MODE_ONE); + ~Decider(); + + void SetInvincibleArmors(const enumeration::ArmorIdFlag& armors); + void SetPriority(const std::vector& detected_result) const; + enumeration::ArmorIdFlag GetSortedArmor(std::vector& armors) const; + bool ArmorFilter(std::vector& armors); + +private: + class Impl; + std::unique_ptr pimpl_; +}; + +} \ No newline at end of file diff --git a/src/tongji/fire_controller/fire_controller.cpp b/src/tongji/fire_controller/fire_controller.cpp index e065cb6..29a14f2 100644 --- a/src/tongji/fire_controller/fire_controller.cpp +++ b/src/tongji/fire_controller/fire_controller.cpp @@ -8,6 +8,7 @@ #include #include "enum/car_id.hpp" +#include "interfaces/armor_in_image.hpp" #include "tongji/fire_controller/aim_solver.hpp" #include "tongji/fire_controller/fire_decision.hpp" #include "tongji/fire_controller/tracker.hpp" @@ -33,7 +34,7 @@ class FireController::Impl { double pitch_offset) : control_delay_(control_delay_in_second) , bullet_speed_(bullet_speed) - , tracker_(std::make_unique()) + , tracker_(std::make_unique()) , aim_solver_(std::make_unique(bullet_speed, yaw_offset, pitch_offset)) , fire_decision_(std::make_unique(auto_fire)) , live_target_manager_(std::make_shared(state_machine)) { } @@ -46,7 +47,7 @@ class FireController::Impl { auto snapshot_manager = std::dynamic_pointer_cast( live_target_manager_->GetPredictor(live_target_manager_->GetLiveTargetIDs())); - auto snapshot = tracker_->SelectTrackingTarget(*identified_armors_, snapshot_manager); + auto snapshot = tracker_->SelectTrackingTarget(identified_armors_, snapshot_manager); bool found = (snapshot != nullptr); tracker_->UpdateState(found); @@ -70,7 +71,9 @@ class FireController::Impl { const CarIDFlag GetAttackCarId() const { return attacked_cars_; } - void UpdateIdentifiedArmors(const IdentifiedArmor& armors) { identified_armors_ = armors; } + void UpdateIdentifiedArmors(std::shared_ptr armors) { + identified_armors_ = armors; + } void UpdateGimbalPosition(const Eigen::Vector3d& gimbal_pos) { gimbal_pos_ = gimbal_pos; }; void SetTimeStamp(const std::time_t& time_stamp) { time_stamp_.SetTimeStamp(time_stamp); } TimeStamp GetTimeStamp() const { return time_stamp_; } @@ -84,8 +87,8 @@ class FireController::Impl { CarIDFlag::None }; // TODO:Mutable should not be used to break const encapsulation - mutable std::optional identified_armors_; - std::unique_ptr tracker_; + std::shared_ptr identified_armors_; + std::unique_ptr tracker_; std::shared_ptr live_target_manager_; std::unique_ptr aim_solver_; std::unique_ptr fire_decision_; @@ -105,7 +108,7 @@ const data ::FireControl FireController::CalculateTarget(const std ::time_t& tim const CarIDFlag FireController::GetAttackCarId() const { return pimpl_->GetAttackCarId(); } -void FireController::UpdateIdentifiedArmors(const IdentifiedArmor& armors) { +void FireController::UpdateIdentifiedArmors(std::shared_ptr armors) { return pimpl_->UpdateIdentifiedArmors(armors); } diff --git a/src/tongji/fire_controller/fire_controller.hpp b/src/tongji/fire_controller/fire_controller.hpp index 6258923..bbbe7a0 100644 --- a/src/tongji/fire_controller/fire_controller.hpp +++ b/src/tongji/fire_controller/fire_controller.hpp @@ -2,8 +2,8 @@ #include +#include "interfaces/armor_in_image.hpp" #include "interfaces/fire_controller.hpp" -#include "tongji/identifier/identified_armor.hpp" #include "tongji/predictor/time_stamp.hpp" #include "tongji/state_machine/state_machine.hpp" @@ -18,7 +18,7 @@ class FireController final : public interfaces::IFireControl { const data ::FireControl CalculateTarget(const std ::time_t& time_duration) const override; const enumeration ::CarIDFlag GetAttackCarId() const override; - void UpdateIdentifiedArmors(const identifier::IdentifiedArmor& armors); + void UpdateIdentifiedArmors(std::shared_ptr armors); void UpdateGimbalPosition(const Eigen::Vector3d& gimbal_pos); void SetTimeStamp(const std::time_t& time_stamp); diff --git a/src/tongji/fire_controller/tracker.hpp b/src/tongji/fire_controller/tracker.hpp index 361a3da..57b9920 100644 --- a/src/tongji/fire_controller/tracker.hpp +++ b/src/tongji/fire_controller/tracker.hpp @@ -1,15 +1,21 @@ #pragma once +#include #include #include +#include #include "../predictor/target_snapshot.hpp" #include "../predictor/target_snapshot_manager.hpp" #include "enum/armor_id.hpp" #include "enum/car_id.hpp" #include "enum/enum_tools.hpp" +#include "interfaces/armor_in_image.hpp" #include "interfaces/car_state.hpp" +#include "tongji/decider/armor_info.hpp" +#include "tongji/decider/decider.hpp" +#include "tongji/fire_controller/aim_point_chooser.hpp" #include "tongji/identifier/identified_armor.hpp" #include "tongji/predictor/time_stamp.hpp" @@ -23,62 +29,75 @@ enum class TrackState { Switching // }; -class DefaultTracker final { +class Tracker final { using TargetSnapshotManager = world_exe::tongji::predictor::TargetSnapshotManager; using TargetSnapshot = world_exe::tongji::predictor::TargetSnapshot; using ArmorInImage = world_exe::tongji::identifier::IdentifiedArmor; using EnemiesState = world_exe::interfaces::ICarState; public: - DefaultTracker() - : last_timestamp_(0) { } + Tracker() + : last_timestamp_(std::time(nullptr)) + , decider_(std::make_unique()) { } - ~DefaultTracker() = default; + ~Tracker() = default; - auto SelectTrackingTarget( // - ArmorInImage& armors, // - const std::shared_ptr& snapshot_manager_ // - ) noexcept -> std::unique_ptr { + auto SelectTrackingTarget(std::shared_ptr armors_in_image, + const std::shared_ptr& snapshot_manager_) noexcept + -> std::unique_ptr { - const auto& sq_armor_list = armors.get_sq_armor(); + auto detected_ids = enumeration::CarIDFlag::None; + auto armor_info_list = std::vector { }; - sq_armor_list->sort([](const world_exe::tongji::identifier::SPArmor& a, - const world_exe::tongji::identifier::SPArmor& b) { - cv::Point2f img_center(1440.0 / 2, 1080.0 / 2); // TODO - auto distance_1 = cv::norm(a.center - img_center); - auto distance_2 = cv::norm(b.center - img_center); - return distance_1 < distance_2; - }); - sq_armor_list->sort([](const auto& a, const auto& b) { return a.priority < b.priority; }); + for (uint32_t i = 0; i < static_cast(CarIDFlag::Count); ++i) { + auto id = static_cast(static_cast(CarIDFlag::Hero) << i); - auto filter_flag = snapshot_manager_->GetId(); - for (const auto& armor : *sq_armor_list) { - if (!enumeration::IsFlagContains(filter_flag, armor.armor.id)) continue; + if (armors_in_image->GetArmors(id).empty()) continue; - auto snapshot = snapshot_manager_->GetSingleSnapshot(armor.armor.id); - if (!snapshot) continue; + detected_ids = static_cast( + static_cast(detected_ids) | static_cast(id)); - if (state_ == TrackState::Tracking && snapshot->GetPriority() < current_priority_) { - SetState(TrackState::Switching); - temp_lost_count_ = 0; + for (const auto& armor : armors_in_image->GetArmors(id)) { + auto armor_info = decider::ArmorInfo(armor); + armor_info_list.emplace_back(armor_info); } + } + + decider_->SetInvincibleArmors(invincible_armors_); + decider_->ArmorFilter(armor_info_list); + decider_->SetPriority(armor_info_list); + + auto sorted_id = decider_->GetSortedArmor(armor_info_list); + if (sorted_id == enumeration::ArmorIdFlag::None) { + return nullptr; + } - auto now = predictor::TimeStamp(std::time(nullptr)); - if (state_ != TrackState::Lost && now.SecondsSince(last_timestamp_) > 0.1) { - SetState(TrackState::Lost); - ResetTracking(); - return nullptr; - } + auto snapshot = snapshot_manager_->GetSingleSnapshot(sorted_id); + + if (state_ == TrackState::Tracking) { + SetState(TrackState::Switching); + temp_lost_count_ = 0; + } - last_timestamp_ = now; - tracking_car_id_ = snapshot->GetID(); - current_priority_ = snapshot->GetPriority(); - return snapshot; + auto now = predictor::TimeStamp(std::time(nullptr)); + if (state_ != TrackState::Lost && now.SecondsSince(last_timestamp_) > 0.1) { + SetState(TrackState::Lost); + ResetTracking(); + return nullptr; } + + last_timestamp_ = now; + tracking_car_id_ = snapshot->GetID(); + return snapshot; + ResetTracking(); return nullptr; } + void SetInvincibleArmors(const CarIDFlag& invincible_armors) { + invincible_armors_ = invincible_armors; + } + world_exe::enumeration::CarIDFlag GetCurrentTargetID() const { return tracking_car_id_; } void UpdateState(bool found) { @@ -153,15 +172,12 @@ class DefaultTracker final { state_ = new_state; } - void ResetTracking() { - tracking_car_id_ = enumeration::CarIDFlag::None; - current_priority_ = 100; - } + void ResetTracking() { tracking_car_id_ = enumeration::CarIDFlag::None; } - int current_priority_ = 100; world_exe::enumeration::CarIDFlag tracking_car_id_ { enumeration::CarIDFlag::None }; TrackState state_ = TrackState::Lost; TrackState pre_state_ = TrackState::Lost; + CarIDFlag invincible_armors_ { CarIDFlag::None }; int detect_count_ = 0; int temp_lost_count_ = 0; @@ -172,6 +188,8 @@ class DefaultTracker final { const int max_switch_count_ = 200; predictor::TimeStamp last_timestamp_; + + std::unique_ptr decider_; }; } \ No newline at end of file diff --git a/src/tongji/identifier/identified_armor.hpp b/src/tongji/identifier/identified_armor.hpp index 950fb32..ceabe04 100644 --- a/src/tongji/identifier/identified_armor.hpp +++ b/src/tongji/identifier/identified_armor.hpp @@ -1,20 +1,20 @@ #pragma once +#include +#include +#include + #include "enum/armor_id.hpp" #include "enum/car_id.hpp" #include "interfaces/armor_in_image.hpp" #include "interfaces/time_stamped.hpp" #include "util/index.hpp" -#include -#include -#include namespace world_exe::tongji::identifier { struct SPArmor { const data::ArmorImageSpacing& armor; const cv::Point2f center; - const int priority = 0; }; class IdentifiedArmor final : public interfaces::IArmorInImage, public interfaces::ITimeStamped { @@ -34,18 +34,6 @@ class IdentifiedArmor final : public interfaces::IArmorInImage, public interface return armors_[util::enumeration::GetIndex(armor_id)]; } - std::shared_ptr> get_sq_armor() { - if (armor_list != nullptr) return armor_list; - - armor_list = std::make_shared>(); - for (const auto& car : armors_) - for (const auto& armor : car) - armor_list->emplace_back( - armor, (armor.image_points[0] + armor.image_points[3]) / 2, 0); - - return armor_list; - } - static IdentifiedArmor DecorateIArmorInImage(const interfaces::IArmorInImage& armor) { throw std::runtime_error("Not implemented"); } @@ -62,7 +50,7 @@ class IdentifiedArmor final : public interfaces::IArmorInImage, public interface } private: - std::time_t time_stamp_ { 0 }; + std::time_t time_stamp_ { std::time(nullptr) }; std::array, 8> armors_; std::shared_ptr> armor_list = nullptr; }; diff --git a/src/tongji/predictor/predict_model.hpp b/src/tongji/predictor/predict_model.hpp index a9942a4..db04478 100644 --- a/src/tongji/predictor/predict_model.hpp +++ b/src/tongji/predictor/predict_model.hpp @@ -48,14 +48,11 @@ class PredictModel { } else { armor_num_ = 4; } - - // TODO :init priority } auto GetP0Dig() const { return P0_dig_; } auto GetRadius() const { return radius_; } auto GetID() const { return car_id_; } - auto GetPriority() const { return priority_; } int GetArmorNum() const { return armor_num_; } // 防止夹角求和出现异常值 @@ -266,7 +263,6 @@ ExtendedKalmanFilter<11, 4>::QMat Q(double dt) const { } private: - int priority_; int armor_num_; enumeration::CarIDFlag car_id_; diff --git a/src/tongji/predictor/target_snapshot.hpp b/src/tongji/predictor/target_snapshot.hpp index ffe37b6..5dd41b9 100644 --- a/src/tongji/predictor/target_snapshot.hpp +++ b/src/tongji/predictor/target_snapshot.hpp @@ -40,7 +40,6 @@ class TargetSnapshot { auto GetTimeStamp() const { return time_stamp_; } auto GetID() const { return model_.GetID(); } auto GetEkfX() const { return ekf_.x; } - auto GetPriority() const { return model_.GetPriority(); } auto Predict(const double& dt) -> Eigen::Vector const { auto predicted_x = model_.f(ekf_.x, dt); diff --git a/src/tongji/state_machine/car_state_manager.hpp b/src/tongji/state_machine/car_state_manager.hpp index 4e940bb..a4717e8 100644 --- a/src/tongji/state_machine/car_state_manager.hpp +++ b/src/tongji/state_machine/car_state_manager.hpp @@ -37,13 +37,9 @@ class CarStateManager { is_locked_ = false; is_converged_ = false; is_diverged_ = true; - priority_ = default_priority_; last_seen_ = predictor::TimeStamp(0); } - void SetPriority(const int& p) { priority_ = p; } - int GetPriority() const { return priority_; } - void SetThreshold(const int& value) { switch_threshold_ = value; } predictor::TimeStamp LastSeen() const { return last_seen_; } @@ -55,11 +51,10 @@ class CarStateManager { int update_count_ = 0; double timeout_sec_; - bool is_locked_ = false; - bool is_converged_ = false; - bool is_diverged_ = true; - int priority_ = 100; - int default_priority_ = 100; + bool is_locked_ = false; + bool is_converged_ = false; + bool is_diverged_ = true; + ; predictor::TimeStamp last_seen_; }; From 344ee501100b68adc19fb600b9ff3b7b57996e58 Mon Sep 17 00:00:00 2001 From: heyeuu <2829004293@qq.com> Date: Thu, 9 Oct 2025 23:53:13 +0800 Subject: [PATCH 48/53] chore(state): remove redundant logic from state machine --- src/tongji/decider/decider.cpp | 6 +++--- src/tongji/decider/decider.hpp | 2 +- src/tongji/fire_controller/tracker.hpp | 4 ++-- src/tongji/state_machine/car_state_manager.hpp | 9 ++------- src/tongji/state_machine/state_machine.cpp | 7 +------ 5 files changed, 9 insertions(+), 19 deletions(-) diff --git a/src/tongji/decider/decider.cpp b/src/tongji/decider/decider.cpp index 14e338d..ec9fb35 100644 --- a/src/tongji/decider/decider.cpp +++ b/src/tongji/decider/decider.cpp @@ -26,10 +26,10 @@ class Decider::Impl { } } - void SetPriority(const std::vector& detected_result) const { + void SetPriority( std::vector& detected_result) const { const PriorityMap& priority_map = (mode_ == PriorityMode::MODE_ONE) ? mode1 : mode2; if (!detected_result.empty()) { - for (auto armor : detected_result) { + for (auto& armor : detected_result) { armor.priority = priority_map.at(armor.armor_spacing.id); } } @@ -117,7 +117,7 @@ void Decider::SetInvincibleArmors(const enumeration::ArmorIdFlag& armors) { return pimpl_->SetInvincibleArmors(armors); } -void Decider::SetPriority(const std::vector& detected_result) const { +void Decider::SetPriority( std::vector& detected_result) const { return pimpl_->SetPriority(detected_result); } diff --git a/src/tongji/decider/decider.hpp b/src/tongji/decider/decider.hpp index 9717d04..0ff0287 100644 --- a/src/tongji/decider/decider.hpp +++ b/src/tongji/decider/decider.hpp @@ -18,7 +18,7 @@ class Decider { ~Decider(); void SetInvincibleArmors(const enumeration::ArmorIdFlag& armors); - void SetPriority(const std::vector& detected_result) const; + void SetPriority(std::vector& detected_result) const; enumeration::ArmorIdFlag GetSortedArmor(std::vector& armors) const; bool ArmorFilter(std::vector& armors); diff --git a/src/tongji/fire_controller/tracker.hpp b/src/tongji/fire_controller/tracker.hpp index 57b9920..68b5d4a 100644 --- a/src/tongji/fire_controller/tracker.hpp +++ b/src/tongji/fire_controller/tracker.hpp @@ -62,9 +62,9 @@ class Tracker final { armor_info_list.emplace_back(armor_info); } } - + decider_->SetInvincibleArmors(invincible_armors_); - decider_->ArmorFilter(armor_info_list); + auto is_empty = decider_->ArmorFilter(armor_info_list); decider_->SetPriority(armor_info_list); auto sorted_id = decider_->GetSortedArmor(armor_info_list); diff --git a/src/tongji/state_machine/car_state_manager.hpp b/src/tongji/state_machine/car_state_manager.hpp index a4717e8..9c40d4b 100644 --- a/src/tongji/state_machine/car_state_manager.hpp +++ b/src/tongji/state_machine/car_state_manager.hpp @@ -18,7 +18,7 @@ class CarStateManager { count_ = std::min(count_ + 1, switch_threshold_); last_seen_ = now; update_count_++; - if (update_count_ >= converge_threshold_ && is_diverged_) is_converged_ = true; + if (update_count_ >= converge_threshold_ && !is_diverged_) is_converged_ = true; } else { count_ = std::max(count_ - 1, 0); } @@ -27,17 +27,13 @@ class CarStateManager { bool IsConverged() const { return is_converged_ && !is_diverged_; } - bool IsLost(const TimeStamp& now) const { - return is_locked_ && now.SecondsSince(last_seen_) > timeout_sec_; - } - void Reset() { count_ = 0; update_count_ = 0; is_locked_ = false; is_converged_ = false; is_diverged_ = true; - last_seen_ = predictor::TimeStamp(0); + last_seen_ = predictor::TimeStamp(std::time(nullptr)); } void SetThreshold(const int& value) { switch_threshold_ = value; } @@ -54,7 +50,6 @@ class CarStateManager { bool is_locked_ = false; bool is_converged_ = false; bool is_diverged_ = true; - ; predictor::TimeStamp last_seen_; }; diff --git a/src/tongji/state_machine/state_machine.cpp b/src/tongji/state_machine/state_machine.cpp index 2ce500e..fc0ffa8 100644 --- a/src/tongji/state_machine/state_machine.cpp +++ b/src/tongji/state_machine/state_machine.cpp @@ -22,7 +22,7 @@ class StateMachine::Impl { const car_state::CarStateManager& GetState(enumeration::CarIDFlag single_id) const { return car_states_[util::enumeration::GetIndex(single_id)]; } - + void Update(const enumeration::CarIDFlag& car_detected, const std::time_t& now) { tracing_state_ = enumeration::CarIDFlag::None; @@ -38,11 +38,6 @@ class StateMachine::Impl { continue; } - if (state.IsLost(now)) { - state.Reset(); - continue; - } - tracing_state_ = static_cast( static_cast(tracing_state_) | static_cast(id)); } From 6b295089a4eb7ac9051d32cdd03a9febd5e0a7c4 Mon Sep 17 00:00:00 2001 From: heyeuu <2829004293@qq.com> Date: Thu, 9 Oct 2025 23:55:35 +0800 Subject: [PATCH 49/53] chore: remove miscellaneous trivial cruft --- src/tongji/state_machine/car_state_manager.hpp | 1 - 1 file changed, 1 deletion(-) diff --git a/src/tongji/state_machine/car_state_manager.hpp b/src/tongji/state_machine/car_state_manager.hpp index 9c40d4b..6592a63 100644 --- a/src/tongji/state_machine/car_state_manager.hpp +++ b/src/tongji/state_machine/car_state_manager.hpp @@ -45,7 +45,6 @@ class CarStateManager { int switch_threshold_; int converge_threshold_ = 3; int update_count_ = 0; - double timeout_sec_; bool is_locked_ = false; bool is_converged_ = false; From 919cae35bfdf31e0112bd3ef31c4b25844de23c7 Mon Sep 17 00:00:00 2001 From: heyeuu <2829004293@qq.com> Date: Fri, 10 Oct 2025 21:47:36 +0800 Subject: [PATCH 50/53] chore(fire controller): remove redundant timestamp setter function --- src/tongji/fire_controller/fire_controller.cpp | 11 +++++------ src/tongji/fire_controller/fire_controller.hpp | 3 +-- src/tongji/fire_controller/fire_decision.hpp | 5 +++-- 3 files changed, 9 insertions(+), 10 deletions(-) diff --git a/src/tongji/fire_controller/fire_controller.cpp b/src/tongji/fire_controller/fire_controller.cpp index 29a14f2..b36c965 100644 --- a/src/tongji/fire_controller/fire_controller.cpp +++ b/src/tongji/fire_controller/fire_controller.cpp @@ -60,7 +60,7 @@ class FireController::Impl { if (!aim_solution.valid) return { .fire_allowance = false }; const Eigen::Vector3d target_pos = aim_solution.aim_point.head<3>(); - const bool fireable = fire_decision_->ShouldFire(aim_solution, target_pos, gimbal_pos_); + const bool fireable = fire_decision_->ShouldFire(aim_solution, target_pos, gimbal_yaw_); if (fireable) attacked_cars_ = snapshot->GetID(); @@ -74,12 +74,11 @@ class FireController::Impl { void UpdateIdentifiedArmors(std::shared_ptr armors) { identified_armors_ = armors; } - void UpdateGimbalPosition(const Eigen::Vector3d& gimbal_pos) { gimbal_pos_ = gimbal_pos; }; - void SetTimeStamp(const std::time_t& time_stamp) { time_stamp_.SetTimeStamp(time_stamp); } + void UpdateGimbalPosition(const double gimbal_yaw) { gimbal_yaw_ = gimbal_yaw; }; TimeStamp GetTimeStamp() const { return time_stamp_; } private: - Eigen::Vector3d gimbal_pos_ { Eigen::Vector3d::Zero() }; + double gimbal_yaw_; double control_delay_; double bullet_speed_; @@ -112,8 +111,8 @@ void FireController::UpdateIdentifiedArmors(std::shared_ptrUpdateIdentifiedArmors(armors); } -void FireController::UpdateGimbalPosition(const Eigen::Vector3d& gimbal_pos) { - return pimpl_->UpdateGimbalPosition(gimbal_pos); +void FireController::UpdateGimbalPosition(const double& gimbal_yaw) { + return pimpl_->UpdateGimbalPosition(gimbal_yaw); }; } \ No newline at end of file diff --git a/src/tongji/fire_controller/fire_controller.hpp b/src/tongji/fire_controller/fire_controller.hpp index bbbe7a0..be9ddea 100644 --- a/src/tongji/fire_controller/fire_controller.hpp +++ b/src/tongji/fire_controller/fire_controller.hpp @@ -19,9 +19,8 @@ class FireController final : public interfaces::IFireControl { const enumeration ::CarIDFlag GetAttackCarId() const override; void UpdateIdentifiedArmors(std::shared_ptr armors); - void UpdateGimbalPosition(const Eigen::Vector3d& gimbal_pos); + void UpdateGimbalPosition(const double& gimbal_yaw); - void SetTimeStamp(const std::time_t& time_stamp); predictor::TimeStamp GetTimeStamp() const; private: diff --git a/src/tongji/fire_controller/fire_decision.hpp b/src/tongji/fire_controller/fire_decision.hpp index 8dda984..e6790db 100644 --- a/src/tongji/fire_controller/fire_decision.hpp +++ b/src/tongji/fire_controller/fire_decision.hpp @@ -18,7 +18,7 @@ class FireDecision { , judge_distance_(judge_distance) { } bool ShouldFire(const AimSolution& aimsolution, const Eigen::Vector3d& valid_target_pos, - const Eigen::Vector3d& gimbal_pos) { + const double& gimbal_yaw) { if (!aimsolution.valid || !auto_fire_) return false; const auto& tolerance = std::sqrt(valid_target_pos.x() * valid_target_pos.x() @@ -29,7 +29,8 @@ class FireDecision { if (std::abs(last_aim_solution_.yaw - aimsolution.yaw) < tolerance * 2 // 此时认为command突变不应该射击 - && std::abs(gimbal_pos[0] - last_aim_solution_.yaw) < tolerance && last_aim_solution_.valid) { + && std::abs(gimbal_yaw - last_aim_solution_.yaw) < tolerance + && last_aim_solution_.valid) { last_aim_solution_ = aimsolution; return true; } From 511127574a8774dfa776a329ce439980a3c0a6b1 Mon Sep 17 00:00:00 2001 From: heyeuu <2829004293@qq.com> Date: Mon, 13 Oct 2025 11:34:28 +0800 Subject: [PATCH 51/53] refactor: complete initial architectural restructuring - Reimplement predictor module with clear dependency boundaries - Rewrite fire_control module to follow proper dependency injection --- .../aim_point_chooser.hpp | 0 .../aim_solver.hpp | 41 ++-- .../trajectory.hpp | 2 +- src/tongji/decider/armor_info.hpp | 26 --- src/tongji/decider/decider.cpp | 131 ------------- src/tongji/decider/decider.hpp | 30 --- .../fire_controller/fire_controller.cpp | 110 ++++++----- .../fire_controller/fire_controller.hpp | 10 +- src/tongji/fire_controller/fire_decision.hpp | 24 +-- src/tongji/identifier/armor_filter.hpp | 45 +++++ src/tongji/identifier/identified_armor.hpp | 22 --- src/tongji/predictor/decider.cpp | 78 ++++++++ src/tongji/predictor/decider.hpp | 35 ++++ .../predictor/extended_kalman_filter.hpp | 2 +- src/tongji/predictor/live_target_manager.cpp | 176 ++++++++---------- src/tongji/predictor/live_target_manager.hpp | 14 +- src/tongji/predictor/target_snapshot.hpp | 31 ++- .../predictor/target_snapshot_manager.cpp | 87 ++++++--- .../predictor/target_snapshot_manager.hpp | 17 +- .../tracker.hpp | 112 +++++------ .../state_machine/car_state_manager.hpp | 55 ------ src/tongji/state_machine/state_machine.cpp | 62 ++---- src/tongji/state_machine/state_machine.hpp | 6 +- 23 files changed, 483 insertions(+), 633 deletions(-) rename src/tongji/{fire_controller => armor_solver}/aim_point_chooser.hpp (100%) rename src/tongji/{fire_controller => armor_solver}/aim_solver.hpp (65%) rename src/tongji/{fire_controller => armor_solver}/trajectory.hpp (96%) delete mode 100644 src/tongji/decider/armor_info.hpp delete mode 100644 src/tongji/decider/decider.cpp delete mode 100644 src/tongji/decider/decider.hpp create mode 100644 src/tongji/identifier/armor_filter.hpp create mode 100644 src/tongji/predictor/decider.cpp create mode 100644 src/tongji/predictor/decider.hpp rename src/tongji/{fire_controller => predictor}/tracker.hpp (56%) delete mode 100644 src/tongji/state_machine/car_state_manager.hpp diff --git a/src/tongji/fire_controller/aim_point_chooser.hpp b/src/tongji/armor_solver/aim_point_chooser.hpp similarity index 100% rename from src/tongji/fire_controller/aim_point_chooser.hpp rename to src/tongji/armor_solver/aim_point_chooser.hpp diff --git a/src/tongji/fire_controller/aim_solver.hpp b/src/tongji/armor_solver/aim_solver.hpp similarity index 65% rename from src/tongji/fire_controller/aim_solver.hpp rename to src/tongji/armor_solver/aim_solver.hpp index c462bf1..db34fe3 100644 --- a/src/tongji/fire_controller/aim_solver.hpp +++ b/src/tongji/armor_solver/aim_solver.hpp @@ -4,13 +4,12 @@ #include #include #include -#include -#include "tongji/fire_controller/aim_point_chooser.hpp" -#include "tongji/fire_controller/trajectory.hpp" +#include "tongji/armor_solver/aim_point_chooser.hpp" +#include "tongji/armor_solver/trajectory.hpp" #include "tongji/predictor/target_snapshot.hpp" -namespace world_exe::tongji::fire_control { +namespace world_exe::tongji::armor_solver { using TargetSnapshot = predictor::TargetSnapshot; @@ -24,20 +23,16 @@ struct AimSolution { class AimingSolver { public: - AimingSolver(const double& bullet_speed, const double& yaw_offset, const double& pitch_offset, - const double& gravity = 9.7833) - : aim_point_chooser_(std::make_unique()) - , bullet_speed_(bullet_speed) + AimingSolver( + const double& yaw_offset, const double& pitch_offset, const double& gravity = 9.7833) + : aim_point_chooser_(std::make_unique()) , yaw_offset_(yaw_offset / 57.3) // degree to rad , pitch_offset_(pitch_offset / 57.3) // degree to rad , g_(gravity) { } - void UpdateSnapshot(std::unique_ptr snapshot) { - snapshot_ = std::move(snapshot); - } - - AimSolution SolveAimSolution(const double& time_delay) { - if (!snapshot_) return { false, 0, 0, { }, 0 }; + AimSolution SolveAimSolution(const std::shared_ptr& snapshot, + const double& bullet_speed, const double& time_delay) { + if (!snapshot) return { false, 0, 0, { }, 0 }; // 迭代求解飞行时间 (最多10次，收敛条件：相邻两次fly_time差 <0.001) double prev_fly_time = 0; @@ -48,10 +43,10 @@ class AimingSolver { // 预测目标在未来 dt时间后的位置 for (int i = 0; i < 10; ++i) { double dt = time_delay + prev_fly_time; - const auto aim = SelectPredictedAim(dt); + const auto aim = SelectPredictedAim(snapshot, dt); if (!aim.has_value()) return { false, 0, 0, { }, 0 }; // failed: no valid aim point - const auto traj = SolveTrajectory(aim->head(3)); + const auto traj = SolveTrajectory(aim->head(3), bullet_speed); if (!traj.has_value()) return { false, 0, 0, { }, 0 }; // failed: trajectory unsolvable if (i > 0 && std::abs(traj->fly_time - prev_fly_time) < 0.001) { @@ -71,23 +66,23 @@ class AimingSolver { } private: - std::optional SelectPredictedAim(double dt) const { + std::optional SelectPredictedAim( + const std::shared_ptr& snapshot, const double& dt) const { const auto& [selectable, aim_point] = aim_point_chooser_->ChooseAimArmor( - snapshot_->Predict(dt), snapshot_->GetPredictedXYZAList(dt), snapshot_->GetID()); + snapshot->Predict(dt), snapshot->GetPredictedXYZAList(dt), snapshot->GetID()); return selectable ? std::optional { aim_point } : std::nullopt; } - std::optional SolveTrajectory(const Eigen::Vector3d& xyz) const { + std::optional SolveTrajectory( + const Eigen::Vector3d& xyz, const double& bullet_speed) const { double d = std::hypot(xyz.x(), xyz.y()); - auto result = TrajectorySolver::SolveTrajectory(bullet_speed_, d, xyz.z(), g_); + auto result = TrajectorySolver::SolveTrajectory(bullet_speed, d, xyz.z(), g_); return result.solvable ? std::optional { result } : std::nullopt; } - double bullet_speed_; // m/s double yaw_offset_, pitch_offset_; const double g_; - std::unique_ptr aim_point_chooser_; - std::unique_ptr snapshot_; + std::unique_ptr aim_point_chooser_; }; } \ No newline at end of file diff --git a/src/tongji/fire_controller/trajectory.hpp b/src/tongji/armor_solver/trajectory.hpp similarity index 96% rename from src/tongji/fire_controller/trajectory.hpp rename to src/tongji/armor_solver/trajectory.hpp index 2b9c5e1..d4f9e68 100644 --- a/src/tongji/fire_controller/trajectory.hpp +++ b/src/tongji/armor_solver/trajectory.hpp @@ -1,7 +1,7 @@ #pragma once #include -namespace world_exe::tongji::fire_control { +namespace world_exe::tongji::armor_solver { struct TrajectoryResult { bool solvable = true; diff --git a/src/tongji/decider/armor_info.hpp b/src/tongji/decider/armor_info.hpp deleted file mode 100644 index a5abb07..0000000 --- a/src/tongji/decider/armor_info.hpp +++ /dev/null @@ -1,26 +0,0 @@ - - -#pragma once - -#include "data/armor_image_spaceing.hpp" - -namespace world_exe::tongji::decider { - -enum class ArmorPriority { - First = 1, // - Second, // - Third, // - Forth, // - Fifth // -}; - -struct ArmorInfo { - ArmorInfo(const data::ArmorImageSpacing& armor_spacing, - const ArmorPriority& priority = ArmorPriority::Fifth) - : armor_spacing(armor_spacing) - , priority(priority) { } - - data::ArmorImageSpacing armor_spacing; - ArmorPriority priority; -}; -} \ No newline at end of file diff --git a/src/tongji/decider/decider.cpp b/src/tongji/decider/decider.cpp deleted file mode 100644 index ec9fb35..0000000 --- a/src/tongji/decider/decider.cpp +++ /dev/null @@ -1,131 +0,0 @@ -#include "decider.hpp" - -#include -#include -#include -#include -#include -#include - -#include "enum/armor_id.hpp" -#include "enum/car_id.hpp" -#include "tongji/decider/armor_info.hpp" -#include "util/index.hpp" - -namespace world_exe::tongji::decider { - -class Decider::Impl { -public: - explicit Impl(PriorityMode mode = PriorityMode::MODE_ONE) - : mode_(mode) { } - - void SetInvincibleArmors(const enumeration::ArmorIdFlag& armors) { - invincible_armor_.clear(); - for (const auto id : util::enumeration::ExpandArmorIdFlags(armors)) { - invincible_armor_.emplace(std::move(armors)); - } - } - - void SetPriority( std::vector& detected_result) const { - const PriorityMap& priority_map = (mode_ == PriorityMode::MODE_ONE) ? mode1 : mode2; - if (!detected_result.empty()) { - for (auto& armor : detected_result) { - armor.priority = priority_map.at(armor.armor_spacing.id); - } - } - } - - enumeration::ArmorIdFlag GetSortedArmor(std::vector& armors) const { - if (armors.empty()) return enumeration::ArmorIdFlag::None; - - // 对每个 DetectionResult 调用 armor_filter 和 set_priority - // ArmorFilter(armors); - // SetPriority(armors); - - // 对每个 DetectionResult 中的 armors 进行排序 - cv::Point2d img_center(1440.0 / 2, 1080.0 / 2); // TODO - std::sort(armors.begin(), armors.end(), [&](const auto& a, const auto& b) { - auto center_a = - (a.armor_spacing.image_points[0] + a.armor_spacing.image_points[3]) * 0.5f; - auto center_b = - (b.armor_spacing.image_points[0] + b.armor_spacing.image_points[3]) * 0.5f; - return cv::norm(center_a - img_center) < cv::norm(center_b - img_center); - }); - - std::sort(armors.begin(), armors.end(), - [](const auto& a, const auto& b) { return a.priority < b.priority; }); - return armors.front().armor_spacing.id; - } - - bool ArmorFilter(std::vector& armors) const { - if (armors.empty()) return true; - - // 25赛季没有5号装甲板 - armors.erase(std::remove_if(armors.begin(), armors.end(), - [](const auto& armor) { return armor.armor_spacing.id == ArmorId::InfantryV; })); - // 不打前哨站 - armors.erase(std::remove_if(armors.begin(), armors.end(), - [](const auto& armor) { return armor.armor_spacing.id == ArmorId::Outpost; })); - // 过滤掉刚复活无敌的装甲板 - armors.erase( - std::remove_if(armors.begin(), armors.end(), - [&](const auto& armor) { return invincible_armor_.count(armor.armor_spacing.id); }), - armors.end()); - - return armors.empty(); - } - -private: - using ArmorPriority = world_exe::tongji::decider::ArmorPriority; - using ArmorId = world_exe::enumeration::ArmorIdFlag; - using ArmorInfo = world_exe::tongji::decider::ArmorInfo; - - using PriorityMap = std::unordered_map; - - const PriorityMap mode1 = { - { ArmorId::Hero, ArmorPriority::Second }, // - { ArmorId::Engineer, ArmorPriority::Forth }, // - { ArmorId::InfantryIII, ArmorPriority::First }, // - { ArmorId::InfantryIV, ArmorPriority::First }, // - { ArmorId::InfantryV, ArmorPriority::Third }, // - { ArmorId::Sentry, ArmorPriority::Third }, // - { ArmorId::Outpost, ArmorPriority::Fifth }, // - { ArmorId::Base, ArmorPriority::Fifth }, // - { ArmorId::Unknow, ArmorPriority::Fifth } // - }; - - const PriorityMap mode2 = { - { ArmorId::Hero, ArmorPriority::Second }, // - { ArmorId::Engineer, ArmorPriority::Forth }, // - { ArmorId::InfantryIII, ArmorPriority::First }, // - { ArmorId::InfantryIV, ArmorPriority::First }, // - { ArmorId::InfantryV, ArmorPriority::Third }, // - { ArmorId::Sentry, ArmorPriority::Third }, // - { ArmorId::Outpost, ArmorPriority::Fifth }, // - { ArmorId::Base, ArmorPriority::Fifth }, // - { ArmorId::Unknow, ArmorPriority::Fifth } // - }; - - std::unordered_set invincible_armor_; - PriorityMode mode_; -}; - -Decider::Decider(PriorityMode mode) - : pimpl_(std::make_unique(mode)) { } - -void Decider::SetInvincibleArmors(const enumeration::ArmorIdFlag& armors) { - return pimpl_->SetInvincibleArmors(armors); -} - -void Decider::SetPriority( std::vector& detected_result) const { - return pimpl_->SetPriority(detected_result); -} - -bool Decider::ArmorFilter(std::vector& armors) { return pimpl_->ArmorFilter(armors); } - -enumeration::CarIDFlag Decider::GetSortedArmor(std::vector& armors) const { - return pimpl_->GetSortedArmor(armors); -} - -Decider::~Decider() = default; -} diff --git a/src/tongji/decider/decider.hpp b/src/tongji/decider/decider.hpp deleted file mode 100644 index 0ff0287..0000000 --- a/src/tongji/decider/decider.hpp +++ /dev/null @@ -1,30 +0,0 @@ - - -#pragma once - -#include -#include - -#include "enum/armor_id.hpp" -#include "tongji/decider/armor_info.hpp" - -namespace world_exe::tongji::decider { - -enum PriorityMode { MODE_ONE = 1, MODE_TWO }; - -class Decider { -public: - Decider(PriorityMode mode = PriorityMode::MODE_ONE); - ~Decider(); - - void SetInvincibleArmors(const enumeration::ArmorIdFlag& armors); - void SetPriority(std::vector& detected_result) const; - enumeration::ArmorIdFlag GetSortedArmor(std::vector& armors) const; - bool ArmorFilter(std::vector& armors); - -private: - class Impl; - std::unique_ptr pimpl_; -}; - -} \ No newline at end of file diff --git a/src/tongji/fire_controller/fire_controller.cpp b/src/tongji/fire_controller/fire_controller.cpp index b36c965..b4f79e6 100644 --- a/src/tongji/fire_controller/fire_controller.cpp +++ b/src/tongji/fire_controller/fire_controller.cpp @@ -1,21 +1,14 @@ #include "fire_controller.hpp" -#include -#include -#include #include +#include -#include - -#include "enum/car_id.hpp" -#include "interfaces/armor_in_image.hpp" -#include "tongji/fire_controller/aim_solver.hpp" +#include "data/fire_control.hpp" +#include "interfaces/target_predictor.hpp" #include "tongji/fire_controller/fire_decision.hpp" -#include "tongji/fire_controller/tracker.hpp" #include "tongji/identifier/identified_armor.hpp" #include "tongji/predictor/live_target_manager.hpp" #include "tongji/predictor/target_snapshot_manager.hpp" -#include "tongji/predictor/time_stamp.hpp" #include "tongji/state_machine/state_machine.hpp" namespace world_exe::tongji::fire_control { @@ -29,77 +22,88 @@ using TimeStamp = predictor::TimeStamp; class FireController::Impl { public: - Impl(std::shared_ptr state_machine, bool auto_fire, - const double& control_delay_in_second, const double& bullet_speed, double yaw_offset, - double pitch_offset) + Impl(bool auto_fire, const double& control_delay_in_second, const double& bullet_speed, + double yaw_offset, double pitch_offset, + std::shared_ptr state_machine, + std::shared_ptr live_target_manager) : control_delay_(control_delay_in_second) , bullet_speed_(bullet_speed) - , tracker_(std::make_unique()) - , aim_solver_(std::make_unique(bullet_speed, yaw_offset, pitch_offset)) , fire_decision_(std::make_unique(auto_fire)) - , live_target_manager_(std::make_shared(state_machine)) { } + , state_machine_(state_machine) + , live_target_manager_(std::move(live_target_manager)) { } // TODO:std::time_t const data ::FireControl CalculateTarget(const std ::time_t& time_duration) const { - if (!identified_armors_ || !tracker_ || !aim_solver_ || !fire_decision_ - || !live_target_manager_) + + if (!identified_armors_ || !fire_decision_ || !state_machine_ || !live_target_manager_) return { .fire_allowance = false }; - auto snapshot_manager = std::dynamic_pointer_cast( - live_target_manager_->GetPredictor(live_target_manager_->GetLiveTargetIDs())); - auto snapshot = tracker_->SelectTrackingTarget(identified_armors_, snapshot_manager); + auto converged_cars = state_machine_->GetAllowdToFires(); + auto snapshot_manager = live_target_manager_->GetPredictor(converged_cars); + if (!snapshot_manager) + return data::FireControl { .time_stamp = time_stamp_.GetTimeStamp(), + .gimbal_dir = Eigen::Vector3d::Constant(std::numeric_limits::quiet_NaN()), + .fire_allowance = false }; - bool found = (snapshot != nullptr); - tracker_->UpdateState(found); - if (!found) return { .fire_allowance = false }; + auto armors_in_gimbal_control = snapshot_manager->Predictor(control_delay_); + allowed_target_id_ = state_machine_->GetAllowdToFires(); - aim_solver_->UpdateSnapshot(std::move(snapshot)); + auto target_gimbal_spacing = + armors_in_gimbal_control->GetArmors(allowed_target_id_).front(); - const auto& aim_solution = aim_solver_->SolveAimSolution(static_cast( - time_duration)); // TODO:std::time_t should not be converted directly to double type - if (!aim_solution.valid) return { .fire_allowance = false }; + auto gimbal_command = + std::dynamic_pointer_cast(snapshot_manager)->GetGimbalCommand(); - const Eigen::Vector3d target_pos = aim_solution.aim_point.head<3>(); - const bool fireable = fire_decision_->ShouldFire(aim_solution, target_pos, gimbal_yaw_); + auto fire_command = + fire_decision_->ShouldFire(gimbal_command, target_gimbal_spacing.position); + firable_ = fire_command; - if (fireable) attacked_cars_ = snapshot->GetID(); + data::FireControl result; + result.fire_allowance = fire_command; + result.gimbal_dir << gimbal_command.yaw, gimbal_command.pitch, 0; + result.time_stamp = time_stamp_.GetTimeStamp(); + return result; + } - return { .time_stamp = time_stamp_.GetTimeStamp(), - .gimbal_dir = Eigen::Vector3d(aim_solution.yaw, aim_solution.pitch, 0), - .fire_allowance = fireable }; + const CarIDFlag GetAttackCarId() const { + if (firable_) { } + return allowed_target_id_; } - const CarIDFlag GetAttackCarId() const { return attacked_cars_; } + void Update(std::shared_ptr armors, const double& gimbal_yaw) { + time_stamp_.SetTimeStamp(std::time(nullptr)); + UpdateIdentifiedArmor(armors); + UpdateGimbalPosition(gimbal_yaw); + } - void UpdateIdentifiedArmors(std::shared_ptr armors) { +private: + void UpdateIdentifiedArmor(std::shared_ptr armors) { identified_armors_ = armors; } - void UpdateGimbalPosition(const double gimbal_yaw) { gimbal_yaw_ = gimbal_yaw; }; + void UpdateGimbalPosition(const double& gimbal_yaw) { gimbal_yaw_ = gimbal_yaw; }; TimeStamp GetTimeStamp() const { return time_stamp_; } -private: double gimbal_yaw_; double control_delay_; double bullet_speed_; - mutable CarIDFlag attacked_cars_ { - CarIDFlag::None - }; // TODO:Mutable should not be used to break const encapsulation + mutable CarIDFlag allowed_target_id_; + mutable double firable_ { false }; std::shared_ptr identified_armors_; - std::unique_ptr tracker_; - std::shared_ptr live_target_manager_; - std::unique_ptr aim_solver_; - std::unique_ptr fire_decision_; + std::unique_ptr fire_decision_; predictor::TimeStamp time_stamp_ { std::time(nullptr) }; + + std::shared_ptr state_machine_; + std::shared_ptr live_target_manager_; }; -FireController::FireController(std::shared_ptr state_machine, bool auto_fire, +FireController::FireController(std::shared_ptr state_machine, bool auto_fire, const double& control_delay_in_second, const double& bullet_speed, double yaw_offset, - double pitch_offset) - : pimpl_(std::make_unique(state_machine, auto_fire, control_delay_in_second, bullet_speed, - yaw_offset, pitch_offset)) { } + double pitch_offset, std::shared_ptr live_target_manager) + : pimpl_(std::make_unique(auto_fire, control_delay_in_second, bullet_speed, yaw_offset, + pitch_offset, state_machine, live_target_manager)) { } const data ::FireControl FireController::CalculateTarget(const std ::time_t& time_duration) const { return pimpl_->CalculateTarget(time_duration); @@ -107,12 +111,4 @@ const data ::FireControl FireController::CalculateTarget(const std ::time_t& tim const CarIDFlag FireController::GetAttackCarId() const { return pimpl_->GetAttackCarId(); } -void FireController::UpdateIdentifiedArmors(std::shared_ptr armors) { - return pimpl_->UpdateIdentifiedArmors(armors); -} - -void FireController::UpdateGimbalPosition(const double& gimbal_yaw) { - return pimpl_->UpdateGimbalPosition(gimbal_yaw); -}; - } \ No newline at end of file diff --git a/src/tongji/fire_controller/fire_controller.hpp b/src/tongji/fire_controller/fire_controller.hpp index be9ddea..eed2838 100644 --- a/src/tongji/fire_controller/fire_controller.hpp +++ b/src/tongji/fire_controller/fire_controller.hpp @@ -3,23 +3,23 @@ #include #include "interfaces/armor_in_image.hpp" +#include "interfaces/car_state.hpp" #include "interfaces/fire_controller.hpp" +#include "interfaces/target_predictor.hpp" #include "tongji/predictor/time_stamp.hpp" -#include "tongji/state_machine/state_machine.hpp" namespace world_exe::tongji::fire_control { class FireController final : public interfaces::IFireControl { public: - FireController(std::shared_ptr state_machine, bool auto_fire, + FireController(std::shared_ptr state_machine, bool auto_fire, const double& control_delay_in_second, const double& bullet_speed, double yaw_offset, - double pitch_offset); + double pitch_offset, std::shared_ptr live_target_manager); const data ::FireControl CalculateTarget(const std ::time_t& time_duration) const override; const enumeration ::CarIDFlag GetAttackCarId() const override; - void UpdateIdentifiedArmors(std::shared_ptr armors); - void UpdateGimbalPosition(const double& gimbal_yaw); + void Update(std::shared_ptr armors, const double& gimbal_yaw); predictor::TimeStamp GetTimeStamp() const; diff --git a/src/tongji/fire_controller/fire_decision.hpp b/src/tongji/fire_controller/fire_decision.hpp index e6790db..7c94956 100644 --- a/src/tongji/fire_controller/fire_decision.hpp +++ b/src/tongji/fire_controller/fire_decision.hpp @@ -3,7 +3,7 @@ #include #include -#include "tongji/fire_controller/aim_solver.hpp" +#include "tongji/predictor/target_snapshot_manager.hpp" namespace world_exe::tongji::fire_control { @@ -12,35 +12,37 @@ class FireDecision { explicit FireDecision(const bool& auto_fire, const double& first_tolerance = 5, const double& second_tolerance = 2, const double& judge_distance = 3) : auto_fire_(auto_fire) - , last_aim_solution_ { false, 0, 0 } + , last_gimbal_command_({ std::numeric_limits::quiet_NaN(), + std::numeric_limits::quiet_NaN() }) , first_tolerance_(first_tolerance) , second_tolerance_(second_tolerance) , judge_distance_(judge_distance) { } - bool ShouldFire(const AimSolution& aimsolution, const Eigen::Vector3d& valid_target_pos, - const double& gimbal_yaw) { + bool ShouldFire( + predictor::GimbalCommand gimbal_command, const Eigen::Vector3d& valid_target_pos) { - if (!aimsolution.valid || !auto_fire_) return false; + if (!auto_fire_) return false; const auto& tolerance = std::sqrt(valid_target_pos.x() * valid_target_pos.x() + valid_target_pos.y() * valid_target_pos.y()) > judge_distance_ ? second_tolerance_ : first_tolerance_; - if (std::abs(last_aim_solution_.yaw - aimsolution.yaw) + if (std::abs(last_gimbal_command_.yaw - gimbal_yaw_) < tolerance * 2 // 此时认为command突变不应该射击 - && std::abs(gimbal_yaw - last_aim_solution_.yaw) < tolerance - && last_aim_solution_.valid) { - last_aim_solution_ = aimsolution; + && std::abs(gimbal_yaw_ - last_gimbal_command_.yaw) < tolerance) { + last_gimbal_command_ = gimbal_command; return true; } - last_aim_solution_ = aimsolution; + last_gimbal_command_ = gimbal_command; return false; } private: bool auto_fire_; - AimSolution last_aim_solution_; + predictor::GimbalCommand last_gimbal_command_; + + double gimbal_yaw_; double first_tolerance_ { 5 }; // 近距离射击容差，degree double second_tolerance_ { 2 }; // 远距离射击容差，degree diff --git a/src/tongji/identifier/armor_filter.hpp b/src/tongji/identifier/armor_filter.hpp new file mode 100644 index 0000000..21699e0 --- /dev/null +++ b/src/tongji/identifier/armor_filter.hpp @@ -0,0 +1,45 @@ +#pragma once + +#include +#include +#include + +#include "data/armor_image_spaceing.hpp" +#include "enum/armor_id.hpp" +#include "enum/car_id.hpp" +#include "util/index.hpp" + +namespace world_exe::tongji::identifier { + +class ArmorFilter { +public: + explicit ArmorFilter() + : invincible_armor_({ }) { } + std::vector FilterArmor( + std::vector armors) const { + + // 25赛季没有5号装甲板 + armors.erase(std::remove_if(armors.begin(), armors.end(), + [](const auto& armor) { return armor.id == enumeration::ArmorIdFlag::InfantryV; })); + // 不打前哨站 + armors.erase(std::remove_if(armors.begin(), armors.end(), + [](const auto& armor) { return armor.id == enumeration::ArmorIdFlag::Outpost; })); + // 过滤掉刚复活无敌的装甲板 + armors.erase(std::remove_if(armors.begin(), armors.end(), + [&](const auto& armor) { return invincible_armor_.count(armor.id); }), + armors.end()); + + return std::move(armors); + } + + void Update(enumeration::CarIDFlag ids) { + invincible_armor_.clear(); + for (auto id : util::enumeration::ExpandArmorIdFlags(ids)) { + invincible_armor_.emplace(std::move(id)); + } + } + +private: + std::unordered_set invincible_armor_; +}; +} \ No newline at end of file diff --git a/src/tongji/identifier/identified_armor.hpp b/src/tongji/identifier/identified_armor.hpp index ceabe04..14295e5 100644 --- a/src/tongji/identifier/identified_armor.hpp +++ b/src/tongji/identifier/identified_armor.hpp @@ -1,22 +1,12 @@ #pragma once -#include -#include -#include - #include "enum/armor_id.hpp" -#include "enum/car_id.hpp" #include "interfaces/armor_in_image.hpp" #include "interfaces/time_stamped.hpp" #include "util/index.hpp" namespace world_exe::tongji::identifier { -struct SPArmor { - const data::ArmorImageSpacing& armor; - const cv::Point2f center; -}; - class IdentifiedArmor final : public interfaces::IArmorInImage, public interfaces::ITimeStamped { public: explicit IdentifiedArmor(const std::vector& armors) { @@ -38,20 +28,8 @@ class IdentifiedArmor final : public interfaces::IArmorInImage, public interface throw std::runtime_error("Not implemented"); } - auto GetDetectedIDs() { - enumeration::CarIDFlag result; - for (int i = 0; i < 8; i++) { - if (!armors_[i].empty()) { - result = static_cast( - static_cast(result) | static_cast(armors_[i][0].id)); - } - } - return result; - } - private: std::time_t time_stamp_ { std::time(nullptr) }; std::array, 8> armors_; - std::shared_ptr> armor_list = nullptr; }; } \ No newline at end of file diff --git a/src/tongji/predictor/decider.cpp b/src/tongji/predictor/decider.cpp new file mode 100644 index 0000000..4db8573 --- /dev/null +++ b/src/tongji/predictor/decider.cpp @@ -0,0 +1,78 @@ +#include "decider.hpp" + +#include +#include +#include +#include + +#include "data/armor_image_spaceing.hpp" +#include "enum/armor_id.hpp" + +namespace world_exe::tongji::predictor { + +class Decider::Impl { +public: + explicit Impl(PriorityMode mode = PriorityMode::MODE_ONE) + : mode_(mode) { } + + enumeration::ArmorIdFlag GetBestArmor(std::vector& armors) const { + if (armors.empty()) return enumeration::ArmorIdFlag::None; + + const PriorityMap& priority_map = (mode_ == PriorityMode::MODE_ONE) ? mode1 : mode2; + std::vector> armors_list; + for (const auto& armor : armors) { + armors_list.emplace_back(armor.id, priority_map.at(armor.id)); + } + + cv::Point2d img_center(1440.0 / 2, 1080.0 / 2); // TODO + std::sort(armors.begin(), armors.end(), [&](const auto& a, const auto& b) { + auto center_a = (a.image_points[0] + a.image_points[3]) * 0.5f; + auto center_b = (b.image_points[0] + b.image_points[3]) * 0.5f; + return cv::norm(center_a - img_center) < cv::norm(center_b - img_center); + }); + + std::sort(armors_list.begin(), armors_list.end(), + [](const auto& a, const auto& b) { return a.second < b.second; }); + return armors.front().id; + } + +private: + using ArmorPriority = world_exe::tongji::predictor::ArmorPriority; + using ArmorId = world_exe::enumeration::ArmorIdFlag; + + using PriorityMap = std::unordered_map; + + const PriorityMap mode1 = { + { ArmorId::Hero, ArmorPriority::Second }, // + { ArmorId::Engineer, ArmorPriority::Forth }, // + { ArmorId::InfantryIII, ArmorPriority::First }, // + { ArmorId::InfantryIV, ArmorPriority::First }, // + { ArmorId::InfantryV, ArmorPriority::Third }, // + { ArmorId::Sentry, ArmorPriority::Third }, // + { ArmorId::Outpost, ArmorPriority::Fifth }, // + { ArmorId::Base, ArmorPriority::Fifth }, // + { ArmorId::Unknow, ArmorPriority::Fifth } // + }; + + const PriorityMap mode2 = { + { ArmorId::Hero, ArmorPriority::Second }, // + { ArmorId::Engineer, ArmorPriority::Forth }, // + { ArmorId::InfantryIII, ArmorPriority::First }, // + { ArmorId::InfantryIV, ArmorPriority::First }, // + { ArmorId::InfantryV, ArmorPriority::Third }, // + { ArmorId::Sentry, ArmorPriority::Third }, // + { ArmorId::Outpost, ArmorPriority::Fifth }, // + { ArmorId::Base, ArmorPriority::Fifth }, // + { ArmorId::Unknow, ArmorPriority::Fifth } // + }; + PriorityMode mode_; +}; + +Decider::Decider(PriorityMode mode) + : pimpl_(std::make_unique(mode)) { } +Decider::~Decider() = default; + +enumeration::ArmorIdFlag Decider::GetBestArmor(std::vector& armors) const { + return pimpl_->GetBestArmor(armors); +} +} diff --git a/src/tongji/predictor/decider.hpp b/src/tongji/predictor/decider.hpp new file mode 100644 index 0000000..0a44730 --- /dev/null +++ b/src/tongji/predictor/decider.hpp @@ -0,0 +1,35 @@ + + +#pragma once + +#include +#include + +#include "data/armor_image_spaceing.hpp" +#include "enum/armor_id.hpp" + +namespace world_exe::tongji::predictor { + +enum PriorityMode { MODE_ONE = 1, MODE_TWO }; + +enum class ArmorPriority { + First = 1, // + Second, // + Third, // + Forth, // + Fifth // +}; + +class Decider { +public: + Decider(PriorityMode mode = PriorityMode::MODE_ONE); + ~Decider(); + + enumeration::ArmorIdFlag GetBestArmor(std::vector& armors) const; + +private: + class Impl; + std::unique_ptr pimpl_; +}; + +} \ No newline at end of file diff --git a/src/tongji/predictor/extended_kalman_filter.hpp b/src/tongji/predictor/extended_kalman_filter.hpp index f016491..9f80180 100644 --- a/src/tongji/predictor/extended_kalman_filter.hpp +++ b/src/tongji/predictor/extended_kalman_filter.hpp @@ -16,7 +16,7 @@ class ExtendedKalmanFilter { using PMat = Eigen::Matrix; using PDig = Eigen::Matrix; using RMat = Eigen::Matrix; - using RDig = Eigen::Matrix; + using RDig = Eigen::Matrix; using QMat = Eigen::Matrix; using HMat = Eigen::Matrix; diff --git a/src/tongji/predictor/live_target_manager.cpp b/src/tongji/predictor/live_target_manager.cpp index c5e6ba2..1b989b9 100644 --- a/src/tongji/predictor/live_target_manager.cpp +++ b/src/tongji/predictor/live_target_manager.cpp @@ -1,28 +1,34 @@ #include "live_target_manager.hpp" #include +#include #include #include -#include #include "enum/armor_id.hpp" #include "enum/car_id.hpp" -#include "enum/enum_tools.hpp" #include "interfaces/predictor_update_package.hpp" #include "tongji/predictor/in_gimbal_control_armor.hpp" #include "tongji/predictor/live_target.hpp" #include "tongji/predictor/target_snapshot_manager.hpp" -#include "tongji/predictor/time_stamp.hpp" -#include "tongji/state_machine/state_machine.hpp" +#include "tongji/predictor/tracker.hpp" #include "util/index.hpp" +#include "util/math.hpp" namespace world_exe::tongji::predictor { class LiveTargetManager::Impl { public: - Impl(std::shared_ptr state_machine, double timeout_sec = 0.1) - : timeout_sec_(timeout_sec) - , state_machine_(state_machine) { } + Impl(const double& time_delay, const double& yaw_offset, const double& pitch_offset, + double timeout_sec = 0.1) + : targets_map_() + , tracker_(std::make_unique()) + , last_update_timestamp_(std::time(nullptr)) + , tracking_id_(enumeration::CarIDFlag::None) + , time_delay_(time_delay) + , yaw_offset_(yaw_offset) + , pitch_offset_(pitch_offset) + , timeout_sec_(timeout_sec) { } std::shared_ptr Predict( const enumeration::ArmorIdFlag& flag, const std::time_t& time_stamp) { @@ -30,142 +36,118 @@ class LiveTargetManager::Impl { result; for (auto id : util::enumeration::ExpandArmorIdFlags(flag)) { - auto it = targets_.find(id); - if (it != targets_.end() && it->second && it->second->IsConverged()) { + auto it = targets_map_.find(id); + if (it != targets_map_.end() && it->second && it->second->IsConverged()) { auto spacings = it->second->GetArmorGimbalControlSpacings(); - result[id] = std::move(spacings); + result[id] = spacings; } } return std::make_shared(result, time_stamp); - } + } // TODO: ** 目前 ** 我认为这个函数是多余的 std::shared_ptr GetPredictor( const enumeration::ArmorIdFlag& flag) const { - std::unordered_map> snapshot_map; - for (auto id : util::enumeration::ExpandArmorIdFlags(flag)) { - auto it = targets_.find(id); - if (it != targets_.end() && it->second && it->second->IsConverged()) { - snapshot_map[id] = it->second; - } - } + if (targets_map_.empty()) return nullptr; // TODO - return std::make_shared(flag, snapshot_map); + return std::make_shared( + flag, targets_map_, last_update_timestamp_, bullet_speed_, yaw_offset_, pitch_offset_); } - void Update(std::shared_ptr data, double dt) { - const auto now = predictor::TimeStamp(std::time(nullptr)); - RemoveLostTargets(now); - AddNewTargets(data, dt); - // TODO:update the state_machine + void Update(std::shared_ptr data, + const std::shared_ptr& armors_in_image, const std::time_t& now, + const double& bullet_speed) { + + UpdateTimeStamp(data->GetTimeStamped().GetTimeStamp()); + UpdateTargetMap(data, now); + UpdateTarget(data, armors_in_image, now); + UpdateBulletSpeed(bullet_speed); } - auto GetLiveTargetIDs() -> enumeration::CarIDFlag const { - enumeration::CarIDFlag result = enumeration::CarIDFlag::None; - for (const auto& [id, target] : targets_) { - if (target && target->IsConverged()) { - result = static_cast( - static_cast(result) | static_cast(id)); - } + auto GetAllowedTargetID() const -> enumeration::CarIDFlag const { + if (targets_map_.at(tracking_id_)->IsConverged()) { + return tracking_id_; } - return result; + return enumeration::CarIDFlag::None; } private: - void RegisterTarget(enumeration::ArmorIdFlag id, std::shared_ptr target) { - targets_[id] = std::move(target); - } - void RemoveTarget(enumeration::ArmorIdFlag id) { targets_.erase(id); } - bool HasTarget(enumeration::ArmorIdFlag id) const { return targets_.count(id) > 0; } - std::shared_ptr GetTarget(enumeration::ArmorIdFlag id) const { - auto it = targets_.find(id); - return (it != targets_.end()) ? it->second : nullptr; - } - - void AddNewTargets( - const std::shared_ptr& data, double dt) { + void UpdateBulletSpeed(const double& bullet_speed) { bullet_speed_ = bullet_speed; } + void UpdateTimeStamp(const time_t& time_stamp) { last_update_timestamp_ = time_stamp; } + void UpdateTargetMap( + std::shared_ptr data, const std::time_t& now) { const Eigen::Affine3d transform = data->GetTransform(); const Eigen::Matrix3d rotation_matrix = transform.linear(); - const Eigen::Quaterniond rotation_quat(rotation_matrix); + const auto armors_interface = data->GetArmors(); - const auto& armors_interface = data->GetArmors(); - for (int i; i < 8; i++) { + targets_map_.clear(); + for (int i; i < static_cast(enumeration::CarIDFlag::Count); i++) { auto id = static_cast( static_cast(enumeration::CarIDFlag::Hero) << i); - const auto& armors = armors_interface->GetArmors(id); - - if (armors.empty()) continue; - UpdateStateMachine(id, std::time(nullptr)); + const auto& armors_list = armors_interface->GetArmors(id); + if (armors_list.empty()) return; - if (!HasTarget(id)) { - const auto& armor = armors.front(); - const Eigen::Vector3d position_in_world = transform * armor.position; - const Eigen::Vector3d ypr_in_world = rotation_matrix.eulerAngles(2, 1, 0); // ZYX + const auto& armor = armors_list.front(); + const Eigen::Vector3d xyz_in_world = transform * armor.position; + const Eigen::Vector3d ypr_in_world = rotation_matrix.eulerAngles(2, 1, 0); // ZYX + targets_map_[id] = + std::move(std::make_shared(xyz_in_world, ypr_in_world, id)); + } + } - auto target = std::make_shared(position_in_world, ypr_in_world, id); - RegisterTarget(id, target); - continue; - } + void UpdateTarget(std::shared_ptr data, + const std::shared_ptr& armors_in_image, const std::time_t& now) { + const Eigen::Affine3d transform = data->GetTransform(); + const Eigen::Matrix3d rotation_matrix = transform.linear(); + const auto armors_interface = data->GetArmors(); - auto target = GetTarget(id); - for (const auto& armor : armors) { - const Eigen::Vector3d position_in_world = transform * armor.position; - const Eigen::Vector3d ypr = - util::math::quaternion_to_euler(Eigen::Quaterniond(rotation_matrix), 2, 1, 0); + tracking_id_ = tracker_->SelectTrackingTargetID(armors_in_image, now); - target->Update(dt, position_in_world, ypr, util::math::xyz2ypd(position_in_world)); - } - } - } + const auto& armors_list = armors_interface->GetArmors(tracking_id_); + if (armors_list.empty()) return; - void RemoveLostTargets(const predictor::TimeStamp& now) { - for (auto it = targets_.begin(); it != targets_.end();) { - if (IsTargetLost(it->second, now)) { - it = targets_.erase(it); - } else { - ++it; - } - } + const auto& armor = armors_list.front(); + const Eigen::Vector3d xyz_in_world = transform * armor.position; + const Eigen::Vector3d ypr_in_world = rotation_matrix.eulerAngles(2, 1, 0); // ZYX + targets_map_[tracking_id_]->Update(static_cast(now - last_update_timestamp_), + xyz_in_world, ypr_in_world, util::math::xyz2ypd(xyz_in_world)); } - void UpdateStateMachine(enumeration::CarIDFlag cars_detected, TimeStamp now) { - state_machine_->Update(cars_detected, now.GetTimeStamp()); - } + std::unordered_map> targets_map_; + std::unique_ptr tracker_; + std::time_t last_update_timestamp_; + enumeration::CarIDFlag tracking_id_; - bool IsTargetLost( - const std::shared_ptr& target, const predictor::TimeStamp& now) const { - return now.SecondsSince(target->LastSeen()) > timeout_sec_; - } + double bullet_speed_; + const double time_delay_; + const double yaw_offset_; + const double pitch_offset_; - std::unordered_map> targets_; - std::shared_ptr state_machine_; - const double timeout_sec_ { 0.1 }; + const double timeout_sec_; }; -LiveTargetManager::LiveTargetManager( - std::shared_ptr state_machine, double timeout_sec) - : pimpl_(std::make_unique(state_machine, timeout_sec)) { } +LiveTargetManager::LiveTargetManager(const double& time_delay, const double& yaw_offset, + const double& pitch_offset, double timeout_sec) + : pimpl_(std::make_unique(time_delay, yaw_offset, pitch_offset, timeout_sec)) { } LiveTargetManager::~LiveTargetManager() = default; std ::shared_ptr LiveTargetManager::Predict( const enumeration ::ArmorIdFlag& id, const std ::time_t& time_stamp) { return pimpl_->Predict(id, time_stamp); } - std ::shared_ptr LiveTargetManager::GetPredictor( const enumeration ::ArmorIdFlag& id) const { return pimpl_->GetPredictor(id); } -auto LiveTargetManager::GetLiveTargetIDs() -> enumeration::CarIDFlag const { - return pimpl_->GetLiveTargetIDs(); +void LiveTargetManager::Update(std::shared_ptr data, + const std::shared_ptr& armors_in_image, const std::time_t& now, + const double& bullet_speed) { + return pimpl_->Update(data, armors_in_image, now, bullet_speed); } - -void LiveTargetManager::Update( - std::shared_ptr data, double dt) { - return pimpl_->Update(data, dt); +auto LiveTargetManager::GetAllowedTargetID() const -> enumeration::ArmorIdFlag const { + return pimpl_->GetAllowedTargetID(); } - } \ No newline at end of file diff --git a/src/tongji/predictor/live_target_manager.hpp b/src/tongji/predictor/live_target_manager.hpp index 7a1c978..88ebfce 100644 --- a/src/tongji/predictor/live_target_manager.hpp +++ b/src/tongji/predictor/live_target_manager.hpp @@ -3,28 +3,28 @@ #include #include "enum/armor_id.hpp" -#include "enum/car_id.hpp" +#include "interfaces/armor_in_image.hpp" #include "interfaces/predictor_update_package.hpp" #include "interfaces/target_predictor.hpp" -#include "tongji/state_machine/state_machine.hpp" namespace world_exe::tongji::predictor { class LiveTargetManager final : public interfaces::ITargetPredictor { public: - LiveTargetManager( - std::shared_ptr state_machine, double timeout_sec = 0.1); + LiveTargetManager(const double& time_delay, const double& yaw_offset, + const double& pitch_offset, double timeout_sec = 0.1); ~LiveTargetManager(); std ::shared_ptr Predict( const enumeration ::ArmorIdFlag& id, const std ::time_t& time_stamp) override; - std ::shared_ptr GetPredictor( const enumeration ::ArmorIdFlag& id) const override; - auto GetLiveTargetIDs() -> enumeration::CarIDFlag const; + void Update(std::shared_ptr data, + const std::shared_ptr& armors_in_image, const std::time_t& now, + const double& bullet_speed); - void Update(std::shared_ptr data, double dt); + auto GetAllowedTargetID() const -> enumeration::ArmorIdFlag const; private: class Impl; diff --git a/src/tongji/predictor/target_snapshot.hpp b/src/tongji/predictor/target_snapshot.hpp index 5dd41b9..e1a73af 100644 --- a/src/tongji/predictor/target_snapshot.hpp +++ b/src/tongji/predictor/target_snapshot.hpp @@ -2,7 +2,6 @@ #include -#include "data/armor_gimbal_control_spacing.hpp" #include "extended_kalman_filter.hpp" #include "tongji/predictor/live_target.hpp" #include "tongji/predictor/predict_model.hpp" @@ -17,21 +16,21 @@ class TargetSnapshot { , ekf_(target.GetEkfX(), target.GetP0Dig().asDiagonal(), model_.x_add) , time_stamp_(target.LastSeen()) { } - std::vector GetArmorGimbalControlSpacings() const { - std::vector armors; - for (int id = 0; id < model_.GetArmorNum(); id++) { - auto angle = - util::math::clamp_pm_pi(this->ekf_.x[6] + id * 2 * CV_PI / model_.GetArmorNum()); - auto xyz = model_.h_armor_xyz(this->ekf_.x, id); - - data::ArmorGimbalControlSpacing armor; - armor.id = model_.GetID(); - armor.position = xyz; - armor.orientation = util::math::euler_to_quaternion(angle, 15. / 180. * CV_PI, 0); - armors.emplace_back(std::move(armor)); - } - return armors; - } + // std::vector GetArmorGimbalControlSpacings() const { + // std::vector armors; + // for (int id = 0; id < model_.GetArmorNum(); id++) { + // auto angle = + // util::math::clamp_pm_pi(this->ekf_.x[6] + id * 2 * CV_PI / model_.GetArmorNum()); + // auto xyz = model_.h_armor_xyz(this->ekf_.x, id); + + // data::ArmorGimbalControlSpacing armor; + // armor.id = model_.GetID(); + // armor.position = xyz; + // armor.orientation = util::math::euler_to_quaternion(angle, 15. / 180. * CV_PI, 0); + // armors.emplace_back(std::move(armor)); + // } + // return armors; + // } auto GetPredictedXYZAList(const double& dt) -> std::vector const { return model_.GetArmorXYZAList(this->Predict(dt)); diff --git a/src/tongji/predictor/target_snapshot_manager.cpp b/src/tongji/predictor/target_snapshot_manager.cpp index 64b5fa8..f2e3240 100644 --- a/src/tongji/predictor/target_snapshot_manager.cpp +++ b/src/tongji/predictor/target_snapshot_manager.cpp @@ -2,64 +2,99 @@ #include #include +#include #include "data/armor_gimbal_control_spacing.hpp" #include "enum/enum_tools.hpp" #include "in_gimbal_control_armor.hpp" +#include "tongji/armor_solver/aim_solver.hpp" +#include "tongji/predictor/live_target.hpp" #include "tongji/predictor/target_snapshot.hpp" namespace world_exe::tongji::predictor { + class TargetSnapshotManager::Impl { public: Impl(const enumeration::ArmorIdFlag& id, - const std::unordered_map>& snapshots) - : id_(id) { - for (const auto& [flag, target] : snapshots) { - snapshots_.emplace(flag, TargetSnapshot(*target)); - } - } + const std::unordered_map>& + live_target_map, + const std::time_t& now, const double& bullet_speed, const double& yaw_offset, + const double& pitch_offset) + : aim_solver_(std::make_unique(yaw_offset, pitch_offset)) + + , now_(now) + , ids_(id) + , bullet_speed_(bullet_speed) + , snapshots_(BuildSnapshots(live_target_map)) + , gimbal_command_({ std::numeric_limits::quiet_NaN(), + std::numeric_limits::quiet_NaN() }) { } - const enumeration::ArmorIdFlag& GetId() const { return id_; } + const enumeration::ArmorIdFlag& GetId() const { return ids_; } std::shared_ptr Predictor( const std::time_t& time_stamp) const { + std::unordered_map> result; - for (const auto& [flag, snapshot] : snapshots_) { - result[flag] = snapshot.GetArmorGimbalControlSpacings(); + for (const auto& [id, snapshot] : snapshots_) { + auto aim_solution = + aim_solver_->SolveAimSolution(std::make_unique(snapshot), + bullet_speed_, snapshot.GetTimeStamp().GetTimeStamp()); + + if (!aim_solution.valid) continue; + + auto target_pos = aim_solution.aim_point.head<3>(); + auto armor_yaw = aim_solution.aim_point[3]; + result.emplace(id, + std::vector { + data::ArmorGimbalControlSpacing { id, target_pos, + util::math::euler_to_quaternion(armor_yaw, 15. / 180. * CV_PI, 0) } }); + + gimbal_command_.yaw = aim_solution.yaw; + gimbal_command_.pitch = aim_solution.pitch; } return std::make_shared(result, time_stamp); } - const std::unique_ptr GetSingleSnapshot( - const enumeration::ArmorIdFlag& id) const { - auto it = snapshots_.find(id); - if (it != snapshots_.end()) { - return std::make_unique(it->second); + auto GetGimbalCommand() const -> GimbalCommand const { return gimbal_command_; } + +private: + static std::unordered_map BuildSnapshots( + const std::unordered_map>& input) { + std::unordered_map result; + for (const auto& [id, target] : input) { + if (target) { + result.emplace(id, TargetSnapshot(*target)); + } } - return nullptr; + return result; } -private: - std::unordered_map snapshots_; - enumeration::ArmorIdFlag id_; + std::unique_ptr aim_solver_; + const std::unordered_map snapshots_; + const std::time_t& now_; + const enumeration::ArmorIdFlag ids_; + const double bullet_speed_; + mutable GimbalCommand gimbal_command_; }; TargetSnapshotManager::TargetSnapshotManager(const enumeration::ArmorIdFlag& id, - const std::unordered_map> snapshots) - : pimpl_(std::make_unique(id, snapshots)) { } + const std::unordered_map>& + live_target_map, + const std::time_t& now, const double& bullet_speed, const double& yaw_offset, + const double& pitch_offset) + : pimpl_(std::make_unique( + id, live_target_map, now, bullet_speed, yaw_offset, pitch_offset)) { } TargetSnapshotManager::~TargetSnapshotManager() = default; const enumeration ::ArmorIdFlag& TargetSnapshotManager::GetId() const { return pimpl_->GetId(); } - -const std::unique_ptr TargetSnapshotManager::GetSingleSnapshot( - const enumeration::ArmorIdFlag& id) const { - return pimpl_->GetSingleSnapshot(id); -} - std ::shared_ptr TargetSnapshotManager::Predictor( const std ::time_t& time_stamp) const { return pimpl_->Predictor(time_stamp); } + +auto TargetSnapshotManager::GetGimbalCommand() const -> GimbalCommand const { + return pimpl_->GetGimbalCommand(); +} } \ No newline at end of file diff --git a/src/tongji/predictor/target_snapshot_manager.hpp b/src/tongji/predictor/target_snapshot_manager.hpp index 1353394..34e2616 100644 --- a/src/tongji/predictor/target_snapshot_manager.hpp +++ b/src/tongji/predictor/target_snapshot_manager.hpp @@ -6,24 +6,29 @@ #include "enum/armor_id.hpp" #include "interfaces/predictor.hpp" #include "tongji/predictor/live_target.hpp" -#include "tongji/predictor/target_snapshot.hpp" namespace world_exe::tongji::predictor { +struct GimbalCommand { + double yaw; + double pitch; +}; + class TargetSnapshotManager final : public interfaces::IPredictor { public: TargetSnapshotManager(const enumeration::ArmorIdFlag& id, - const std::unordered_map> snapshots); + const std::unordered_map>& + live_target_map, + const std::time_t& now, const double& bullet_speed, const double& yaw_offset, + const double& pitch_offset); ~TargetSnapshotManager(); const enumeration ::ArmorIdFlag& GetId() const override; - - const std::unique_ptr GetSingleSnapshot( - const enumeration::ArmorIdFlag& id) const; - std ::shared_ptr Predictor( const std ::time_t& time_stamp) const override; + auto GetGimbalCommand() const -> GimbalCommand const; + private: class Impl; std::unique_ptr pimpl_; diff --git a/src/tongji/fire_controller/tracker.hpp b/src/tongji/predictor/tracker.hpp similarity index 56% rename from src/tongji/fire_controller/tracker.hpp rename to src/tongji/predictor/tracker.hpp index 68b5d4a..1af1f6b 100644 --- a/src/tongji/fire_controller/tracker.hpp +++ b/src/tongji/predictor/tracker.hpp @@ -1,25 +1,19 @@ #pragma once -#include #include - #include +#include #include -#include "../predictor/target_snapshot.hpp" -#include "../predictor/target_snapshot_manager.hpp" #include "enum/armor_id.hpp" -#include "enum/car_id.hpp" -#include "enum/enum_tools.hpp" -#include "interfaces/armor_in_image.hpp" -#include "interfaces/car_state.hpp" -#include "tongji/decider/armor_info.hpp" -#include "tongji/decider/decider.hpp" -#include "tongji/fire_controller/aim_point_chooser.hpp" +#include "tongji/identifier/armor_filter.hpp" #include "tongji/identifier/identified_armor.hpp" +#include "tongji/predictor/decider.hpp" +#include "tongji/predictor/target_snapshot.hpp" +#include "tongji/predictor/target_snapshot_manager.hpp" #include "tongji/predictor/time_stamp.hpp" -namespace world_exe::tongji::fire_control { +namespace world_exe::tongji::predictor { enum class TrackState { Lost, // @@ -33,81 +27,55 @@ class Tracker final { using TargetSnapshotManager = world_exe::tongji::predictor::TargetSnapshotManager; using TargetSnapshot = world_exe::tongji::predictor::TargetSnapshot; using ArmorInImage = world_exe::tongji::identifier::IdentifiedArmor; - using EnemiesState = world_exe::interfaces::ICarState; public: Tracker() - : last_timestamp_(std::time(nullptr)) - , decider_(std::make_unique()) { } + : armor_filter_(std::make_unique()) + , decider_(std::make_unique()) + , last_track_timestamp_(std::time(nullptr)) { } ~Tracker() = default; - auto SelectTrackingTarget(std::shared_ptr armors_in_image, - const std::shared_ptr& snapshot_manager_) noexcept - -> std::unique_ptr { - - auto detected_ids = enumeration::CarIDFlag::None; - auto armor_info_list = std::vector { }; + auto SelectTrackingTargetID(const std::shared_ptr& armors_in_image, + const std::time_t& now) noexcept -> enumeration::ArmorIdFlag const { + CheckCameraOffline(now); + last_track_timestamp_.SetTimeStamp(now); - for (uint32_t i = 0; i < static_cast(CarIDFlag::Count); ++i) { - auto id = static_cast(static_cast(CarIDFlag::Hero) << i); + auto filtered_ids = enumeration::ArmorIdFlag::None; + auto detected_ids = enumeration::ArmorIdFlag::None; + std::vector filtered_armors; + for (uint32_t i = 0; i < static_cast(enumeration::ArmorIdFlag::Count); ++i) { + auto id = static_cast( + static_cast(enumeration::ArmorIdFlag::Hero) << i); if (armors_in_image->GetArmors(id).empty()) continue; - detected_ids = static_cast( + // 图像中出现的装甲板 + auto armors = armors_in_image->GetArmors(id); + detected_ids = static_cast( static_cast(detected_ids) | static_cast(id)); - for (const auto& armor : armors_in_image->GetArmors(id)) { - auto armor_info = decider::ArmorInfo(armor); - armor_info_list.emplace_back(armor_info); + // 对从图像识别到的装甲板进行过滤 + filtered_armors = std::move(armor_filter_->FilterArmor(std::move(armors))); + if (!filtered_armors.empty()) { + filtered_ids = + static_cast(static_cast(filtered_ids) + | static_cast(filtered_armors.at(0).id)); } } - decider_->SetInvincibleArmors(invincible_armors_); - auto is_empty = decider_->ArmorFilter(armor_info_list); - decider_->SetPriority(armor_info_list); - - auto sorted_id = decider_->GetSortedArmor(armor_info_list); - if (sorted_id == enumeration::ArmorIdFlag::None) { - return nullptr; - } - - auto snapshot = snapshot_manager_->GetSingleSnapshot(sorted_id); - - if (state_ == TrackState::Tracking) { - SetState(TrackState::Switching); - temp_lost_count_ = 0; - } - - auto now = predictor::TimeStamp(std::time(nullptr)); - if (state_ != TrackState::Lost && now.SecondsSince(last_timestamp_) > 0.1) { - SetState(TrackState::Lost); - ResetTracking(); - return nullptr; - } - - last_timestamp_ = now; - tracking_car_id_ = snapshot->GetID(); - return snapshot; + UpdateState(!(detected_ids == enumeration::ArmorIdFlag::None)); - ResetTracking(); - return nullptr; + tracking_car_id_ = decider_->GetBestArmor(filtered_armors); + return tracking_car_id_; } - void SetInvincibleArmors(const CarIDFlag& invincible_armors) { - invincible_armors_ = invincible_armors; - } - - world_exe::enumeration::CarIDFlag GetCurrentTargetID() const { return tracking_car_id_; } - void UpdateState(bool found) { switch (state_) { case TrackState::Lost: { if (found) { SetState(TrackState::Detecting); - detect_count_++; - } else { - ResetTracking(); + detect_count_ = 1; } break; } @@ -167,6 +135,14 @@ class Tracker final { TrackState GetState() const { return state_; } private: + void CheckCameraOffline(const std::time_t& now) { + // TODO:If the underlying timestamp is std::time_t, then this if branch will never be + // entered + if (state_ != TrackState::Lost + && static_cast(now - last_track_timestamp_.GetTimeStamp()) < 0.1) + SetState(TrackState::Lost); + } + void SetState(TrackState new_state) { pre_state_ = state_; state_ = new_state; @@ -177,7 +153,9 @@ class Tracker final { world_exe::enumeration::CarIDFlag tracking_car_id_ { enumeration::CarIDFlag::None }; TrackState state_ = TrackState::Lost; TrackState pre_state_ = TrackState::Lost; - CarIDFlag invincible_armors_ { CarIDFlag::None }; + + std::unique_ptr armor_filter_; + std::unique_ptr decider_; int detect_count_ = 0; int temp_lost_count_ = 0; @@ -187,9 +165,7 @@ class Tracker final { const int normal_max_temp_lost_count_ = max_temp_lost_count_; const int max_switch_count_ = 200; - predictor::TimeStamp last_timestamp_; - - std::unique_ptr decider_; + predictor::TimeStamp last_track_timestamp_; }; } \ No newline at end of file diff --git a/src/tongji/state_machine/car_state_manager.hpp b/src/tongji/state_machine/car_state_manager.hpp deleted file mode 100644 index 6592a63..0000000 --- a/src/tongji/state_machine/car_state_manager.hpp +++ /dev/null @@ -1,55 +0,0 @@ -#pragma once - -#include - -#include "tongji/predictor/time_stamp.hpp" - -namespace world_exe::tongji::car_state { - -using TimeStamp = predictor::TimeStamp; -class CarStateManager { -public: - CarStateManager(int switch_threshold = 5) - : switch_threshold_(switch_threshold) - , last_seen_(std::time(nullptr)) { } - - void Update(bool detected, const TimeStamp& now) { - if (detected) { - count_ = std::min(count_ + 1, switch_threshold_); - last_seen_ = now; - update_count_++; - if (update_count_ >= converge_threshold_ && !is_diverged_) is_converged_ = true; - } else { - count_ = std::max(count_ - 1, 0); - } - is_locked_ = (count_ >= switch_threshold_); - } - - bool IsConverged() const { return is_converged_ && !is_diverged_; } - - void Reset() { - count_ = 0; - update_count_ = 0; - is_locked_ = false; - is_converged_ = false; - is_diverged_ = true; - last_seen_ = predictor::TimeStamp(std::time(nullptr)); - } - - void SetThreshold(const int& value) { switch_threshold_ = value; } - - predictor::TimeStamp LastSeen() const { return last_seen_; } - -private: - int count_ = 0; - int switch_threshold_; - int converge_threshold_ = 3; - int update_count_ = 0; - - bool is_locked_ = false; - bool is_converged_ = false; - bool is_diverged_ = true; - - predictor::TimeStamp last_seen_; -}; -} \ No newline at end of file diff --git a/src/tongji/state_machine/state_machine.cpp b/src/tongji/state_machine/state_machine.cpp index fc0ffa8..b6bb318 100644 --- a/src/tongji/state_machine/state_machine.cpp +++ b/src/tongji/state_machine/state_machine.cpp @@ -1,70 +1,38 @@ #include "state_machine.hpp" -#include -#include +#include #include "enum/car_id.hpp" -#include "tongji/state_machine/car_state_manager.hpp" -#include "util/index.hpp" +#include "interfaces/target_predictor.hpp" +#include "tongji/predictor/live_target_manager.hpp" namespace world_exe::tongji::state_machine { -using TimeStamp = car_state::TimeStamp; class StateMachine::Impl { public: - Impl() - : car_states_ { } { - std::generate( - car_states_.begin(), car_states_.end(), []() { return car_state::CarStateManager(); }); - } - const enumeration::CarIDFlag& GetAllowdToFires() const { return tracing_state_; } - - const car_state::CarStateManager& GetState(enumeration::CarIDFlag single_id) const { - return car_states_[util::enumeration::GetIndex(single_id)]; - } - - void Update(const enumeration::CarIDFlag& car_detected, const std::time_t& now) { - tracing_state_ = enumeration::CarIDFlag::None; - - for (int i = 0; i < static_cast(enumeration::CarIDFlag::Count); ++i) { - auto id = static_cast(1 << i); - bool detected = (static_cast(car_detected) >> i) & 0x01; - - auto& state = car_states_[i]; - state.Update(detected, now); + Impl(std::shared_ptr live_target_manager) + : live_target_manager_(std::move(live_target_manager)) { } - if (!state.IsConverged()) { - state.Reset(); - continue; - } + const enumeration::CarIDFlag& GetAllowdToFires() const { return target_ids_; } - tracing_state_ = static_cast( - static_cast(tracing_state_) | static_cast(id)); - } + void Update() { + auto live_target_manager = + std::dynamic_pointer_cast(live_target_manager_); + target_ids_ = live_target_manager->GetAllowedTargetID(); } private: - enumeration::CarIDFlag tracing_state_ = enumeration::CarIDFlag::None; - std::array car_states_; + std::shared_ptr live_target_manager_; + enumeration::CarIDFlag target_ids_; }; -StateMachine::StateMachine() - : pimpl_(std::make_unique()) { } - +StateMachine::StateMachine( + std::shared_ptr live_target_manager) + : pimpl_(std::make_unique(live_target_manager)) { } StateMachine::~StateMachine() = default; const enumeration::CarIDFlag& StateMachine::GetAllowdToFires() const { return pimpl_->GetAllowdToFires(); } -const car_state::CarStateManager& StateMachine::GetState(enumeration::CarIDFlag single_id) const { - return pimpl_->GetState(single_id); -} - -const interfaces::ICarState& StateMachine::Update( - const enumeration::CarIDFlag& car_detected, const std::time_t& now) { - pimpl_->Update(car_detected, now); - return *this; -} - } diff --git a/src/tongji/state_machine/state_machine.hpp b/src/tongji/state_machine/state_machine.hpp index 780fe07..c3417e3 100644 --- a/src/tongji/state_machine/state_machine.hpp +++ b/src/tongji/state_machine/state_machine.hpp @@ -5,7 +5,7 @@ #include "enum/car_id.hpp" #include "interfaces/car_state.hpp" -#include "tongji/state_machine/car_state_manager.hpp" +#include "interfaces/target_predictor.hpp" namespace world_exe::tongji::state_machine { class StateMachine final : public interfaces::ICarState { @@ -14,9 +14,7 @@ class StateMachine final : public interfaces::ICarState { ~StateMachine(); const enumeration ::CarIDFlag& GetAllowdToFires() const override; - const car_state::CarStateManager& GetState(enumeration::CarIDFlag single_id) const; - const interfaces::ICarState& Update( - const enumeration::CarIDFlag& car_detected, const std::time_t& now); + StateMachine(std::shared_ptr live_target_manager); private: class Impl; From 7b15810673fbdd19c2b4460391e00151f5f2ccbe Mon Sep 17 00:00:00 2001 From: heyeuu <2829004293@qq.com> Date: Mon, 13 Oct 2025 12:17:48 +0800 Subject: [PATCH 52/53] refactor(project): reorganize directory structure to match new architecture --- src/tongji/fire_controller/fire_controller.cpp | 10 +++++----- src/tongji/fire_controller/fire_controller.hpp | 2 +- src/tongji/fire_controller/fire_decision.hpp | 2 +- src/tongji/identifier/identifier.cpp | 2 +- src/tongji/predictor/in_gimbal_control_armor.hpp | 2 +- .../{ => kalman_filter}/extended_kalman_filter.hpp | 3 ++- .../predictor/{ => kalman_filter}/predict_model.hpp | 2 +- .../predictor/{ => live_target_manager}/decider.cpp | 0 .../predictor/{ => live_target_manager}/decider.hpp | 0 .../{ => live_target_manager}/live_target.hpp | 8 ++++---- .../live_target_manager.cpp | 8 ++++---- .../live_target_manager.hpp | 0 .../predictor/{ => live_target_manager}/tracker.hpp | 12 ++++++------ .../target_snapshot_manager}/aim_point_chooser.hpp | 0 .../target_snapshot_manager}/aim_solver.hpp | 6 +++--- .../target_snapshot.hpp | 8 ++++---- .../target_snapshot_manager.cpp | 8 ++++---- .../target_snapshot_manager.hpp | 2 +- .../target_snapshot_manager}/trajectory.hpp | 0 src/tongji/state_machine/state_machine.cpp | 2 +- src/tongji/{predictor => time_stamp}/time_stamp.hpp | 0 21 files changed, 39 insertions(+), 38 deletions(-) rename src/tongji/predictor/{ => kalman_filter}/extended_kalman_filter.hpp (99%) rename src/tongji/predictor/{ => kalman_filter}/predict_model.hpp (99%) rename src/tongji/predictor/{ => live_target_manager}/decider.cpp (100%) rename src/tongji/predictor/{ => live_target_manager}/decider.hpp (100%) rename src/tongji/predictor/{ => live_target_manager}/live_target.hpp (95%) rename src/tongji/predictor/{ => live_target_manager}/live_target_manager.cpp (96%) rename src/tongji/predictor/{ => live_target_manager}/live_target_manager.hpp (100%) rename src/tongji/predictor/{ => live_target_manager}/tracker.hpp (95%) rename src/tongji/{armor_solver => predictor/target_snapshot_manager}/aim_point_chooser.hpp (100%) rename src/tongji/{armor_solver => predictor/target_snapshot_manager}/aim_solver.hpp (95%) rename src/tongji/predictor/{ => target_snapshot_manager}/target_snapshot.hpp (89%) rename src/tongji/predictor/{ => target_snapshot_manager}/target_snapshot_manager.cpp (95%) rename src/tongji/predictor/{ => target_snapshot_manager}/target_snapshot_manager.hpp (94%) rename src/tongji/{armor_solver => predictor/target_snapshot_manager}/trajectory.hpp (100%) rename src/tongji/{predictor => time_stamp}/time_stamp.hpp (100%) diff --git a/src/tongji/fire_controller/fire_controller.cpp b/src/tongji/fire_controller/fire_controller.cpp index b4f79e6..b3eccd7 100644 --- a/src/tongji/fire_controller/fire_controller.cpp +++ b/src/tongji/fire_controller/fire_controller.cpp @@ -3,13 +3,13 @@ #include #include +#include "../identifier/identified_armor.hpp" +#include "../predictor/live_target_manager/live_target_manager.hpp" +#include "../predictor/target_snapshot_manager/target_snapshot_manager.hpp" +#include "../state_machine/state_machine.hpp" #include "data/fire_control.hpp" +#include "fire_decision.hpp" #include "interfaces/target_predictor.hpp" -#include "tongji/fire_controller/fire_decision.hpp" -#include "tongji/identifier/identified_armor.hpp" -#include "tongji/predictor/live_target_manager.hpp" -#include "tongji/predictor/target_snapshot_manager.hpp" -#include "tongji/state_machine/state_machine.hpp" namespace world_exe::tongji::fire_control { diff --git a/src/tongji/fire_controller/fire_controller.hpp b/src/tongji/fire_controller/fire_controller.hpp index eed2838..f193145 100644 --- a/src/tongji/fire_controller/fire_controller.hpp +++ b/src/tongji/fire_controller/fire_controller.hpp @@ -2,11 +2,11 @@ #include +#include "../time_stamp/time_stamp.hpp" #include "interfaces/armor_in_image.hpp" #include "interfaces/car_state.hpp" #include "interfaces/fire_controller.hpp" #include "interfaces/target_predictor.hpp" -#include "tongji/predictor/time_stamp.hpp" namespace world_exe::tongji::fire_control { diff --git a/src/tongji/fire_controller/fire_decision.hpp b/src/tongji/fire_controller/fire_decision.hpp index 7c94956..46088e8 100644 --- a/src/tongji/fire_controller/fire_decision.hpp +++ b/src/tongji/fire_controller/fire_decision.hpp @@ -3,7 +3,7 @@ #include #include -#include "tongji/predictor/target_snapshot_manager.hpp" +#include "tongji/predictor/target_snapshot_manager/target_snapshot_manager.hpp" namespace world_exe::tongji::fire_control { diff --git a/src/tongji/identifier/identifier.cpp b/src/tongji/identifier/identifier.cpp index c314f74..f70af8f 100644 --- a/src/tongji/identifier/identifier.cpp +++ b/src/tongji/identifier/identifier.cpp @@ -19,7 +19,7 @@ #include "enum/armor_id.hpp" #include "identified_armor.hpp" #include "interfaces/armor_in_image.hpp" -#include "tongji/identifier/classifier.hpp" +#include "../identifier/classifier.hpp" #include "util/logger.hpp" #include "util/stringifier.hpp" diff --git a/src/tongji/predictor/in_gimbal_control_armor.hpp b/src/tongji/predictor/in_gimbal_control_armor.hpp index 117203a..cdf4981 100644 --- a/src/tongji/predictor/in_gimbal_control_armor.hpp +++ b/src/tongji/predictor/in_gimbal_control_armor.hpp @@ -4,10 +4,10 @@ #include #include +#include "../time_stamp/time_stamp.hpp" #include "data/armor_gimbal_control_spacing.hpp" #include "enum/armor_id.hpp" #include "interfaces/armor_in_gimbal_control.hpp" -#include "tongji/predictor/time_stamp.hpp" namespace world_exe::tongji::predictor { diff --git a/src/tongji/predictor/extended_kalman_filter.hpp b/src/tongji/predictor/kalman_filter/extended_kalman_filter.hpp similarity index 99% rename from src/tongji/predictor/extended_kalman_filter.hpp rename to src/tongji/predictor/kalman_filter/extended_kalman_filter.hpp index 9f80180..ce90cfd 100644 --- a/src/tongji/predictor/extended_kalman_filter.hpp +++ b/src/tongji/predictor/kalman_filter/extended_kalman_filter.hpp @@ -1,11 +1,12 @@ #pragma once -#include #include #include #include #include +#include + namespace world_exe::tongji::predictor { template // class ExtendedKalmanFilter { diff --git a/src/tongji/predictor/predict_model.hpp b/src/tongji/predictor/kalman_filter/predict_model.hpp similarity index 99% rename from src/tongji/predictor/predict_model.hpp rename to src/tongji/predictor/kalman_filter/predict_model.hpp index db04478..5af8395 100644 --- a/src/tongji/predictor/predict_model.hpp +++ b/src/tongji/predictor/kalman_filter/predict_model.hpp @@ -7,7 +7,7 @@ #include #include "enum/car_id.hpp" -#include "tongji/predictor/extended_kalman_filter.hpp" +#include "extended_kalman_filter.hpp" #include "util/math.hpp" namespace world_exe::tongji::predictor { diff --git a/src/tongji/predictor/decider.cpp b/src/tongji/predictor/live_target_manager/decider.cpp similarity index 100% rename from src/tongji/predictor/decider.cpp rename to src/tongji/predictor/live_target_manager/decider.cpp diff --git a/src/tongji/predictor/decider.hpp b/src/tongji/predictor/live_target_manager/decider.hpp similarity index 100% rename from src/tongji/predictor/decider.hpp rename to src/tongji/predictor/live_target_manager/decider.hpp diff --git a/src/tongji/predictor/live_target.hpp b/src/tongji/predictor/live_target_manager/live_target.hpp similarity index 95% rename from src/tongji/predictor/live_target.hpp rename to src/tongji/predictor/live_target_manager/live_target.hpp index d25bfdb..710c87c 100644 --- a/src/tongji/predictor/live_target.hpp +++ b/src/tongji/predictor/live_target_manager/live_target.hpp @@ -5,11 +5,11 @@ #include #include +#include "../../time_stamp/time_stamp.hpp" +#include "../kalman_filter/extended_kalman_filter.hpp" +#include "../kalman_filter/predict_model.hpp" #include "data/armor_gimbal_control_spacing.hpp" #include "enum/car_id.hpp" -#include "extended_kalman_filter.hpp" -#include "predict_model.hpp" -#include "tongji/predictor/time_stamp.hpp" namespace world_exe::tongji::predictor { @@ -36,7 +36,7 @@ class LiveTarget { 0, 0; ExtendedKalmanFilter<11, 4>::PMat P0 = model_.GetP0Dig().asDiagonal(); - ekf_ = ExtendedKalmanFilter<11, 4>( + ekf_ = ExtendedKalmanFilter<11, 4>( x0, P0, model_.x_add); // 初始化滤波器（预测量、预测量协方差） } diff --git a/src/tongji/predictor/live_target_manager.cpp b/src/tongji/predictor/live_target_manager/live_target_manager.cpp similarity index 96% rename from src/tongji/predictor/live_target_manager.cpp rename to src/tongji/predictor/live_target_manager/live_target_manager.cpp index 1b989b9..ad20219 100644 --- a/src/tongji/predictor/live_target_manager.cpp +++ b/src/tongji/predictor/live_target_manager/live_target_manager.cpp @@ -5,13 +5,13 @@ #include #include +#include "../in_gimbal_control_armor.hpp" +#include "../target_snapshot_manager/target_snapshot_manager.hpp" #include "enum/armor_id.hpp" #include "enum/car_id.hpp" #include "interfaces/predictor_update_package.hpp" -#include "tongji/predictor/in_gimbal_control_armor.hpp" -#include "tongji/predictor/live_target.hpp" -#include "tongji/predictor/target_snapshot_manager.hpp" -#include "tongji/predictor/tracker.hpp" +#include "live_target.hpp" +#include "tracker.hpp" #include "util/index.hpp" #include "util/math.hpp" diff --git a/src/tongji/predictor/live_target_manager.hpp b/src/tongji/predictor/live_target_manager/live_target_manager.hpp similarity index 100% rename from src/tongji/predictor/live_target_manager.hpp rename to src/tongji/predictor/live_target_manager/live_target_manager.hpp diff --git a/src/tongji/predictor/tracker.hpp b/src/tongji/predictor/live_target_manager/tracker.hpp similarity index 95% rename from src/tongji/predictor/tracker.hpp rename to src/tongji/predictor/live_target_manager/tracker.hpp index 1af1f6b..a03c12d 100644 --- a/src/tongji/predictor/tracker.hpp +++ b/src/tongji/predictor/live_target_manager/tracker.hpp @@ -5,13 +5,13 @@ #include #include +#include "../../identifier/armor_filter.hpp" +#include "../../identifier/identified_armor.hpp" +#include "../../time_stamp/time_stamp.hpp" +#include "../target_snapshot_manager/target_snapshot.hpp" +#include "../target_snapshot_manager/target_snapshot_manager.hpp" +#include "decider.hpp" #include "enum/armor_id.hpp" -#include "tongji/identifier/armor_filter.hpp" -#include "tongji/identifier/identified_armor.hpp" -#include "tongji/predictor/decider.hpp" -#include "tongji/predictor/target_snapshot.hpp" -#include "tongji/predictor/target_snapshot_manager.hpp" -#include "tongji/predictor/time_stamp.hpp" namespace world_exe::tongji::predictor { diff --git a/src/tongji/armor_solver/aim_point_chooser.hpp b/src/tongji/predictor/target_snapshot_manager/aim_point_chooser.hpp similarity index 100% rename from src/tongji/armor_solver/aim_point_chooser.hpp rename to src/tongji/predictor/target_snapshot_manager/aim_point_chooser.hpp diff --git a/src/tongji/armor_solver/aim_solver.hpp b/src/tongji/predictor/target_snapshot_manager/aim_solver.hpp similarity index 95% rename from src/tongji/armor_solver/aim_solver.hpp rename to src/tongji/predictor/target_snapshot_manager/aim_solver.hpp index db34fe3..732c8a2 100644 --- a/src/tongji/armor_solver/aim_solver.hpp +++ b/src/tongji/predictor/target_snapshot_manager/aim_solver.hpp @@ -5,9 +5,9 @@ #include #include -#include "tongji/armor_solver/aim_point_chooser.hpp" -#include "tongji/armor_solver/trajectory.hpp" -#include "tongji/predictor/target_snapshot.hpp" +#include "aim_point_chooser.hpp" +#include "target_snapshot.hpp" +#include "trajectory.hpp" namespace world_exe::tongji::armor_solver { diff --git a/src/tongji/predictor/target_snapshot.hpp b/src/tongji/predictor/target_snapshot_manager/target_snapshot.hpp similarity index 89% rename from src/tongji/predictor/target_snapshot.hpp rename to src/tongji/predictor/target_snapshot_manager/target_snapshot.hpp index e1a73af..f055594 100644 --- a/src/tongji/predictor/target_snapshot.hpp +++ b/src/tongji/predictor/target_snapshot_manager/target_snapshot.hpp @@ -2,10 +2,10 @@ #include -#include "extended_kalman_filter.hpp" -#include "tongji/predictor/live_target.hpp" -#include "tongji/predictor/predict_model.hpp" -#include "tongji/predictor/time_stamp.hpp" +#include "../../time_stamp/time_stamp.hpp" +#include "../kalman_filter/extended_kalman_filter.hpp" +#include "../kalman_filter/predict_model.hpp" +#include "../live_target_manager/live_target.hpp" namespace world_exe::tongji::predictor { diff --git a/src/tongji/predictor/target_snapshot_manager.cpp b/src/tongji/predictor/target_snapshot_manager/target_snapshot_manager.cpp similarity index 95% rename from src/tongji/predictor/target_snapshot_manager.cpp rename to src/tongji/predictor/target_snapshot_manager/target_snapshot_manager.cpp index f2e3240..a1d9614 100644 --- a/src/tongji/predictor/target_snapshot_manager.cpp +++ b/src/tongji/predictor/target_snapshot_manager/target_snapshot_manager.cpp @@ -4,12 +4,12 @@ #include #include +#include "../in_gimbal_control_armor.hpp" +#include "../live_target_manager/live_target.hpp" +#include "../target_snapshot_manager/target_snapshot.hpp" +#include "aim_solver.hpp" #include "data/armor_gimbal_control_spacing.hpp" #include "enum/enum_tools.hpp" -#include "in_gimbal_control_armor.hpp" -#include "tongji/armor_solver/aim_solver.hpp" -#include "tongji/predictor/live_target.hpp" -#include "tongji/predictor/target_snapshot.hpp" namespace world_exe::tongji::predictor { diff --git a/src/tongji/predictor/target_snapshot_manager.hpp b/src/tongji/predictor/target_snapshot_manager/target_snapshot_manager.hpp similarity index 94% rename from src/tongji/predictor/target_snapshot_manager.hpp rename to src/tongji/predictor/target_snapshot_manager/target_snapshot_manager.hpp index 34e2616..3176e9a 100644 --- a/src/tongji/predictor/target_snapshot_manager.hpp +++ b/src/tongji/predictor/target_snapshot_manager/target_snapshot_manager.hpp @@ -3,9 +3,9 @@ #include #include +#include "../live_target_manager/live_target.hpp" #include "enum/armor_id.hpp" #include "interfaces/predictor.hpp" -#include "tongji/predictor/live_target.hpp" namespace world_exe::tongji::predictor { diff --git a/src/tongji/armor_solver/trajectory.hpp b/src/tongji/predictor/target_snapshot_manager/trajectory.hpp similarity index 100% rename from src/tongji/armor_solver/trajectory.hpp rename to src/tongji/predictor/target_snapshot_manager/trajectory.hpp diff --git a/src/tongji/state_machine/state_machine.cpp b/src/tongji/state_machine/state_machine.cpp index b6bb318..990bf99 100644 --- a/src/tongji/state_machine/state_machine.cpp +++ b/src/tongji/state_machine/state_machine.cpp @@ -4,7 +4,7 @@ #include "enum/car_id.hpp" #include "interfaces/target_predictor.hpp" -#include "tongji/predictor/live_target_manager.hpp" +#include "tongji/predictor/live_target_manager/live_target_manager.hpp" namespace world_exe::tongji::state_machine { diff --git a/src/tongji/predictor/time_stamp.hpp b/src/tongji/time_stamp/time_stamp.hpp similarity index 100% rename from src/tongji/predictor/time_stamp.hpp rename to src/tongji/time_stamp/time_stamp.hpp From 9ccbf6526fbe1f4e73dcd69b97e886675f6a47b3 Mon Sep 17 00:00:00 2001 From: heyeuu <2829004293@qq.com> Date: Mon, 13 Oct 2025 13:39:42 +0800 Subject: [PATCH 53/53] refactor(namespace): update namespaces to align with new architecture --- src/tongji/fire_controller/fire_controller.cpp | 4 ++-- src/tongji/fire_controller/fire_controller.hpp | 2 +- src/tongji/predictor/in_gimbal_control_armor.hpp | 2 +- src/tongji/predictor/live_target_manager/live_target.hpp | 2 +- src/tongji/predictor/live_target_manager/tracker.hpp | 3 ++- .../predictor/target_snapshot_manager/aim_point_chooser.hpp | 2 +- src/tongji/predictor/target_snapshot_manager/aim_solver.hpp | 6 +++--- .../predictor/target_snapshot_manager/target_snapshot.hpp | 2 +- .../target_snapshot_manager/target_snapshot_manager.cpp | 5 ++--- src/tongji/predictor/target_snapshot_manager/trajectory.hpp | 2 +- src/tongji/time_stamp/time_stamp.hpp | 2 +- 11 files changed, 16 insertions(+), 16 deletions(-) diff --git a/src/tongji/fire_controller/fire_controller.cpp b/src/tongji/fire_controller/fire_controller.cpp index b3eccd7..6443340 100644 --- a/src/tongji/fire_controller/fire_controller.cpp +++ b/src/tongji/fire_controller/fire_controller.cpp @@ -18,7 +18,7 @@ using StateMachine = state_machine::StateMachine; using IdentifiedArmor = identifier::IdentifiedArmor; using CarIDFlag = enumeration::CarIDFlag; using LiveTargetManager = predictor::LiveTargetManager; -using TimeStamp = predictor::TimeStamp; +using TimeStamp = time_stamp::TimeStamp; class FireController::Impl { public: @@ -93,7 +93,7 @@ class FireController::Impl { std::shared_ptr identified_armors_; std::unique_ptr fire_decision_; - predictor::TimeStamp time_stamp_ { std::time(nullptr) }; + time_stamp::TimeStamp time_stamp_ { std::time(nullptr) }; std::shared_ptr state_machine_; std::shared_ptr live_target_manager_; diff --git a/src/tongji/fire_controller/fire_controller.hpp b/src/tongji/fire_controller/fire_controller.hpp index f193145..19343e0 100644 --- a/src/tongji/fire_controller/fire_controller.hpp +++ b/src/tongji/fire_controller/fire_controller.hpp @@ -21,7 +21,7 @@ class FireController final : public interfaces::IFireControl { void Update(std::shared_ptr armors, const double& gimbal_yaw); - predictor::TimeStamp GetTimeStamp() const; + time_stamp::TimeStamp GetTimeStamp() const; private: class Impl; diff --git a/src/tongji/predictor/in_gimbal_control_armor.hpp b/src/tongji/predictor/in_gimbal_control_armor.hpp index cdf4981..c012806 100644 --- a/src/tongji/predictor/in_gimbal_control_armor.hpp +++ b/src/tongji/predictor/in_gimbal_control_armor.hpp @@ -28,7 +28,7 @@ class InGimbalControlArmor final : public interfaces::IArmorInGimbalControl { const interfaces::ITimeStamped& GetTimeStamped() const override { return time_stamp_; } private: - TimeStamp time_stamp_; + time_stamp::TimeStamp time_stamp_; std::unordered_map> armors_map_; static const std::vector empty; diff --git a/src/tongji/predictor/live_target_manager/live_target.hpp b/src/tongji/predictor/live_target_manager/live_target.hpp index 710c87c..d9369a4 100644 --- a/src/tongji/predictor/live_target_manager/live_target.hpp +++ b/src/tongji/predictor/live_target_manager/live_target.hpp @@ -43,7 +43,7 @@ class LiveTarget { ExtendedKalmanFilter<11, 4>::XVec GetEkfX() const { return ekf_.x; } ExtendedKalmanFilter<11, 4>::PDig GetP0Dig() const { return model_.GetP0Dig(); } const PredictModel& GetModel() const { return model_; } - predictor::TimeStamp LastSeen() const { return predictor::TimeStamp(last_see_time_stamp_); } + time_stamp::TimeStamp LastSeen() const { return time_stamp::TimeStamp(last_see_time_stamp_); } std::vector GetArmorGimbalControlSpacings() const { std::vector armors; diff --git a/src/tongji/predictor/live_target_manager/tracker.hpp b/src/tongji/predictor/live_target_manager/tracker.hpp index a03c12d..c30b2eb 100644 --- a/src/tongji/predictor/live_target_manager/tracker.hpp +++ b/src/tongji/predictor/live_target_manager/tracker.hpp @@ -1,5 +1,6 @@ #pragma once +#include #include #include #include @@ -165,7 +166,7 @@ class Tracker final { const int normal_max_temp_lost_count_ = max_temp_lost_count_; const int max_switch_count_ = 200; - predictor::TimeStamp last_track_timestamp_; + time_stamp::TimeStamp last_track_timestamp_; }; } \ No newline at end of file diff --git a/src/tongji/predictor/target_snapshot_manager/aim_point_chooser.hpp b/src/tongji/predictor/target_snapshot_manager/aim_point_chooser.hpp index 5ba8b72..2f2c15d 100644 --- a/src/tongji/predictor/target_snapshot_manager/aim_point_chooser.hpp +++ b/src/tongji/predictor/target_snapshot_manager/aim_point_chooser.hpp @@ -3,7 +3,7 @@ #include "enum/car_id.hpp" #include "util/math.hpp" -namespace world_exe::tongji::fire_control { +namespace world_exe::tongji::predictor { using CarIDFlag = enumeration::CarIDFlag; diff --git a/src/tongji/predictor/target_snapshot_manager/aim_solver.hpp b/src/tongji/predictor/target_snapshot_manager/aim_solver.hpp index 732c8a2..4c9122e 100644 --- a/src/tongji/predictor/target_snapshot_manager/aim_solver.hpp +++ b/src/tongji/predictor/target_snapshot_manager/aim_solver.hpp @@ -9,7 +9,7 @@ #include "target_snapshot.hpp" #include "trajectory.hpp" -namespace world_exe::tongji::armor_solver { +namespace world_exe::tongji::predictor { using TargetSnapshot = predictor::TargetSnapshot; @@ -25,7 +25,7 @@ class AimingSolver { public: AimingSolver( const double& yaw_offset, const double& pitch_offset, const double& gravity = 9.7833) - : aim_point_chooser_(std::make_unique()) + : aim_point_chooser_(std::make_unique()) , yaw_offset_(yaw_offset / 57.3) // degree to rad , pitch_offset_(pitch_offset / 57.3) // degree to rad , g_(gravity) { } @@ -83,6 +83,6 @@ class AimingSolver { double yaw_offset_, pitch_offset_; const double g_; - std::unique_ptr aim_point_chooser_; + std::unique_ptr aim_point_chooser_; }; } \ No newline at end of file diff --git a/src/tongji/predictor/target_snapshot_manager/target_snapshot.hpp b/src/tongji/predictor/target_snapshot_manager/target_snapshot.hpp index f055594..a1fa6bf 100644 --- a/src/tongji/predictor/target_snapshot_manager/target_snapshot.hpp +++ b/src/tongji/predictor/target_snapshot_manager/target_snapshot.hpp @@ -48,7 +48,7 @@ class TargetSnapshot { private: PredictModel model_; ExtendedKalmanFilter<11, 4> ekf_; - TimeStamp time_stamp_; + time_stamp::TimeStamp time_stamp_; }; } \ No newline at end of file diff --git a/src/tongji/predictor/target_snapshot_manager/target_snapshot_manager.cpp b/src/tongji/predictor/target_snapshot_manager/target_snapshot_manager.cpp index a1d9614..ad9a586 100644 --- a/src/tongji/predictor/target_snapshot_manager/target_snapshot_manager.cpp +++ b/src/tongji/predictor/target_snapshot_manager/target_snapshot_manager.cpp @@ -6,7 +6,6 @@ #include "../in_gimbal_control_armor.hpp" #include "../live_target_manager/live_target.hpp" -#include "../target_snapshot_manager/target_snapshot.hpp" #include "aim_solver.hpp" #include "data/armor_gimbal_control_spacing.hpp" #include "enum/enum_tools.hpp" @@ -20,7 +19,7 @@ class TargetSnapshotManager::Impl { live_target_map, const std::time_t& now, const double& bullet_speed, const double& yaw_offset, const double& pitch_offset) - : aim_solver_(std::make_unique(yaw_offset, pitch_offset)) + : aim_solver_(std::make_unique(yaw_offset, pitch_offset)) , now_(now) , ids_(id) @@ -71,7 +70,7 @@ class TargetSnapshotManager::Impl { return result; } - std::unique_ptr aim_solver_; + std::unique_ptr aim_solver_; const std::unordered_map snapshots_; const std::time_t& now_; const enumeration::ArmorIdFlag ids_; diff --git a/src/tongji/predictor/target_snapshot_manager/trajectory.hpp b/src/tongji/predictor/target_snapshot_manager/trajectory.hpp index d4f9e68..ff5adc1 100644 --- a/src/tongji/predictor/target_snapshot_manager/trajectory.hpp +++ b/src/tongji/predictor/target_snapshot_manager/trajectory.hpp @@ -1,7 +1,7 @@ #pragma once #include -namespace world_exe::tongji::armor_solver { +namespace world_exe::tongji::predictor { struct TrajectoryResult { bool solvable = true; diff --git a/src/tongji/time_stamp/time_stamp.hpp b/src/tongji/time_stamp/time_stamp.hpp index 90deacf..aa93ae0 100644 --- a/src/tongji/time_stamp/time_stamp.hpp +++ b/src/tongji/time_stamp/time_stamp.hpp @@ -2,7 +2,7 @@ #include "interfaces/time_stamped.hpp" -namespace world_exe::tongji::predictor { +namespace world_exe::tongji::time_stamp { class TimeStamp : public interfaces::ITimeStamped { public: TimeStamp(const std::time_t& time_stamp)