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
This commit is contained in:
@@ -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
|
||||
|
||||
+126
-7
@@ -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 <opencv2/opencv.hpp>
|
||||
|
||||
#include <array>
|
||||
#include <optional>
|
||||
#include <string>
|
||||
#include <vector>
|
||||
|
||||
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<cv::Point2f, 4> 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<ArmorTarget> detect(const cv::Mat &bgr);
|
||||
|
||||
const LightBarConfig &config() const { return cfg_; }
|
||||
const std::vector<LightBar> &bars() const { return bars_; }
|
||||
const cv::Mat &mask() const { return mask_debug_; }
|
||||
|
||||
/** 调试绘制：灯条（黄）、配对矩形与角点（绿） */
|
||||
static void drawDebug(cv::Mat &img, const std::vector<LightBar> &bars,
|
||||
const std::optional<ArmorTarget> &armor);
|
||||
|
||||
/** 切换敌方颜色（现场调试用） */
|
||||
void setEnemyColor(const std::string &color) { cfg_.enemy_color = color; }
|
||||
|
||||
private:
|
||||
std::vector<LightBar> findBars(const cv::Mat &bgr);
|
||||
std::optional<ArmorTarget> pairBars(const cv::Mat &gray,
|
||||
std::vector<LightBar> &bars);
|
||||
|
||||
/** 从轮廓构造灯条（借鉴 jlu types.cpp LightBar(contour)） */
|
||||
static LightBar fromContour(const std::vector<cv::Point> &contour);
|
||||
|
||||
};
|
||||
/** PCA 角点修正（借鉴 jlu lightbar_corrector）；成功返回 true */
|
||||
bool correctBar(const cv::Mat &gray, LightBar &bar);
|
||||
std::optional<cv::Point2f> findCorner(const cv::Mat &gray,
|
||||
const LightBar &bar,
|
||||
const cv::Point2f &axis_dir,
|
||||
const cv::Point2f ¢roid,
|
||||
float mean_val, bool toward_top);
|
||||
|
||||
#endif
|
||||
LightBarConfig cfg_;
|
||||
std::vector<LightBar> bars_;
|
||||
cv::Mat mask_debug_;
|
||||
};
|
||||
|
||||
} // namespace rm27
|
||||
|
||||
#endif // LIGHT_BAR_HPP
|
||||
|
||||
Reference in New Issue
Block a user