Feat/outpost filter

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

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 <optional>
+#include <span>
+#include <vector>
+
 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<void, std::string> {
-        {
-            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<Identifier::Result> {
+        auto detected_armors = armor_detection.sync_detect(src);
+        if (!detected_armors.has_value()) return std::nullopt;
+
+        auto outpost_armors = std::vector<Armor2D> {};
+        auto base_armors    = std::vector<Armor2D> {};
+        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<void, std::string> {
     return pimpl->initialize(yaml);
 }
 
-auto Identifier::sync_identify(const Image& src) noexcept -> std::optional<std::vector<Armor2D>> {
+auto Identifier::sync_identify(const Image& src) noexcept -> std::optional<Result> {
     return pimpl->identify(src);
 }
 

src/kernel/identifier.hpp

@@ -5,6 +5,9 @@
 #include "utility/robot/armor.hpp"
 
 #include <expected>
+#include <optional>
+
+#include <opencv2/core/types.hpp>
 #include <yaml-cpp/node/node.h>
 
 namespace rmcs::kernel {
@@ -13,9 +16,15 @@ class Identifier {
     RMCS_PIMPL_DEFINITION(Identifier)
 
 public:
+    struct Result {
+        Armor2Ds armors;
+        std::optional<cv::Rect2i> outpost_green_light;
+        std::optional<cv::Rect2i> base_green_light;
+    };
+
     auto initialize(const YAML::Node&) noexcept -> std::expected<void, std::string>;
 
-    auto sync_identify(const Image&) noexcept -> std::optional<std::vector<Armor2D>>;
+    auto sync_identify(const Image&) noexcept -> std::optional<Result>;
 };
 
 }

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 <algorithm>
+#include <cmath>
+#include <numbers>
+#include <optional>
+
+#include <opencv2/imgproc.hpp>
+
+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<void, std::string> {
+        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<cv::Rect2i> {
+        auto green_light = std::optional<cv::Rect2i> {};
+
+        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<double>(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<std::vector<cv::Point>> {};
+        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<double> * 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<double>(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<void, std::string> {
+    return pimpl->initialize(yaml);
+}
+
+auto GreenLightDetection::sync_detect(const Image& image, const cv::Rect2i& roi) noexcept
+    -> std::optional<cv::Rect2i> {
+    return pimpl->sync_detect(image, roi);
+}
+
+GreenLightDetection::GreenLightDetection() noexcept
+    : pimpl { std::make_unique<Impl>() } { }
+
+GreenLightDetection::~GreenLightDetection() noexcept = default;

src/module/identifier/green_light_detection.hpp

@@ -0,0 +1,22 @@
+#pragma once
+
+#include "utility/image/image.hpp"
+#include "utility/pimpl.hpp"
+
+#include <expected>
+#include <optional>
+
+#include <opencv2/core/types.hpp>
+#include <yaml-cpp/yaml.h>
+
+namespace rmcs::identifier {
+
+class GreenLightDetection {
+    RMCS_PIMPL_DEFINITION(GreenLightDetection)
+
+public:
+    auto initialize(const YAML::Node&) noexcept -> std::expected<void, std::string>;
+    auto sync_detect(const Image&, const cv::Rect2i&) noexcept -> std::optional<cv::Rect2i>;
+};
+
+}