From f3b06d6abe3c3f13c470f36fa3e838c6e1164260 Mon Sep 17 00:00:00 2001 From: mzxwww Date: Thu, 17 Sep 2026 00:34:24 +0800 Subject: [PATCH] Add light bar detector and detect_test - traditional CV pipeline referencing jlu_vision_26: HSV mask -> contour LightBar -> PCA corner refine -> pairing gates - big/small armor classification (bar distance / bar length ratio) - detect_test with synthetic selftest, 6/6 passed - measured armor size 131.56x55mm, solver NaN guard + ITERATIVE fallback --- README.md | 53 +++++- configs/camera.yaml | 18 +- configs/light_bar.yaml | 51 ++++++ inc/camera_calib.hpp | 16 +- inc/light_bar.hpp | 133 +++++++++++++- src/camera_calib.cpp | 90 +++++----- src/detect_test.cpp | 367 ++++++++++++++++++++++++++++++++++++++ src/light_bar.cpp | 395 +++++++++++++++++++++++++++++++++++++++++ src/pose_test.cpp | 6 +- xmake.lua | 11 ++ 10 files changed, 1063 insertions(+), 77 deletions(-) create mode 100644 configs/light_bar.yaml create mode 100644 src/detect_test.cpp diff --git a/README.md b/README.md index 03bbf36..80d4202 100644 --- a/README.md +++ b/README.md @@ -27,13 +27,15 @@ xmake project -k compile_commands .vscode | `capture_calib` | **相机标定图采集工具** | | `calibrate` | **标定计算工具**（照片 → 内参/畸变 → `configs/camera.yaml`） | | `pose_test` | **几何链路验证工具**（4 角点 → 距离/yaw/pitch，含合成自检） | +| `detect_test` | **灯条检测验证工具**（检测 → PnP 一条龙，含合成自检） | ## 配置 ``` configs/ ├── camera.yaml # 相机内参 + 畸变 + 标定元信息（由 calibrate 生成，需入库） -└── camera_runtime.yaml # 运行时参数：曝光/增益/白平衡/帧率（手工维护，需入库） +├── camera_runtime.yaml # 运行时参数：曝光/增益/白平衡/帧率（手工维护，需入库） +└── light_bar.yaml # 灯条检测参数：HSV 范围/配对门限/PCA（手工维护，需入库） ``` 代码里这样用（`inc/camera_calib.hpp`）： @@ -76,7 +78,7 @@ cam.setFrameSpeed(2); // 帧率档位 0/1/2 ## pose_test：几何链路验证 写检测器之前，先用它验证「标定 + 坐标约定 + PnP 解算」： -拿真的装甲板（或打印 135mm 矩形）摆在相机前，鼠标点 4 个角， +拿真的装甲板（或打印 131.56×55mm 矩形）摆在相机前，鼠标点 4 个角， 输出的距离/yaw/pitch 与卷尺、量角器核对。之后接检测器出问题必是检测的锅。 ```bash @@ -96,12 +98,55 @@ cam.setFrameSpeed(2); // 帧率档位 0/1/2 ### 两个踩过的坑（写检测器时别重蹈） -1. **IPPE + 畸变 + 板内旋转 = 垃圾解**（OpenCV 4.x/5.x 都中招）。 - 解法：角点先去畸变再喂 IPPE，重投影回原图算（`solvePose` 已内置）。 +1. **OpenCV 4.x 的 IPPE 有两个数值雷区**（实测）： + ①「畸变 + 板内旋转」→ 解出垃圾解；②「完美轴对称矩形」→ 返回 NaN。 + 解法：IPPE 全候选逐个筛（有限值/z>0/重投影），**全被筛掉就自动退回 + `SOLVEPNP_ITERATIVE`**——两条退化路径都稳（`solvePose` 已内置）。 2. **4 个共面点的顺序错误查不出来**：循环/镜像顺序 = 同一个四边形 = 同一个解（无害）；交叉乱序会被重投影门限拦下。真正需要 roll 门限的是 「检测器给出假四边形」场景，等写检测器时照搬 jlu 的 `roll_thres 15°`。 +### 装甲板尺寸（2026-09-16 实测） + +| 板型 | W 灯条中心距 | L 灯条长度 | 来源 | +|---|---|---|---| +| 小装甲（2/3/4/5 号、哨兵） | **131.56 mm** | **55.0 mm** | 实测 3 号板，与官方规格 131.5mm 一致 | +| 大装甲（英雄 1 号） | 230 mm（名义值） | 55.0 mm | 待实测校正 W | + +⚠️ 尺寸误差 1% = 距离误差 1%；拿 135mm 模型解 230mm 大板，距离会差 0.59 倍。 +检测器将来必须同时输出「4 角点 + 大小装甲分类」。 + +## detect_test：灯条检测 + PnP 一条龙 + +传统 CV 管线（借鉴 jlu_vision_26）：HSV 颜色掩膜 → 轮廓 → LightBar +（minAreaRect+质心+端点）→ PCA 角点修正（灰度矩对称轴 + 亮度梯度找端点） +→ 左右配对五道门 → 大小装甲分类 → 4 角点 → `solvePose`。 + +```bash +./build/linux/x86_64/debug/detect_test # 相机实时 +./build/linux/x86_64/debug/detect_test --image 照片.jpg # 静态图 +./build/linux/x86_64/debug/detect_test --selftest # 合成自检（无需相机） +``` + +- 按键：`q` 退出 | `s` 截图 | `c` 切换敌方颜色（红方打蓝，默认蓝） | `r` 重载参数 +- 参数都在 `configs/light_bar.yaml`，现场按 `r` 热重载调参 +- 敌方颜色默认 `blue`（主人是红方）；拿自己红板测试时改 `red` 或现场按 `c` +- `--selftest` 自检已通过 6/6：正前方/右偏/上仰/复合/板内滚转/大装甲， + 距离误差 ≤3.1%、yaw/pitch 误差 ≤0.05°，大小分类全部正确 + +### 调参时踩过的三个坑（已在默认参数里避开） + +1. **形态学闭运算别开 2 次**：远景灯条只有 2~3px 宽，腐蚀两轮整个抹掉 +2. **长宽比门限别设太高**（默认 1.5）：远景灯条模糊+阈值后掩膜被撑到 + 10px 宽，实测比只剩 1.7；防误检主要靠配对五道门 +3. **角点顺序的镜像坑**：板子朝向相机时，板的左条出现在画面右侧 + （面对面镜像），输出角点已按模型约定「板左下→板左上→板右上→板右下」处理 + +### 渲染自检的教训（写自检时别重蹈） + +圆管式画灯条会让端弧超出模型端点 4px+，距离直接偏 17%；用填充四边形、 +端点严格对齐模型点。 + ## 相机标定流程 > **本工程标定板规格**：10 × 7 内角点、**方格 20.0 mm**（卡尺实测值，不是打印标称值） diff --git a/configs/camera.yaml b/configs/camera.yaml index b0d7d5b..04cd6de 100644 --- a/configs/camera.yaml +++ b/configs/camera.yaml @@ -6,15 +6,15 @@ # ⚠️ M12 镜头的【对焦环位置】会改变有效焦距： # 一旦调焦，必须重新标定并重新做卷尺距离验证！ # ============================================================ -camera_name: "MV-SUA131GC" -lens: "MV-LD-M12-2.8MM-C" -sensor: "1/3\" CMOS, 3.75um pixel" -resolution: "1280x1024" -calib_date: "2026-09-15" -board_spec: "10x7 inner corners, square=20.0mm" -calib_images: "31 (old_run 12 + new 19)" -distance_verify: "ruler 700mm -> computed 687~697mm, error <2%" -notes: "refocus invalidates this calibration; must re-verify with ruler" +camera_name: "MV-SUA131GC" # 相机型号 +lens: "MV-LD-M12-2.8MM-C" # 镜头型号，焦距 2.8mm +sensor: "1/3\" CMOS, 3.75um pixel" # 传感器类型 +resolution: "1280x1024" # 分辨率 +calib_date: "2026-09-15" # 标定日期 +board_spec: "10x7 inner corners, square=20.0mm" # 标定板规格 +calib_images: "31 (old_run 12 + new 19)" # 标定图像数量 +distance_verify: "ruler 700mm -> computed 687~697mm, error <2%" # 用卷尺验证 PnP 测距的精度 +notes: "refocus invalidates this calibration; must re-verify with ruler" # 备注 camera_matrix: !!opencv-matrix rows: 3 diff --git a/configs/light_bar.yaml b/configs/light_bar.yaml new file mode 100644 index 0000000..04940fa --- /dev/null +++ b/configs/light_bar.yaml @@ -0,0 +1,51 @@ +%YAML:1.0 +--- +# ============================================================ +# 灯条检测参数（27RM） +# 敌方颜色：主人是红方 → 打蓝。用自己红板测试时可改成 red。 +# 调参现场：./build/linux/x86_64/debug/detect_test 里按 c 切换颜色、r 重载本文件 +# ============================================================ +enemy_color: "blue" + +# ---------- 蓝色 HSV 范围（OpenCV H∈[0,180], S/V∈[0,255]） ---------- +blue_h_min: 100 +blue_h_max: 140 +blue_s_min: 60 +blue_v_min: 60 + +# ---------- 红色 HSV 范围（H 跨 0 和 180 两端） ---------- +red_h_min_1: 0 +red_h_max_1: 12 +red_h_min_2: 168 +red_h_max_2: 180 +red_s_min: 60 +red_v_min: 60 + +# ---------- 形态学（闭运算补洞） ---------- +# 注意：iters 别开大——远景灯条只有 2~3px 宽，腐蚀两轮会被整个抹掉 +morph_kernel: 3 +morph_iters: 1 + +# ---------- 灯条筛选 ---------- +min_length: 8.0 # 最小灯条长度（px）；8px ≈ 4.5m 处的 55mm 灯条 +min_ratio: 1.5 # 长/宽最小比。远景灯条只有2px宽，模糊+阈值会把 + # 掩膜撑宽到10px、比降到1.7，别设太高；真正的 + # 防误检靠配对五道门 +max_tilt_deg: 45.0 # 与竖直方向最大夹角 +max_area: 200000.0 + +# ---------- 配对 ---------- +max_angle_diff_deg: 15.0 # 两灯条倾角差上限 +max_len_ratio: 2.2 # 两灯条长度比上限 +max_center_y_ratio: 0.7 # 中心纵向差 / 平均灯条长 上限 +big_small_ratio_thres: 3.0 # 灯条中心距/平均灯条长 > 此值 → 大装甲 + # 小: 131.56/55 ≈ 2.4 大: 230/55 ≈ 4.2 + +# ---------- PCA 角点修正（jlu lightbar_corrector 同款参数） ---------- +use_pca: 1 # 是否启用 PCA 角点修正。1 = 启用 +pass_optimize_lightbar_width: 3 # 灯条宽度方向的搜索半径，单位像素。 +normalize_max_brightness: 25.0 # 亮度归一化的最大值 +lightbar_min_mean_brightness: 0.0 # 灯条平均亮度下限 +padding_scale: 0.07 # 灯条宽度方向的搜索半径，单位像素。 +search_start_ratio: 0.4 # 灯条宽度方向的搜索起始位置，单位像素。 +search_end_ratio: 0.6 # 灯条宽度方向的搜索结束位置，单位像素。 diff --git a/inc/camera_calib.hpp b/inc/camera_calib.hpp index 15aa80b..a710d4c 100644 --- a/inc/camera_calib.hpp +++ b/inc/camera_calib.hpp @@ -37,10 +37,12 @@ struct ArmorSize { double width_mm; double height_mm; - /** 大装甲板：英雄（一号）230mm 间距 */ - static ArmorSize big() { return ArmorSize{230.0, 56.0}; } - /** 小装甲板：2/3/4/5 号、哨兵 135mm 间距 */ - static ArmorSize small() { return ArmorSize{135.0, 56.0}; } + /** 大装甲板：英雄（一号）。W=230mm 为官方灯条中心距名义值， + * 等实测大板后再校正；L=55mm 与小装甲灯条同长 */ + static ArmorSize big() { return ArmorSize{230.0, 55.0}; } + /** 小装甲板：2/3/4/5 号、哨兵。2026-09-16 实测 3 号板： + * W=131.56mm（官方规格灯条中心距 131.5mm，实测一致），L=55.0mm */ + static ArmorSize small() { return ArmorSize{131.56, 55.0}; } }; /** 位姿解算结果（相机坐标系，单位 mm） */ @@ -72,9 +74,9 @@ public: /** * 四个角点 → 装甲板位姿 - * 内部流程：solvePnPGeneric(IPPE) 取回全部候选解 → - * 依次筛掉「z<=0（相机后方）」「重投影超差」→ 取重投影最小者。 - * 注：角点在解算前先做去畸变（IPPE+畸变+板内旋转组合有数值坑）。 + * 内部流程：solvePnPGeneric(IPPE) 取回全部候选解 → 筛掉 + * 「非有限值」「z<=0（相机后方）」「重投影超差」→ 取重投影最小者； + * 若 IPPE 数值退化（全被筛掉），自动退回 SOLVEPNP_ITERATIVE。 * @param corners 图像坐标（原图，含畸变），顺序：左下、左上、右上、右下 * @param size 装甲板物理尺寸 * @return 成功返回位姿；全部候选被筛掉时返回 std::nullopt diff --git a/inc/light_bar.hpp b/inc/light_bar.hpp index 5ce3321..629d6b6 100644 --- a/inc/light_bar.hpp +++ b/inc/light_bar.hpp @@ -1,16 +1,135 @@ #ifndef LIGHT_BAR_HPP #define LIGHT_BAR_HPP -class LightBar -{ -public: - LightBar(); // 构造函数声明 - ~LightBar(); // 析构函数声明 +/** + * @file light_bar.hpp + * @brief 传统 CV 灯条检测器（27RM） + * + * 管线（借鉴 jlu_vision_26 的方法）： + * 1. HSV 颜色掩膜（敌方颜色：默认蓝）→ 闭运算 → 轮廓 + * 2. 轮廓 → LightBar（minAreaRect + 质心 + 上下端点 + 倾角） + * 3. PCA 角点修正（jlu lightbar_corrector 同款： + * 灰度矩求对称轴，沿轴找最大亮度梯度 → 精确端点） + * 4. 左右配对（倾角差/长度比/中心高度差）→ 大/小装甲分类 + * 5. 输出 4 角点：左下 → 左上 → 右上 → 右下 + * （与 CameraCalib::solvePose 的模型点顺序一致） + */ +#include + +#include +#include +#include +#include + +namespace rm27 { + +/** 检测参数（configs/light_bar.yaml 可覆盖） */ +struct LightBarConfig { + // ---- 颜色（OpenCV HSV：H∈[0,180]，S/V∈[0,255]）---- + std::string enemy_color = "blue"; + int blue_h_min = 100, blue_h_max = 140, blue_s_min = 60, blue_v_min = 60; + int red_h_min_1 = 0, red_h_max_1 = 12; // 红色 H 跨两端 + int red_h_min_2 = 168, red_h_max_2 = 180; + int red_s_min = 60, red_v_min = 60; + + // ---- 形态学 ---- + int morph_kernel = 3; + int morph_iters = 2; + + // ---- 灯条筛选 ---- + double min_length = 15.0; // 最小灯条长度（px） + double min_ratio = 2.0; // 长/宽最小比（灯条是细长条） + double max_tilt_deg = 45.0; // 与竖直方向最大夹角 + double max_area = 200000.0; + + // ---- 配对 ---- + double max_angle_diff_deg = 15.0; // 两灯条倾角差 + double max_len_ratio = 2.2; // 两灯条长度比上限 + double max_center_y_ratio = 0.7; // 中心纵向差 / 平均长度 + double big_small_ratio_thres = 3.0; // 中心距/平均长度 > 此值 → 大装甲 + // (小 131.56/55≈2.4，大 230/55≈4.2) + + // ---- PCA 角点修正（jlu lightbar_corrector 同款参数）---- + bool use_pca = true; + int pass_optimize_lightbar_width = 3; + float normalize_max_brightness = 25.0f; + float lightbar_min_mean_brightness = 0.0f; + float padding_scale = 0.07f; + float search_start_ratio = 0.4f; + float search_end_ratio = 0.6f; +}; + +/** 一根灯条 */ +struct LightBar { + cv::RotatedRect rect; // 轮廓最小外接矩形 + cv::Point2f center; // 质心（比矩形对角线交点稳） + cv::Point2f top, bottom; // 端点（图像坐标，y 向下） + float length = 0.f; // 端点距离 + float width = 0.f; // 矩形短边 + cv::Point2f axis; // bottom→top 单位方向 + float tilt_deg = 0.f; // 与竖直方向夹角（无符号） + double signed_tilt = 0.0; // 有符号倾角（正 = 向右倾） + bool pca_ok = false; // PCA 修正成功（端点已精修） +}; + +/** 一块完整装甲板 */ +struct ArmorTarget { + LightBar left_light, right_light; + std::array corners; // 左下 左上 右上 右下 + cv::Point2f center; + bool is_big = false; +}; + +class LightBarDetector { +public: + LightBarDetector() = default; + explicit LightBarDetector(const std::string &yaml_path) { + loadConfig(yaml_path); + } + + /** 加载参数；文件不存在时保留默认值并打印警告 */ + bool loadConfig(const std::string &yaml_path); + + /** + * 检测一帧，返回最优目标（多个候选时取灯条平均长度最大者）。 + * @param bgr 原始帧（BGR） + * @return 没找到时 std::nullopt + */ + std::optional detect(const cv::Mat &bgr); + + const LightBarConfig &config() const { return cfg_; } + const std::vector &bars() const { return bars_; } + const cv::Mat &mask() const { return mask_debug_; } + + /** 调试绘制：灯条（黄）、配对矩形与角点（绿） */ + static void drawDebug(cv::Mat &img, const std::vector &bars, + const std::optional &armor); + + /** 切换敌方颜色（现场调试用） */ + void setEnemyColor(const std::string &color) { cfg_.enemy_color = color; } private: + std::vector findBars(const cv::Mat &bgr); + std::optional pairBars(const cv::Mat &gray, + std::vector &bars); + /** 从轮廓构造灯条（借鉴 jlu types.cpp LightBar(contour)） */ + static LightBar fromContour(const std::vector &contour); -}; + /** PCA 角点修正（借鉴 jlu lightbar_corrector）；成功返回 true */ + bool correctBar(const cv::Mat &gray, LightBar &bar); + std::optional findCorner(const cv::Mat &gray, + const LightBar &bar, + const cv::Point2f &axis_dir, + const cv::Point2f ¢roid, + float mean_val, bool toward_top); -#endif \ No newline at end of file + LightBarConfig cfg_; + std::vector bars_; + cv::Mat mask_debug_; +}; + +} // namespace rm27 + +#endif // LIGHT_BAR_HPP diff --git a/src/camera_calib.cpp b/src/camera_calib.cpp index 50e5ac9..ee514e5 100644 --- a/src/camera_calib.cpp +++ b/src/camera_calib.cpp @@ -1,5 +1,6 @@ #include "camera_calib.hpp" +#include #include #include @@ -65,66 +66,61 @@ std::optional CameraCalib::solvePose( const std::vector img_points{corners[0], corners[1], corners[2], corners[3]}; - // ⚠️ 先把角点去畸变到理想像素坐标，再用 IPPE（D 传空）。 - // 实测：IPPE + 畸变系数 + 板内旋转 三者组合会解出垃圾解 - // （OpenCV 4.x/5.x 都有这坑），去畸变后数值稳定。 - std::vector ideal; - cv::undistortPoints(img_points, ideal, camera_matrix_, dist_coeffs_, - cv::noArray(), camera_matrix_); - - // 平面靶标有镜像歧义：IPPE 会给出两个候选解， - // solvePnPGeneric 一次性全部取回，逐个筛。 - std::vector rvecs, tvecs; - const int n = cv::solvePnPGeneric(obj_points, ideal, camera_matrix_, - cv::Mat(), rvecs, tvecs, false, - cv::SOLVEPNP_IPPE); - if (n <= 0) { - return std::nullopt; - } - - int best = -1; - double best_err = 1e18; - std::vector proj; - for (int i = 0; i < n; ++i) { - const cv::Mat &rv = rvecs[i]; - const cv::Mat &tv = tvecs[i]; - - // ① 目标必须在相机前方（z > 0）。镜像解常落在相机后方。 + // 单个候选解的验收：有限值 → z>0（相机前方）→ 原图重投影门限 + auto tryAccept = [&](const cv::Mat &rv, + const cv::Mat &tv) -> std::optional { const double z = tv.at(2); - if (z <= 0.0) { - continue; + if (!std::isfinite(z) || z <= 0.0) { + return std::nullopt; // 镜像解常落在相机后方；NaN 直接丢 } - - // ② 重投影误差在【原图（含畸变）】上计算，与角点像素一致。 - // 4 个共面点的循环/镜像顺序给出同一个四边形→同一个解（无害）； - // 真正有害的交叉乱序（蝴蝶结）会在这里误差爆炸被拦下。 + std::vector proj; cv::projectPoints(obj_points, rv, tv, camera_matrix_, dist_coeffs_, proj); double err = 0.0; for (std::size_t k = 0; k < proj.size(); ++k) { - err += cv::norm(proj[k] - img_points[k]); + const double dx = proj[k].x - img_points[k].x; + const double dy = proj[k].y - img_points[k].y; + if (!std::isfinite(dx) || !std::isfinite(dy)) { + return std::nullopt; + } + err += std::sqrt(dx * dx + dy * dy); } err /= static_cast(proj.size()); if (err > max_reproj_err_px_) { - continue; + return std::nullopt; } + ArmorPose pose; + pose.rvec = + cv::Vec3d(rv.at(0), rv.at(1), rv.at(2)); + pose.tvec = + cv::Vec3d(tv.at(0), tv.at(1), tv.at(2)); + pose.reproj_error_px = err; + pose.distance_mm = cv::norm(pose.tvec); + return pose; + }; - if (err < best_err) { - best_err = err; - best = i; + // 路径 A：IPPE 全候选（平面靶标标准做法，一次拿到两个解便于筛镜像） + std::vector rvecs, tvecs; + const int n = cv::solvePnPGeneric(obj_points, img_points, camera_matrix_, + dist_coeffs_, rvecs, tvecs, false, + cv::SOLVEPNP_IPPE); + std::optional best; + for (int i = 0; i < n; ++i) { + auto p = tryAccept(rvecs[i], tvecs[i]); + if (p && (!best || p->reproj_error_px < best->reproj_error_px)) { + best = p; } } - - if (best < 0) { - return std::nullopt; + if (best) { + return best; } - ArmorPose pose; - const cv::Mat &rv = rvecs[best]; - const cv::Mat &tv = tvecs[best]; - pose.rvec = cv::Vec3d(rv.at(0), rv.at(1), rv.at(2)); - pose.tvec = cv::Vec3d(tv.at(0), tv.at(1), tv.at(2)); - pose.reproj_error_px = best_err; - pose.distance_mm = cv::norm(pose.tvec); - return pose; + // 路径 B：IPPE 数值退化（实测 OpenCV 4.x 在「完美轴对称矩形」上返回 NaN、 + // 「畸变+板内旋转」上返回垃圾解）时退回 ITERATIVE，两条退化情形都稳。 + cv::Mat rv, tv; + if (cv::solvePnP(obj_points, img_points, camera_matrix_, dist_coeffs_, rv, + tv, false, cv::SOLVEPNP_ITERATIVE)) { + return tryAccept(rv, tv); + } + return std::nullopt; } diff --git a/src/detect_test.cpp b/src/detect_test.cpp new file mode 100644 index 0000000..eda15bd --- /dev/null +++ b/src/detect_test.cpp @@ -0,0 +1,367 @@ +/** + * @file detect_test.cpp + * @brief 灯条检测器验证工具（27RM） + * + * 检测 → 4 角点 → PnP → 距离/yaw/pitch，一条龙验证。 + * + * 模式： + * ./detect_test 相机实时（按 c 切敌方颜色、r 重载参数） + * ./detect_test --image 照片.jpg 静态图交互 + * ./detect_test --image 照片.jpg --once 跑一次就退出（脚本/沙箱用） + * ./detect_test --selftest 合成图自检：用标定内参把已知位姿的 + * 装甲板渲染成图 → 检测 → PnP 对比 + */ + +#include "camera_calib.hpp" +#include "camera_set.hpp" +#include "light_bar.hpp" + +#include + +#include +#include +#include + +namespace { + +// --------------------------------------------------------------- +// 合成渲染：按标定内参把已知位姿的装甲板画成发光灯条图 +// --------------------------------------------------------------- +void drawBar(cv::Mat &img, const cv::Point2f &a, const cv::Point2f &b, + double half_width, const cv::Scalar &color) { + // 填充四边形，端点严格落在 a/b（与模型点一致，圆管端弧会超出去） + cv::Point2f d = b - a; + const float n = cv::norm(d); + if (n <= 0.f) return; + const cv::Point2f perp(-d.y / n, d.x / n); + const cv::Point2f hw = perp * static_cast(half_width); + std::vector poly; + poly.push_back(a + hw); + poly.push_back(b + hw); + poly.push_back(b - hw); + poly.push_back(a - hw); + cv::fillConvexPoly(img, poly, color); +} + +cv::Mat renderArmor(const CameraCalib &calib, const cv::Vec3d &rvec, + const cv::Vec3d &tvec, const ArmorSize &size, + const std::string &color) { + const double W = size.width_mm, L = size.height_mm; + const std::vector obj{ + {0.f, static_cast(W / 2), static_cast(-L / 2)}, + {0.f, static_cast(W / 2), static_cast(L / 2)}, + {0.f, static_cast(-W / 2), static_cast(L / 2)}, + {0.f, static_cast(-W / 2), static_cast(-L / 2)}, + }; + std::vector pts; + cv::projectPoints(obj, rvec, tvec, calib.K(), calib.D(), pts); + + cv::Mat img(calib.imageSize(), CV_8UC3, cv::Scalar(10, 12, 16)); + cv::Mat noise(calib.imageSize(), CV_8UC3); + cv::randn(noise, cv::Scalar(0, 0, 0), cv::Scalar(6, 6, 6)); + img += noise; + + const cv::Scalar c = (color == "blue") ? cv::Scalar(230, 90, 30) + : cv::Scalar(30, 60, 240); + // 灯条宽 ≈ 长度/6.7（jlu 的 ratio_length_width） + const double half_w = L / 6.7 / 2.0; + drawBar(img, pts[0], pts[1], half_w, c); // 左条 + drawBar(img, pts[3], pts[2], half_w, c); // 右条 + + // 干扰：两个小亮斑（检测器应忽略） + cv::circle(img, cv::Point(160, 180), 4, cv::Scalar(180, 180, 180), -1); + cv::circle(img, cv::Point(1120, 860), 5, cv::Scalar(200, 200, 200), -1); + + cv::GaussianBlur(img, img, cv::Size(3, 3), 0.8); + return img; +} + +// --------------------------------------------------------------- +// 合成自检 +// --------------------------------------------------------------- +int runDetectSelftest(CameraCalib &calib, rm27::LightBarDetector &det) { + std::printf("=== 检测器合成自检 ===\n"); + std::printf("渲染已知位姿的发光装甲板 → 灯条检测 → PnP → 与真值对比\n\n"); + + auto tvecOf = [](double yawDeg, double pitchDeg, double d) { + const double y = yawDeg * CV_PI / 180.0, p = pitchDeg * CV_PI / 180.0; + return cv::Vec3d(d * std::sin(y) * std::cos(p), -d * std::sin(p), + d * std::cos(y) * std::cos(p)); + }; + auto rvecOf = [](double rollDeg, double yawDeg, double pitchDeg) { + const double r = rollDeg * CV_PI / 180.0, y = yawDeg * CV_PI / 180.0, + p = pitchDeg * CV_PI / 180.0; + cv::Mat R = (cv::Mat_(3, 3) << 0, 0, 1, 0, 1, 0, -1, 0, 0); + R = R * (cv::Mat_(3, 3) << 1, 0, 0, 0, 0, -1, 0, 1, 0); + cv::Mat Rz = (cv::Mat_(3, 3) << std::cos(r), -std::sin(r), 0, + std::sin(r), std::cos(r), 0, 0, 0, 1); + cv::Mat Ry = (cv::Mat_(3, 3) << std::cos(y), 0, std::sin(y), 0, + 1, 0, -std::sin(y), 0, std::cos(y)); + cv::Mat Rx = (cv::Mat_(3, 3) << 1, 0, 0, 0, std::cos(p), + -std::sin(p), 0, std::sin(p), std::cos(p)); + const cv::Mat M = Rx * Ry * Rz * R; + cv::Mat rv; + cv::Rodrigues(M, rv); + return cv::Vec3d(rv.at(0), rv.at(1), rv.at(2)); + }; + + struct Case { + const char *name; + cv::Vec3d tvec; + cv::Vec3d rvec; + bool is_big; + cv::Vec3d expect; // (距离m, yaw°, pitch°) + }; + const std::vector cases{ + {"正前方 1.5m 小", tvecOf(0, 0, 1500), rvecOf(0, 0, 0), false, {1.5, 0, 0}}, + {"右偏 20°, 2.0m 小", tvecOf(20, 0, 2000), rvecOf(0, 20, 0), false, {2.0, 20, 0}}, + {"上仰 10°, 2.5m 小", tvecOf(0, 10, 2500), rvecOf(0, 0, 10), false, {2.5, 0, 10}}, + {"右偏 15° 上仰 8°, 1.8m 小", tvecOf(15, 8, 1800), rvecOf(0, 15, 8), false, {1.8, 15, 8}}, + {"板内 roll 10°, 1.5m 小", tvecOf(0, 0, 1500), rvecOf(10, 0, 0), false, {1.5, 0, 0}}, + {"正前方 2.0m 大", tvecOf(0, 0, 2000), rvecOf(0, 0, 0), true, {2.0, 0, 0}}, + }; + + bool all_pass = true; + int idx = 0; + for (const auto &c : cases) { + const ArmorSize size = c.is_big ? ArmorSize::big() : ArmorSize::small(); + cv::Mat img = renderArmor(calib, c.rvec, c.tvec, size, det.config().enemy_color); + + auto armor = det.detect(img); + cv::Mat dbg = img.clone(); + rm27::LightBarDetector::drawDebug(dbg, det.bars(), armor); + char fname[64]; + std::snprintf(fname, sizeof(fname), "detect_case%d.jpg", idx); + cv::imwrite(fname, dbg); + + bool pass = false; + if (!armor) { + std::printf("%-28s ❌ 未检测到目标（灯条候选 %zu 个）\n", c.name, + det.bars().size()); + for (const auto &b : det.bars()) { + std::printf(" bar: len=%.1f w=%.1f tilt=%.1f pca=%d\n", + b.length, b.width, b.tilt_deg, (int)b.pca_ok); + } + } else { + auto pose = calib.solvePose(armor->corners, size); + if (!pose) { + std::printf("%-28s ❌ 检测到但 PnP 失败\n", c.name); + } else { + double yaw, pitch; + CameraCalib::poseToYawPitch(pose->tvec, yaw, pitch); + const double d_err = std::fabs(pose->distance_mm / 1000.0 - c.expect[0]); + const double y_err = std::fabs(yaw * 180.0 / CV_PI - c.expect[1]); + const double p_err = std::fabs(pitch * 180.0 / CV_PI - c.expect[2]); + const bool size_ok = armor->is_big == c.is_big; + pass = d_err < 0.05 * c.expect[0] && y_err < 1.5 && p_err < 1.5 && size_ok; + std::printf( + "%-28s 距离 %.3fm(真 %.3f) | yaw %+.2f°(真 %+.2f) | pitch %+.2f°(真 %+.2f) | " + "%s(真 %s) | 重投影 %.2fpx | 灯条%zu %s\n", + c.name, pose->distance_mm / 1000.0, c.expect[0], + yaw * 180.0 / CV_PI, c.expect[1], pitch * 180.0 / CV_PI, + c.expect[2], armor->is_big ? "大" : "小", c.is_big ? "大" : "小", + pose->reproj_error_px, det.bars().size(), + pass ? "✅" : "❌"); + if (idx == 0) { + // 对照真值角点与检测角点（诊断用） + const double W = size.width_mm, L = size.height_mm; + const std::vector obj{ + {0.f, (float)(W / 2), (float)(-L / 2)}, + {0.f, (float)(W / 2), (float)(L / 2)}, + {0.f, (float)(-W / 2), (float)(L / 2)}, + {0.f, (float)(-W / 2), (float)(-L / 2)}}; + std::vector gt; + cv::projectPoints(obj, c.rvec, c.tvec, calib.K(), calib.D(), gt); + std::printf(" 真值: (%7.1f,%7.1f)(%7.1f,%7.1f)(%7.1f,%7.1f)(%7.1f,%7.1f)\n", + gt[0].x, gt[0].y, gt[1].x, gt[1].y, gt[2].x, gt[2].y, gt[3].x, gt[3].y); + std::printf(" 检测: (%7.1f,%7.1f)(%7.1f,%7.1f)(%7.1f,%7.1f)(%7.1f,%7.1f)\n", + armor->corners[0].x, armor->corners[0].y, + armor->corners[1].x, armor->corners[1].y, + armor->corners[2].x, armor->corners[2].y, + armor->corners[3].x, armor->corners[3].y); + for (const auto &b : det.bars()) { + std::printf(" 灯条: top(%6.1f,%6.1f) bottom(%6.1f,%6.1f) " + "len=%.1f width=%.1f pca=%d\n", + b.top.x, b.top.y, b.bottom.x, b.bottom.y, + b.length, b.width, (int)b.pca_ok); + } + } + } + } + all_pass = all_pass && pass; + ++idx; + } + std::printf("\n调试图已存 detect_case0~%d.jpg（工程根目录，.gitignore 已忽略）\n", idx - 1); + std::printf("=== 检测自检%s ===\n", all_pass ? "全部通过 ✅" : "有失败项 ❌"); + return all_pass ? 0 : 1; +} + +// --------------------------------------------------------------- +// 参数解析与主流程 +// --------------------------------------------------------------- +void printUsage() { + std::printf( + "用法:\n" + " detect_test 相机实时\n" + " detect_test --image 照片.jpg 静态图交互\n" + " detect_test --image 照片.jpg --once 跑一次退出（脚本/沙箱用）\n" + " detect_test --selftest 合成图自检\n" + "\n" + " 其他: --calib 标定文件 | --lightbar 检测参数 | --runtime 相机运行时参数\n" + "\n" + "按键: q/ESC 退出 | s 截图 | c 切换敌方颜色 | r 重载检测参数\n"); +} + +} // namespace + +int main(int argc, char **argv) { + std::string image_path, calib_path = "configs/camera.yaml", + lightbar_path = "configs/light_bar.yaml", + runtime_path = "configs/camera_runtime.yaml"; + bool once = false, selftest = false; + for (int i = 1; i < argc; ++i) { + std::string a = argv[i]; + if (a == "--image" && i + 1 < argc) { + image_path = argv[++i]; + } else if (a == "--calib" && i + 1 < argc) { + calib_path = argv[++i]; + } else if (a == "--lightbar" && i + 1 < argc) { + lightbar_path = argv[++i]; + } else if (a == "--runtime" && i + 1 < argc) { + runtime_path = argv[++i]; + } else if (a == "--once") { + once = true; + } else if (a == "--selftest") { + selftest = true; + } else if (a == "-h" || a == "--help") { + printUsage(); + return 0; + } else { + std::fprintf(stderr, "未知参数: %s\n", a.c_str()); + printUsage(); + return 2; + } + } + + CameraCalib calib(calib_path); + if (!calib.valid()) return -1; + rm27::LightBarDetector det(lightbar_path); + + if (selftest) { + return runDetectSelftest(calib, det); + } + + // ---------- 交互/实时 ---------- + const bool use_camera = image_path.empty(); + CameraGrabber cam; + cv::Mat raw; + + if (use_camera) { + if (!cam.initializeCamera()) { + std::fprintf(stderr, "相机初始化失败\n"); + return -1; + } + // 应用曝光/增益（与 pose_test 相同的逻辑） + cv::FileStorage rfs(runtime_path, cv::FileStorage::READ); + if (rfs.isOpened()) { + int ae = 0, autoWb = 0, fspeed = 2; + double exposureUs = 3000.0, gain = 2.0; + rfs["ae_state"] >> ae; + rfs["exposure_time_us"] >> exposureUs; + rfs["analog_gain"] >> gain; + rfs["auto_wb"] >> autoWb; + rfs["frame_speed"] >> fspeed; + rfs.release(); + cam.setAeState(ae != 0); + cam.setExposureTime(exposureUs); + cam.setAnalogGainTimes(gain); + cam.setWbMode(autoWb != 0); + cam.setFrameSpeed(fspeed); + std::printf("相机参数: 曝光 %s %.0fus 增益 %.2fx\n", ae ? "自动" : "手动", + exposureUs, gain); + } + if (!cam.startCamera()) { + std::fprintf(stderr, "相机取流失败\n"); + return -1; + } + } else { + raw = cv::imread(image_path); + if (raw.empty()) { + std::fprintf(stderr, "打不开图片: %s\n", image_path.c_str()); + return -1; + } + } + + std::printf("q=退出 s=截图 c=切敌方颜色 r=重载参数\n"); + if (!once) { + cv::namedWindow("detect_test", cv::WINDOW_NORMAL); + } + + int frame_idx = 0; + bool run_once = true; + while (true) { + if (use_camera && !cam.getFrame(raw)) { + std::printf("取帧失败，退出\n"); + break; + } + + const int64 t0 = cv::getTickCount(); + auto armor = det.detect(raw); + const double dt_ms = + (cv::getTickCount() - t0) * 1000.0 / cv::getTickFrequency(); + + if (armor) { + const ArmorSize size = armor->is_big ? ArmorSize::big() + : ArmorSize::small(); + if (auto pose = calib.solvePose(armor->corners, size)) { + double yaw, pitch; + CameraCalib::poseToYawPitch(pose->tvec, yaw, pitch); + std::printf( + "%s装甲 | 距离 %.3f m | yaw %+.2f° | pitch %+.2f° | " + "重投影 %.2fpx | 检测 %.1fms\n", + armor->is_big ? "大" : "小", pose->distance_mm / 1000.0, + yaw * 180.0 / CV_PI, pitch * 180.0 / CV_PI, + pose->reproj_error_px, dt_ms); + } + } else { + std::printf("无目标（灯条候选 %zu）\n", det.bars().size()); + for (const auto &b : det.bars()) { + std::printf(" bar len=%.1f w=%.1f ratio=%.1f tilt=%.1f pca=%d\n", + b.length, b.width, b.length / b.width, b.tilt_deg, + (int)b.pca_ok); + } + } + + if (!once) { + cv::Mat view = raw.clone(); + rm27::LightBarDetector::drawDebug(view, det.bars(), armor); + cv::putText(view, "q=quit s=save c=color r=reload", + cv::Point(10, 28), cv::FONT_HERSHEY_SIMPLEX, 0.6, + cv::Scalar(0, 255, 0), 2); + cv::imshow("detect_test", view); + const int key = cv::waitKey(5) & 0xFF; + if (key == 27 || key == 'q') break; + if (key == 'c') { + const std::string c = det.config().enemy_color == "blue" ? "red" : "blue"; + det.setEnemyColor(c); + std::printf("敌方颜色切换为 %s\n", c.c_str()); + } + if (key == 'r') { + det.loadConfig(lightbar_path); + } + if (key == 's') { + char name[64]; + std::snprintf(name, sizeof(name), "detect_%04d.jpg", frame_idx); + cv::imwrite(name, view); + std::printf("已保存 %s\n", name); + } + } + if (once) { + break; + } + ++frame_idx; + } + + if (use_camera) cam.stopCamera(); + return 0; +} diff --git a/src/light_bar.cpp b/src/light_bar.cpp index e69de29..9fedafd 100644 --- a/src/light_bar.cpp +++ b/src/light_bar.cpp @@ -0,0 +1,395 @@ +#include "light_bar.hpp" + +#include +#include +#include + +namespace rm27 { + +// ------------------------------------------------------------------ +// 配置加载 +// ------------------------------------------------------------------ +bool LightBarDetector::loadConfig(const std::string &yaml_path) { + cv::FileStorage fs(yaml_path, cv::FileStorage::READ); + if (!fs.isOpened()) { + std::fprintf(stderr, + "[LightBar] 打不开 %s，使用内置默认参数（敌方=蓝）\n", + yaml_path.c_str()); + return false; + } + auto readI = [&](const char *k, int &v) { + if (!fs[k].isNone()) fs[k] >> v; + }; + auto readD = [&](const char *k, double &v) { + if (!fs[k].isNone()) fs[k] >> v; + }; + auto readF = [&](const char *k, float &v) { + if (!fs[k].isNone()) fs[k] >> v; + }; + int tmp = 0; + + fs["enemy_color"] >> cfg_.enemy_color; + + readI("blue_h_min", cfg_.blue_h_min); + readI("blue_h_max", cfg_.blue_h_max); + readI("blue_s_min", cfg_.blue_s_min); + readI("blue_v_min", cfg_.blue_v_min); + readI("red_h_min_1", cfg_.red_h_min_1); + readI("red_h_max_1", cfg_.red_h_max_1); + readI("red_h_min_2", cfg_.red_h_min_2); + readI("red_h_max_2", cfg_.red_h_max_2); + readI("red_s_min", cfg_.red_s_min); + readI("red_v_min", cfg_.red_v_min); + readI("morph_kernel", cfg_.morph_kernel); + readI("morph_iters", cfg_.morph_iters); + + readD("min_length", cfg_.min_length); + readD("min_ratio", cfg_.min_ratio); + readD("max_tilt_deg", cfg_.max_tilt_deg); + readD("max_area", cfg_.max_area); + readD("max_angle_diff_deg", cfg_.max_angle_diff_deg); + readD("max_len_ratio", cfg_.max_len_ratio); + readD("max_center_y_ratio", cfg_.max_center_y_ratio); + readD("big_small_ratio_thres", cfg_.big_small_ratio_thres); + + if (!fs["use_pca"].isNone()) { + fs["use_pca"] >> tmp; + cfg_.use_pca = tmp != 0; + } + readI("pass_optimize_lightbar_width", cfg_.pass_optimize_lightbar_width); + readF("normalize_max_brightness", cfg_.normalize_max_brightness); + readF("lightbar_min_mean_brightness", cfg_.lightbar_min_mean_brightness); + readF("padding_scale", cfg_.padding_scale); + readF("search_start_ratio", cfg_.search_start_ratio); + readF("search_end_ratio", cfg_.search_end_ratio); + fs.release(); + + std::printf("[LightBar] %s 已加载: 敌方=%s PCA=%s\n", yaml_path.c_str(), + cfg_.enemy_color.c_str(), cfg_.use_pca ? "开" : "关"); + return true; +} + +// ------------------------------------------------------------------ +// 轮廓 → 灯条（借鉴 jlu types.cpp LightBar(contour)） +// ------------------------------------------------------------------ +LightBar LightBarDetector::fromContour(const std::vector &contour) { + LightBar bar; + bar.rect = cv::minAreaRect(contour); + + // 用轮廓质心当中心（比矩形对角线交点稳） + cv::Point2f c(0.f, 0.f); + for (const auto &p : contour) { + c += cv::Point2f(static_cast(p.x), static_cast(p.y)); + } + bar.center = c / static_cast(contour.size()); + + // 4 角按 y 排序 → 上两点中点=top，下两点中点=bottom + cv::Point2f p[4]; + bar.rect.points(p); + std::sort(p, p + 4, + [](const cv::Point2f &a, const cv::Point2f &b) { return a.y < b.y; }); + bar.top = (p[0] + p[1]) * 0.5f; + bar.bottom = (p[2] + p[3]) * 0.5f; + bar.length = cv::norm(bar.top - bar.bottom); + bar.width = cv::norm(p[0] - p[1]); + + cv::Point2f ax = bar.top - bar.bottom; + const float n = cv::norm(ax); + bar.axis = n > 0.f ? ax / n : cv::Point2f(0.f, -1.f); + bar.tilt_deg = static_cast( + std::atan2(std::fabs(bar.axis.x), std::fabs(bar.axis.y)) * 180.0 / + CV_PI); + bar.signed_tilt = std::atan2(static_cast(bar.axis.x), + -static_cast(bar.axis.y)); + return bar; +} + +// ------------------------------------------------------------------ +// 颜色掩膜 → 灯条候选 +// ------------------------------------------------------------------ +std::vector LightBarDetector::findBars(const cv::Mat &bgr) { + cv::Mat hsv; + cv::cvtColor(bgr, hsv, cv::COLOR_BGR2HSV); + cv::Mat mask(hsv.size(), CV_8UC1, cv::Scalar(0)); + + const bool blue = (cfg_.enemy_color == "blue" || cfg_.enemy_color == "Blue"); + if (blue) { + cv::inRange(hsv, + cv::Scalar(cfg_.blue_h_min, cfg_.blue_s_min, cfg_.blue_v_min), + cv::Scalar(cfg_.blue_h_max, 255, 255), mask); + } else { + cv::Mat m1, m2; + cv::inRange(hsv, + cv::Scalar(cfg_.red_h_min_1, cfg_.red_s_min, cfg_.red_v_min), + cv::Scalar(cfg_.red_h_max_1, 255, 255), m1); + cv::inRange(hsv, + cv::Scalar(cfg_.red_h_min_2, cfg_.red_s_min, cfg_.red_v_min), + cv::Scalar(cfg_.red_h_max_2, 255, 255), m2); + cv::bitwise_or(m1, m2, mask); + } + + if (cfg_.morph_kernel > 0) { + const cv::Mat k = cv::getStructuringElement( + cv::MORPH_RECT, cv::Size(cfg_.morph_kernel, cfg_.morph_kernel)); + cv::morphologyEx(mask, mask, cv::MORPH_CLOSE, k, cv::Point(-1, -1), + cfg_.morph_iters); + } + mask_debug_ = mask.clone(); + + std::vector> contours; + cv::findContours(mask, contours, cv::RETR_EXTERNAL, cv::CHAIN_APPROX_NONE); + + std::vector bars; + for (const auto &contour : contours) { + const double area = cv::contourArea(contour); + if (area < 20.0 || area > cfg_.max_area) continue; + + LightBar bar = fromContour(contour); + if (bar.length < cfg_.min_length) continue; + if (bar.width < 1.f) continue; + if (bar.length / bar.width < cfg_.min_ratio) continue; + if (bar.tilt_deg > cfg_.max_tilt_deg) continue; + bars.push_back(bar); + } + return bars; +} + +// ------------------------------------------------------------------ +// PCA 角点修正（借鉴 jlu lightbar_corrector.cpp） +// ------------------------------------------------------------------ +std::optional LightBarDetector::findCorner( + const cv::Mat &gray, const LightBar &bar, const cv::Point2f &axis_dir, + const cv::Point2f ¢roid, float mean_val, bool toward_top) { + const float L = bar.length; + const float oper = toward_top ? 1.f : -1.f; + const float dx = axis_dir.x * oper; + const float dy = axis_dir.y * oper; + + std::vector candidates; + const int n = static_cast(bar.width) - 2; + const int half_n = static_cast(std::round(n / 2.0)); + for (int i = -half_n; i <= half_n; ++i) { + const float x0 = centroid.x + L * cfg_.search_start_ratio * dx + + static_cast(i); + const float y0 = centroid.y + L * cfg_.search_start_ratio * dy; + + cv::Point2f prev(x0, y0); + cv::Point2f corner(x0, y0); + float max_diff = 0.f; + bool has_corner = false; + for (float x = x0 + dx, y = y0 + dy; + std::sqrt((x - x0) * (x - x0) + (y - y0) * (y - y0)) < + L * (cfg_.search_end_ratio - cfg_.search_start_ratio); + x += dx, y += dy) { + const cv::Point2f cur(x, y); + if (cur.x < 0 || cur.x >= gray.cols || cur.y < 0 || + cur.y >= gray.rows) { + break; + } + // 沿轴从亮（灯条内）走向暗（背景），亮度骤降处 = 端点 + const float diff = + static_cast(gray.at(static_cast(prev.y), + static_cast(prev.x))) - + static_cast(gray.at(static_cast(cur.y), + static_cast(cur.x))); + if (diff > max_diff && + static_cast(gray.at(static_cast(prev.y), + static_cast(prev.x))) > + mean_val) { + max_diff = diff; + corner = prev; + has_corner = true; + } + prev = cur; + } + if (has_corner) { + candidates.push_back(corner); + } + } + if (candidates.empty()) { + return std::nullopt; + } + cv::Point2f sum(0.f, 0.f); + for (const auto &p : candidates) { + sum += p; + } + return sum / static_cast(candidates.size()); +} + +bool LightBarDetector::correctBar(const cv::Mat &gray, LightBar &bar) { + if (bar.width <= static_cast(cfg_.pass_optimize_lightbar_width)) { + return false; + } + // 1) 扩大包围盒，留出搜索余量 + cv::Rect box = bar.rect.boundingRect(); + const int padx = static_cast(box.width * cfg_.padding_scale); + const int pady = static_cast(box.height * cfg_.padding_scale); + box.x -= padx; + box.y -= pady; + box.width += padx * 2; + box.height += pady * 2; + box.x = std::max(box.x, 0); + box.y = std::max(box.y, 0); + box.x = std::min(box.x, gray.cols - 1); + box.y = std::min(box.y, gray.rows - 1); + box.width = std::min(box.width, gray.cols - box.x); + box.height = std::min(box.height, gray.rows - box.y); + if (box.width <= 0 || box.height <= 0) { + return false; + } + + cv::Mat roi = gray(box); + const float mean_val = static_cast(cv::mean(roi)[0]); + if (mean_val <= cfg_.lightbar_min_mean_brightness) { + return false; // 太暗，不是灯条 + } + cv::Mat roi_f; + roi.convertTo(roi_f, CV_32F); + cv::normalize(roi_f, roi_f, 0, cfg_.normalize_max_brightness, cv::NORM_MINMAX); + + // 2) 矩分析求对称轴（jlu：从点云 PCA 换成矩分析，更快） + const cv::Moments m = cv::moments(roi_f, false); + if (m.m00 == 0.0) { + return false; + } + cv::Point2f centroid(static_cast(m.m10 / m.m00), + static_cast(m.m01 / m.m00)); + centroid += cv::Point2f(static_cast(box.x), + static_cast(box.y)); + const double mu20 = m.mu20, mu11 = m.mu11, mu02 = m.mu02; + if (mu20 == 0.0 && mu11 == 0.0 && mu02 == 0.0) { + return false; + } + const double theta = 0.5 * std::atan2(2.0 * mu11, mu20 - mu02); + cv::Point2f axis(static_cast(std::cos(theta)), + static_cast(std::sin(theta))); + axis /= cv::norm(axis); + if (axis.y > 0) { + axis = -axis; // 指向上方（图像坐标 y 向下） + } + + // 3) 沿轴找两端角点（最大亮度下降处） + auto top = findCorner(gray, bar, axis, centroid, mean_val, true); + auto bottom = findCorner(gray, bar, axis, centroid, mean_val, false); + if (!top || !bottom) { + return false; + } + + bar.center = centroid; + bar.axis = axis; + bar.top = *top; + bar.bottom = *bottom; + bar.length = cv::norm(bar.top - bar.bottom); + bar.signed_tilt = + std::atan2(static_cast(bar.axis.x), -static_cast(bar.axis.y)); + bar.pca_ok = true; + return true; +} + +// ------------------------------------------------------------------ +// 灯条配对 → 装甲板 +// ------------------------------------------------------------------ +std::optional LightBarDetector::pairBars( + const cv::Mat & /*gray*/, std::vector &bars) { + ArmorTarget best; + bool found = false; + double best_score = 0.0; + + const int n = static_cast(bars.size()); + for (int i = 0; i < n; ++i) { + for (int j = i + 1; j < n; ++j) { + LightBar l = bars[i]; + LightBar r = bars[j]; + if (l.center.x > r.center.x) { + std::swap(l, r); // l = 左条, r = 右条 + } + + const float avg_len = (l.length + r.length) * 0.5f; + const float dist = std::fabs(l.center.x - r.center.x); + + // ① 倾角差 + const double angle_diff = + std::fabs(l.signed_tilt - r.signed_tilt) * 180.0 / CV_PI; + if (angle_diff > cfg_.max_angle_diff_deg) continue; + + // ② 长度比 + const float len_ratio = + std::max(l.length, r.length) / std::max(1e-3f, std::min(l.length, r.length)); + if (len_ratio > cfg_.max_len_ratio) continue; + + // ③ 中心纵向差 + if (std::fabs(l.center.y - r.center.y) > + cfg_.max_center_y_ratio * avg_len) continue; + + // ④ 中心横向距离合理（小装甲 ≈2.4 倍灯条长，大装甲 ≈4.2 倍） + if (dist < avg_len * 0.8f || dist > avg_len * 6.5f) continue; + + // ⑤ PCA 精修（开了就必须两边都成功，避免半精修导致 yaw 偏差） + if (cfg_.use_pca && !(l.pca_ok && r.pca_ok)) continue; + + const double score = + static_cast(avg_len) - std::fabs(l.center.y - r.center.y); + if (!found || score > best_score) { + found = true; + best_score = score; + best.left_light = l; + best.right_light = r; + best.center = (l.center + r.center) * 0.5f; + // 角点顺序：板左下 → 板左上 → 板右上 → 板右下 + // ⚠️ 镜像：板子朝向相机时，板的左条(+y)出现在画面右侧， + // 所以「板左下」= 画面右条的底端（r = 画面右条） + best.corners = {r.bottom, r.top, l.top, l.bottom}; + best.is_big = (dist / avg_len) > cfg_.big_small_ratio_thres; + } + } + } + if (!found) { + return std::nullopt; + } + return best; +} + +// ------------------------------------------------------------------ +// 主入口 +// ------------------------------------------------------------------ +std::optional LightBarDetector::detect(const cv::Mat &bgr) { + if (bgr.empty()) { + return std::nullopt; + } + bars_ = findBars(bgr); + if (cfg_.use_pca) { + cv::Mat gray; + cv::cvtColor(bgr, gray, cv::COLOR_BGR2GRAY); + for (auto &b : bars_) { + correctBar(gray, b); + } + } + return pairBars(bgr, bars_); +} + +// ------------------------------------------------------------------ +// 调试绘制 +// ------------------------------------------------------------------ +void LightBarDetector::drawDebug(cv::Mat &img, const std::vector &bars, + const std::optional &armor) { + for (const auto &b : bars) { + cv::line(img, b.bottom, b.top, cv::Scalar(0, 255, 255), 2); + cv::circle(img, b.center, 3, cv::Scalar(0, 255, 255), -1); + } + if (!armor) { + return; + } + const auto &a = *armor; + for (int k = 0; k < 4; ++k) { + cv::line(img, a.corners[k], a.corners[(k + 1) % 4], + cv::Scalar(0, 255, 0), 2); + } + for (const auto &p : a.corners) { + cv::circle(img, p, 4, cv::Scalar(0, 255, 0), -1); + } + cv::putText(img, a.is_big ? "BIG" : "SMALL", + a.center + cv::Point2f(-30.f, -15.f), cv::FONT_HERSHEY_SIMPLEX, + 0.8, cv::Scalar(0, 255, 0), 2); +} + +} // namespace rm27 diff --git a/src/pose_test.cpp b/src/pose_test.cpp index fdd38da..c98252a 100644 --- a/src/pose_test.cpp +++ b/src/pose_test.cpp @@ -3,7 +3,7 @@ * @brief 几何链路验证工具：4 角点 → PnP → 距离 / yaw / pitch * * 在写装甲板检测器之前，先用它验证「标定 + 坐标约定 + PnP 解算」 - * 是否正确：拿一块真的装甲板（或打印的 135mm 矩形）摆在相机前， + * 是否正确：拿一块真的装甲板（或打印的 131.56×55mm 矩形）摆在相机前， * 鼠标点 4 个角，用卷尺/量角器核对输出的距离与角度。 * 之后接任何检测器出问题，都一定是检测的锅。 * @@ -55,7 +55,7 @@ void printUsage() "\n" " 其他: --calib 标定文件(默认 configs/camera.yaml)\n" " --runtime 相机运行时参数(默认 configs/camera_runtime.yaml)\n" - " --big 大装甲板 230mm（默认小装甲板 135mm）\n" + " --big 大装甲板 230mm（默认小装甲板 131.56mm，实测 3 号板）\n" " --size WxL 自定义矩形尺寸(毫米)，如 --size 180x120\n" "\n" "相机模式按键: r 清除重选 | b 切换大小装甲 | u 切换去畸变显示 | s 截图 | q/ESC 退出\n"); @@ -405,7 +405,7 @@ int main(int argc, char **argv) } } - std::printf("装甲板: %s (%.0f x %.0f mm)\n", bigArmor ? "大 230mm" : "小 135mm", + std::printf("装甲板: %s (%.2f x %.2f mm)\n", bigArmor ? "大 230mm" : "小(实测3号)", size.width_mm, size.height_mm); std::printf("请按顺序点 4 个角: 左下 → 左上 → 右上 → 右下\n"); std::printf("r=清除 b=切换大小装甲 u=切换去畸变显示 s=截图 q=退出\n"); diff --git a/xmake.lua b/xmake.lua index 581dc05..2c97987 100644 --- a/xmake.lua +++ b/xmake.lua @@ -30,6 +30,7 @@ target("27RM") remove_files("src/capture_calib.cpp") remove_files("src/calibrate.cpp") remove_files("src/pose_test.cpp") + remove_files("src/detect_test.cpp") -- 标定图采集工具 -- ./build/linux/x86_64/debug/capture_calib （MindVision 相机） @@ -53,3 +54,13 @@ target("pose_test") add_rules("rm_deps") set_kind("binary") add_files("src/pose_test.cpp", "src/camera_calib.cpp", "src/camera_set.cpp") + +-- 灯条检测验证工具（检测 → 4 角点 → PnP → 距离/yaw/pitch） +-- ./build/linux/x86_64/debug/detect_test （相机实时） +-- ./build/linux/x86_64/debug/detect_test --selftest （合成自检，无需相机） +-- ./build/linux/x86_64/debug/detect_test --image x.jpg --once +target("detect_test") + add_rules("rm_deps") + set_kind("binary") + add_files("src/detect_test.cpp", "src/light_bar.cpp", + "src/camera_calib.cpp", "src/camera_set.cpp")