diff --git a/config/config.yaml b/config/config.yaml index b64f63a..104fd62 100644 --- a/config/config.yaml +++ b/config/config.yaml @@ -22,7 +22,7 @@ capturer: invert_image: false software_sync: false trigger_mode: false - fixed_framerate: true + fixed_framerate: false local_video: # 替换为你具体的路径 location: "/path/to/your/video" @@ -51,6 +51,12 @@ identifier: score_threshold: 0.7 nms_threshold: 0.3 + green_light_filter: + green_threshold: 120 + min_area: 20 + min_circularity: 0.6 + max_aspect_ratio: 1.5 + tracker: # enemy_color: red enemy_color: blue @@ -73,7 +79,7 @@ pose_estimator: [-0.064232403853946, -0.087667493884102, 0, 0, 0.792381808294582] fire_control: - initial_bullet_speed: 21.4 # m/s + initial_bullet_speed: 23.2 # m/s shoot_delay: 0.07 # s mpc_enable: true yaw_offset: 0.0 # degree @@ -82,8 +88,8 @@ fire_control: aim_point_chooser: coming_angle: 70.0 # degree leaving_angle: 20.0 # degree - outpost_coming_angle: 70.0 # degree - outpost_leaving_angle: 30.0 # degree + outpost_coming_angle: 60.0 # degree + outpost_leaving_angle: 25.0 # degree mpc: mpc_max_yaw_acc: 50.0 diff --git a/src/kernel/identifier.cpp b/src/kernel/identifier.cpp index 0770c18..aadf899 100644 --- a/src/kernel/identifier.cpp +++ b/src/kernel/identifier.cpp @@ -1,33 +1,88 @@ #include "identifier.hpp" #include "module/identifier/armor_detection.hpp" +#include "module/identifier/green_light_locator.hpp" #include "utility/robot/armor.hpp" +#include +#include +#include + using namespace rmcs; using namespace rmcs::kernel; using namespace rmcs::identifier; struct Identifier::Impl { ArmorDetection armor_detection; + GreenLightLocator green_light_locator; auto initialize(const YAML::Node& yaml) noexcept -> std::expected { - { - auto result = armor_detection.initialize(yaml); - if (!result.has_value()) { - return std::unexpected { result.error() }; - } - } + auto armor_result = armor_detection.initialize(yaml); + if (!armor_result.has_value()) return std::unexpected { armor_result.error() }; + auto locator_result = green_light_locator.initialize(yaml["green_light_filter"]); + if (!locator_result.has_value()) return std::unexpected { locator_result.error() }; return {}; } - auto identify(const Image& src) noexcept { return armor_detection.sync_detect(src); } + auto identify(const Image& src) noexcept -> std::optional { + auto detected_armors = armor_detection.sync_detect(src); + if (!detected_armors.has_value()) return std::nullopt; + + auto outpost_armors = std::vector {}; + auto base_armors = std::vector {}; + outpost_armors.reserve(detected_armors->size()); + base_armors.reserve(detected_armors->size()); + for (const auto& armor : *detected_armors) { + if (armor.genre == DeviceId::OUTPOST) outpost_armors.push_back(armor); + if (armor.genre == DeviceId::BASE) base_armors.push_back(armor); + } + + const auto outpost_locator_result = green_light_locator.locate(src, outpost_armors); + const auto base_locator_result = green_light_locator.locate(src, base_armors); + + auto filtered = Armor2Ds {}; + filtered.reserve(detected_armors->size()); + + if (!outpost_locator_result.green_light.has_value() + && !base_locator_result.green_light.has_value()) { + filtered = *detected_armors; + } else { + const auto outpost_threshold_y = outpost_locator_result.green_light.has_value() + ? std::optional { + outpost_locator_result.green_light->y + + outpost_locator_result.green_light->height, + } + : std::nullopt; + const auto base_threshold_y = base_locator_result.green_light.has_value() + ? std::optional { + base_locator_result.green_light->y + base_locator_result.green_light->height, + } + : std::nullopt; + + for (const auto& armor : *detected_armors) { + const auto threshold_y = armor.genre == DeviceId::OUTPOST ? outpost_threshold_y + : armor.genre == DeviceId::BASE ? base_threshold_y + : std::nullopt; + // 过滤掉绿灯之上的对应装甲板(图像坐标系 y 向下为正) + if (threshold_y.has_value() && (armor.center.y < *threshold_y)) continue; + + filtered.push_back(armor); + } + } + + return Identifier::Result { + .armors = std::move(filtered), + .outpost_green_light = outpost_locator_result.green_light, + .base_green_light = base_locator_result.green_light, + }; + } }; auto Identifier::initialize(const YAML::Node& yaml) noexcept -> std::expected { return pimpl->initialize(yaml); } -auto Identifier::sync_identify(const Image& src) noexcept -> std::optional> { +auto Identifier::sync_identify(const Image& src) noexcept -> std::optional { return pimpl->identify(src); } diff --git a/src/kernel/identifier.hpp b/src/kernel/identifier.hpp index 3f0e583..e395df7 100644 --- a/src/kernel/identifier.hpp +++ b/src/kernel/identifier.hpp @@ -5,6 +5,9 @@ #include "utility/robot/armor.hpp" #include +#include + +#include #include namespace rmcs::kernel { @@ -13,9 +16,15 @@ class Identifier { RMCS_PIMPL_DEFINITION(Identifier) public: + struct Result { + Armor2Ds armors; + std::optional outpost_green_light; + std::optional base_green_light; + }; + auto initialize(const YAML::Node&) noexcept -> std::expected; - auto sync_identify(const Image&) noexcept -> std::optional>; + auto sync_identify(const Image&) noexcept -> std::optional; }; } diff --git a/src/module/identifier/green_light_detection.cpp b/src/module/identifier/green_light_detection.cpp new file mode 100644 index 0000000..d17b0e1 --- /dev/null +++ b/src/module/identifier/green_light_detection.cpp @@ -0,0 +1,149 @@ +#include "green_light_detection.hpp" + +#include "utility/image/image.details.hpp" +#include "utility/serializable.hpp" + +#include +#include +#include +#include + +#include + +using namespace rmcs::identifier; + +struct GreenLightDetection::Impl { + struct Config : util::Serializable { + int green_threshold; + + double min_area; + double min_circularity; + double max_aspect_ratio; + + constexpr static std::tuple metas { + &Config::green_threshold, + "green_threshold", + &Config::min_area, + "min_area", + &Config::min_circularity, + "min_circularity", + &Config::max_aspect_ratio, + "max_aspect_ratio", + }; + } config; + + auto initialize(const YAML::Node& yaml) noexcept -> std::expected { + auto result = config.serialize(yaml); + if (!result.has_value()) { + return std::unexpected { result.error() }; + } + + if ((config.green_threshold < 0) || (config.green_threshold > 255)) { + return std::unexpected { "green_threshold must satisfy 0 <= green_threshold <= 255" }; + } + + if (!(config.min_area > 0.0)) { + return std::unexpected { "min_area must be > 0" }; + } + + if ((config.min_circularity <= 0.0) || (config.min_circularity > 1.0)) { + return std::unexpected { "min_circularity must satisfy 0 < min_circularity <= 1" }; + } + + if (!(config.max_aspect_ratio >= 1.0)) { + return std::unexpected { "max_aspect_ratio must be >= 1" }; + } + + return {}; + } + + auto sync_detect(const Image& image, const cv::Rect2i& roi) const noexcept + -> std::optional { + auto green_light = std::optional {}; + + const auto& mat = image.details().mat; + if (mat.empty()) { + return std::nullopt; + } + + if ((roi.width <= 0) || (roi.height <= 0)) { + return std::nullopt; + } + + const auto image_rect = cv::Rect2i { 0, 0, mat.cols, mat.rows }; + const auto clipped_roi = roi & image_rect; + if ((clipped_roi.width != roi.width) || (clipped_roi.height != roi.height)) { + return std::nullopt; + } + + const auto roi_mat = mat(roi); + + auto green_channel = cv::Mat {}; + cv::extractChannel(roi_mat, green_channel, 1); + + auto mask = cv::Mat {}; + cv::threshold(green_channel, mask, static_cast(config.green_threshold), 255.0, + cv::THRESH_BINARY); + + auto kernel = cv::getStructuringElement(cv::MORPH_ELLIPSE, cv::Size { 5, 5 }); + cv::morphologyEx(mask, mask, cv::MORPH_CLOSE, kernel); + + auto contours = std::vector> {}; + cv::findContours(mask, contours, cv::RETR_EXTERNAL, cv::CHAIN_APPROX_SIMPLE); + + auto best_area = 0.0; + + for (const auto& contour : contours) { + const auto area = cv::contourArea(contour); + if (area < config.min_area) { + continue; + } + + const auto perimeter = cv::arcLength(contour, true); + if (!(perimeter > 0.0)) { + continue; + } + + const auto circularity = + 4.0 * std::numbers::pi_v * area / (perimeter * perimeter); + if (circularity < config.min_circularity) { + continue; + } + + const auto rect = cv::boundingRect(contour); + if ((rect.width <= 0) || (rect.height <= 0)) { + continue; + } + + const auto aspect_ratio = static_cast(std::max(rect.width, rect.height)) + / std::min(rect.width, rect.height); + if (aspect_ratio > config.max_aspect_ratio) { + continue; + } + + if (!green_light.has_value() || (area > best_area)) { + best_area = area; + green_light = rect; + green_light->x += roi.x; + green_light->y += roi.y; + } + } + + return green_light; + } +}; + +auto GreenLightDetection::initialize(const YAML::Node& yaml) noexcept + -> std::expected { + return pimpl->initialize(yaml); +} + +auto GreenLightDetection::sync_detect(const Image& image, const cv::Rect2i& roi) noexcept + -> std::optional { + return pimpl->sync_detect(image, roi); +} + +GreenLightDetection::GreenLightDetection() noexcept + : pimpl { std::make_unique() } { } + +GreenLightDetection::~GreenLightDetection() noexcept = default; diff --git a/src/module/identifier/green_light_detection.hpp b/src/module/identifier/green_light_detection.hpp new file mode 100644 index 0000000..a08d744 --- /dev/null +++ b/src/module/identifier/green_light_detection.hpp @@ -0,0 +1,22 @@ +#pragma once + +#include "utility/image/image.hpp" +#include "utility/pimpl.hpp" + +#include +#include + +#include +#include + +namespace rmcs::identifier { + +class GreenLightDetection { + RMCS_PIMPL_DEFINITION(GreenLightDetection) + +public: + auto initialize(const YAML::Node&) noexcept -> std::expected; + auto sync_detect(const Image&, const cv::Rect2i&) noexcept -> std::optional; +}; + +} diff --git a/src/module/identifier/green_light_locator.cpp b/src/module/identifier/green_light_locator.cpp new file mode 100644 index 0000000..4fda458 --- /dev/null +++ b/src/module/identifier/green_light_locator.cpp @@ -0,0 +1,87 @@ +#include "green_light_locator.hpp" + +#include "module/identifier/green_light_detection.hpp" +#include "utility/image/image.details.hpp" + +#include +#include +#include + +#include + +using namespace rmcs::identifier; + +struct GreenLightLocator::Impl { + GreenLightDetection green_light_detection; + + auto initialize(const YAML::Node& yaml) noexcept -> std::expected { + return green_light_detection.initialize(yaml); + } + + auto locate(const Image& image, std::span armors) noexcept + -> GreenLightLocator::Result { + auto result = GreenLightLocator::Result { + .green_light = std::nullopt, + }; + if (armors.empty()) return result; + + auto detect_roi = compute_detect_roi(image, armors); + if (!detect_roi.has_value()) return result; + + result.green_light = green_light_detection.sync_detect(image, *detect_roi); + return result; + } + +private: + static auto compute_detect_roi(const Image& image, std::span armors) + -> std::optional { + constexpr auto kExpandPixels = 300; + + if (armors.empty()) return std::nullopt; + + const auto& mat = image.details().mat; + if (mat.empty()) return std::nullopt; + + auto union_rect = cv::boundingRect(std::vector { + armors.front().tl, + armors.front().tr, + armors.front().br, + armors.front().bl, + }); + + for (const auto& armor : armors | std::views::drop(1)) { + const auto armor_rect = cv::boundingRect( + std::vector { armor.tl, armor.tr, armor.br, armor.bl }); + union_rect |= armor_rect; + } + + auto roi = cv::Rect2i { + union_rect.x - kExpandPixels, + union_rect.y - kExpandPixels, + union_rect.width + kExpandPixels * 2, + union_rect.height + kExpandPixels * 2, + }; + + roi &= cv::Rect2i { 0, 0, mat.cols, mat.rows }; + if ((roi.width <= 0) || (roi.height <= 0)) { + return std::nullopt; + } + + return roi; + } +}; + +auto GreenLightLocator::initialize(const YAML::Node& yaml) noexcept + -> std::expected { + return pimpl->initialize(yaml); +} + +auto GreenLightLocator::locate(const Image& image, std::span armors) noexcept + -> Result { + return pimpl->locate(image, armors); +} + +GreenLightLocator::GreenLightLocator() noexcept + : pimpl { std::make_unique() } { } + +GreenLightLocator::~GreenLightLocator() noexcept = default; diff --git a/src/module/identifier/green_light_locator.hpp b/src/module/identifier/green_light_locator.hpp new file mode 100644 index 0000000..d7c66c4 --- /dev/null +++ b/src/module/identifier/green_light_locator.hpp @@ -0,0 +1,29 @@ +#pragma once + +#include "utility/image/image.hpp" +#include "utility/pimpl.hpp" +#include "utility/robot/armor.hpp" + +#include +#include +#include + +#include + +#include + +namespace rmcs::identifier { + +class GreenLightLocator { + RMCS_PIMPL_DEFINITION(GreenLightLocator) + +public: + struct Result { + std::optional green_light; + }; + + auto initialize(const YAML::Node&) noexcept -> std::expected; + auto locate(const Image&, std::span) noexcept -> Result; +}; + +} diff --git a/src/module/tracker/armor_filter.cpp b/src/module/tracker/armor_filter.cpp index a0f83fe..bf26bc5 100644 --- a/src/module/tracker/armor_filter.cpp +++ b/src/module/tracker/armor_filter.cpp @@ -10,7 +10,7 @@ struct ArmorFilter::Impl { auto filter(std::span const& armors) const -> std::vector { auto filtered = armors | std::views::filter([&](Armor2D const& armor) { - return (armor.genre != DeviceId::INFANTRY_5) + return (armor.genre != DeviceId::INFANTRY_5) && (armor.genre != DeviceId::UNKNOWN) && (armor_color2camp_color(armor.color) == enemy_color) && (!invincible_armors.contains(armor.genre)); }); diff --git a/src/runtime.cpp b/src/runtime.cpp index 9ede29f..5ec40d8 100644 --- a/src/runtime.cpp +++ b/src/runtime.cpp @@ -8,6 +8,8 @@ #include "module/debug/framerate.hpp" #include "utility/image/armor.hpp" +#include "utility/image/green_light.hpp" +#include "utility/image/text.hpp" #include "utility/logging_util.hpp" #include "utility/math/linear.hpp" #include "utility/panic.hpp" @@ -143,8 +145,13 @@ auto main() -> int { continue; // 一般不会推理出错喵~ } if (use_painted_image) { - for (const auto& armor : *result) + for (const auto& armor : result->armors) util::draw(*image, armor); + + if (result->outpost_green_light.has_value()) + util::draw_green_light(*image, *result->outpost_green_light); + if (result->base_green_light.has_value()) + util::draw_green_light(*image, *result->base_green_light); } logging.reset("detection", 5); @@ -157,7 +164,7 @@ auto main() -> int { tracker.set_enemy_color(CampColor::RED); tracker.set_invincible_armors(context.invincible_devices); - armors_2d = tracker.filter_armors(*result); + armors_2d = tracker.filter_armors(result->armors); if (armors_2d.empty()) continue; } @@ -205,6 +212,9 @@ auto main() -> int { command.pitch_rate, command.yaw_acc, command.pitch_acc); } } + if (use_visualization) { + util::draw_text(*image, command.should_shoot ? "ATTACK" : "IDLE"); + } /// 4. Transmit State /// diff --git a/src/utility/image/green_light.cpp b/src/utility/image/green_light.cpp new file mode 100644 index 0000000..ee4654a --- /dev/null +++ b/src/utility/image/green_light.cpp @@ -0,0 +1,20 @@ +#include "green_light.hpp" + +#include "utility/image/image.details.hpp" + +#include + +#include + +namespace rmcs::util { + +auto draw_green_light(Image& canvas, const cv::Rect2i& rect) noexcept -> void { + auto& opencv_mat = canvas.details().mat; + + cv::rectangle(opencv_mat, rect, cv::Scalar { 0, 255, 0 }, 2, cv::LINE_AA); + cv::putText(opencv_mat, "green_light", + cv::Point2i { rect.x, std::max(rect.y - 6, 0) }, cv::FONT_HERSHEY_SIMPLEX, 0.6, + cv::Scalar { 0, 255, 0 }, 1, cv::LINE_AA); +} + +} diff --git a/src/utility/image/green_light.hpp b/src/utility/image/green_light.hpp new file mode 100644 index 0000000..4f32daf --- /dev/null +++ b/src/utility/image/green_light.hpp @@ -0,0 +1,11 @@ +#pragma once + +#include "utility/image/image.hpp" + +#include + +namespace rmcs::util { + +auto draw_green_light(Image&, const cv::Rect2i&) noexcept -> void; + +} diff --git a/src/utility/image/text.cpp b/src/utility/image/text.cpp new file mode 100644 index 0000000..a5aab2c --- /dev/null +++ b/src/utility/image/text.cpp @@ -0,0 +1,32 @@ +#include "text.hpp" + +#include "utility/image/image.details.hpp" + +#include + +#include + +namespace rmcs::util { + +auto draw_text(Image& canvas, const std::string& text) noexcept -> void { + auto& opencv_mat = canvas.details().mat; + + const auto thickness = 1; + const auto font = cv::FONT_HERSHEY_SIMPLEX; + const auto scale = 0.6; + const auto margin = 10; + const auto white = cv::Scalar { 255, 255, 255 }; + const auto black = cv::Scalar { 0, 0, 0 }; + + auto baseline = 0; + const auto size = cv::getTextSize(text, font, scale, thickness, &baseline); + const auto x = std::max(canvas.details().get_cols() - size.width - margin, 0); + const auto y = std::max(canvas.details().get_rows() - baseline - margin, size.height); + const auto org = cv::Point2i { x, y }; + + cv::putText(opencv_mat, text, org + cv::Point2i { 1, 1 }, font, scale, black, thickness + 2, + cv::LINE_AA); + cv::putText(opencv_mat, text, org, font, scale, white, thickness, cv::LINE_AA); +} + +} diff --git a/src/utility/image/text.hpp b/src/utility/image/text.hpp new file mode 100644 index 0000000..0b1f697 --- /dev/null +++ b/src/utility/image/text.hpp @@ -0,0 +1,11 @@ +#pragma once + +#include "utility/image/image.hpp" + +#include + +namespace rmcs::util { + +auto draw_text(Image&, const std::string& text) noexcept -> void; + +}