spdis
730 lines
27 KiB
C++
730 lines
27 KiB
C++
/*
|
||
* 斑马线实时检测 — 龙芯2K0300 小车摄像头版 (含逆透视变换 IPM)
|
||
* =================================================================
|
||
*
|
||
* 【设计目的】
|
||
* 在车辆行驶过程中实时识别前方斑马线，将检测结果写入标志文件
|
||
* 供巡线主控模块 (control.cpp) 读取，做出减速/停车等安全决策。
|
||
*
|
||
* 【算法来源】
|
||
* 移植自 Python 开源项目: https://github.com/TomMao23/ZebraCrossing_Detection
|
||
* 利用斑马线的四个视觉特征做传统图像处理检测:
|
||
* (1) 梯度一致性 — 黑白交替 → 边缘方向高度集中
|
||
* (2) 等间隔 — 线条等距分布 (当前未使用)
|
||
* (3) 多根线 — 一个区域内有大量平行边缘
|
||
* (4) 宽度大 — 斑马线宽度明显大于车道线
|
||
*
|
||
* 【处理流水线 (每帧)】
|
||
*
|
||
* 摄像头 320×240 (MJPG, /dev/video0)
|
||
* ↓
|
||
* ┌── [IPM] 逆透视变换 ──────────────────────────────┐
|
||
* │ 前视图 → 鸟瞰图 (BEV, Bird's Eye View) │
|
||
* │ cv::warpPerspective(frame, bev, iM, 640×640) │
|
||
* │ 将倾斜的透视角度"拉平"，使斑马线恢复矩形形状 │
|
||
* │ 矩阵 M 需针对摄像头安装角度/高度单独标定 │
|
||
* │ 标定方法：拍摄地面矩形标定板 → 四点透视变换 │
|
||
* └──────────────────────────────────────────────────┘
|
||
* ↓ if USE_IPM
|
||
* 640×640 BEV 图 (已校正透视畸变)
|
||
* ↓ cv::resize
|
||
* 400×400 正方形 BGR 图
|
||
* ↓ extractChannel(0) → 只取 B 通道 (利用蓝色通道衰减黄色减速带)
|
||
* medianBlur 5×5 → 去砖缝/椒盐噪声, 保留梯度大小
|
||
* ↓
|
||
* MORPH_OPEN 3×3 iter=4 → 多次腐蚀去窄车道线, 斑马线保留
|
||
* ↓
|
||
* MORPH_CLOSE 5×5 iter=3 → 先膨胀填斑马线缺损, 再腐蚀抑制路面箭头碎片
|
||
* ↓
|
||
* Canny(30, 90) → 双阈值边缘检测, 利用连通性合并弱边缘
|
||
* ↓
|
||
* Sobel 3×3 → 计算每个边缘像素的梯度模值 Amplitude 和方向 theta(0~90°)
|
||
* ↓ 弱边缘过滤 (Amplitude < 30 → 置零)
|
||
* 滑动窗口 100×302, 步长 50, 水平范围固定 [39, 341]
|
||
* ↓
|
||
* 每个窗口内:
|
||
* - 统计有效梯度点 (Amplitude > 0, theta ∈ [0, 70°)) 的方向直方图
|
||
* - 14 个 bin, 每 bin 5° (0°~5°, 5°~10°, ..., 65°~70°)
|
||
* - 取峰值 bin 的点数
|
||
* - 峰值 > 1500 → 判为斑马线
|
||
* ↓
|
||
* 所有阳性窗口的纵向跨度 = 最终斑马线位置 (紫色框)
|
||
* ↓
|
||
* LCD 叠加显示 + 写入 ./zebra_detected 文件
|
||
*
|
||
* 【为何需要逆透视变换 (IPM)】
|
||
* 前视摄像头拍到的斑马线是透视畸变的 (近大远小、倾斜变形)
|
||
* → 斑马线的平行特征被破坏 → 梯度一致性减弱 → 检测效果下降
|
||
* IPM 将图像变换为鸟瞰视角 → 斑马线恢复矩形/平行特征
|
||
* → Sobel 梯度方向集中在同一角度 → 直方图峰值更高 → 检测更准
|
||
*
|
||
* 【注意】IPM 矩阵必须针对实车摄像头标定！
|
||
* 当前矩阵是 Python 项目原始标定值，在 2K0300 小车上可能不适用。
|
||
* 建议先关闭 IPM 测试 (USE_IPM=0)，确认检测逻辑正确后，
|
||
* 再用标定板重新标定并开启 IPM。
|
||
*
|
||
* 【为何方向限制在 0~70°】
|
||
* 排除水平停止线干扰:
|
||
* 停止线 = 横线 → Sobel dx 大 / dy → 0 → 方向接近 0°
|
||
* 斑马线 = 纵线/斜线 → 方向偏大 (30°~90°)
|
||
* 实际上 bin 0 也在统计范围但峰值不高, 主要靠阈值过滤
|
||
*
|
||
* 【为何窗口固定水平范围 [39, 341]】
|
||
* 排除道路两侧的行人道/建筑/绿化等干扰
|
||
* 400 像素宽, 窗口 302 宽, 居中左偏 39px, 右侧留 59px
|
||
*
|
||
* 【为何单独用蓝色通道而非灰度图】
|
||
* BGR 三通道中:
|
||
* B 通道 = 蓝色分量
|
||
* 黄色减速带 ≈ R+G (无蓝色) → B 通道响应 ≈ 0 → 完全消除减速带边缘
|
||
* 白色斑马线 ≈ R+G+B 均等 → B 通道正常响应 → 斑马线边缘保留
|
||
* 如果用灰度 (0.299R + 0.587G + 0.114B), 黄色减速带会产生强边缘
|
||
*
|
||
* 【为何用中值滤波而非高斯/均值滤波】
|
||
* 中值滤波最大程度保留原始梯度大小 (不模糊边缘), 同时完美消灭人行道砖缝纹理
|
||
* 高斯/均值会平滑边缘 → 梯度幅度衰减 → 影响判定阈值
|
||
*
|
||
* 【为何先开运算再闭运算】
|
||
* 开运算 (先 Erosion 后 Dilation):
|
||
* iter=4 次连续腐蚀 → 窄车道线被彻底腐蚀消失
|
||
* 后续膨胀 → 恢复剩余斑马线的尺寸
|
||
* 等效: 保留 "宽度 > 某阈值" 的线条 (斑马线)，去除 "窄的" 线条 (车道线)
|
||
* 闭运算 (先 Dilation 后 Erosion):
|
||
* 先膨胀 → 填补斑马线内部的微小缺损/裂缝
|
||
* 后腐蚀 → 恢复原始尺寸，同时消除箭头碎片的虚假边缘
|
||
*
|
||
* 【为何用 Canny 而非直接对 Sobel 结果判定】
|
||
* (1) Canny 双阈值 + 连通性分析 → 孤立噪点被抑制, 只有真正连续的边缘通过
|
||
* (2) Canny 使一条边缘只保留最细的 1px 脊线 → 斑马线 "多根线" 特征更突出
|
||
* (直接 Sobel 一根粗边缘产生 N 个梯度点 → 降低了线数的区分度)
|
||
* (3) Canny 两个阈值让调参范围更大, 比单梯度阈值更鲁棒
|
||
*
|
||
* 【2K0300 算力评估 (400×400 分辨率)】
|
||
* IPM warpPerspective: O(640×640×常数) ≈ 2.5M
|
||
* 蓝色通道提取: O(160K) ≈ 忽略
|
||
* medianBlur 5×5: O(160K × 25) ≈ 4M
|
||
* morphOpen ×4: O(160K × 9 × 8) ≈ 11.5M
|
||
* morphClose ×3: O(160K × 25 × 6) ≈ 24M
|
||
* Canny: O(160K × 常数) ≈ 几M
|
||
* Sobel ×2: O(160K × 9 × 2) ≈ 2.9M
|
||
* 梯度逐像素: O(160K × 10) ≈ 1.6M
|
||
* 滑动窗口 (7窗): O(7 × 30200) ≈ 0.2M
|
||
* ---------------------------------------------------
|
||
* 总计约 50~55M 操作/帧, 2K0300 0.43 GOPS → 理论 7~10 FPS
|
||
* 优化: 关闭 IPM 可节省 ~2.5M/帧; 增大 WIN_STEP 减少窗口数
|
||
*
|
||
* 用法:
|
||
* ./zebra_demo 带 LCD 显示 + 检测 (含 IPM)
|
||
* ./zebra_demo --no-display 无头模式 (只写状态文件, 省算力)
|
||
* ./zebra_demo --no-ipm 关闭逆透视变换 (使用前视图)
|
||
*
|
||
* 跨模块通信:
|
||
* 写入 ./zebra_detected: "1" = 检测到斑马线, "0" = 未检测到
|
||
* 控制模块 (control.cpp) 可用 readFlag("./zebra_detected") 读取
|
||
* 仅在状态变化时写入, 减少文件 I/O
|
||
*
|
||
* 退出: 按 Ctrl+C (SIGINT) 或 kill (SIGTERM) 优雅清理资源
|
||
*/
|
||
|
||
#include <opencv2/opencv.hpp>
|
||
#include <iostream>
|
||
#include <vector>
|
||
#include <cmath>
|
||
#include <fstream>
|
||
#include <csignal>
|
||
#include <atomic>
|
||
|
||
// Linux 特定: 帧缓冲屏 + mmap + ioctl
|
||
#include <fcntl.h>
|
||
#include <unistd.h>
|
||
#include <sys/mman.h>
|
||
#include <sys/ioctl.h>
|
||
#include <linux/fb.h>
|
||
|
||
// ============================================================
|
||
// 可调参数 — 与 Python 版算法完全对齐
|
||
// ============================================================
|
||
|
||
// --- 逆透视变换 (IPM) ---
|
||
// 是否启用 IPM: 0=关闭(使用前视图), 1=开启(使用鸟瞰图)
|
||
// 启用前必须用标定板重新标定矩阵 M, 否则效果可能比关闭更差!
|
||
#define USE_IPM 1
|
||
|
||
// IPM 输出鸟瞰图尺寸 (与 Python 版 gd.py 一致)
|
||
const int IPM_SIZE = 640;
|
||
|
||
// --- 滑动窗口 ---
|
||
const int WIN_H = 100;
|
||
const int WIN_W = 302;
|
||
const int WIN_STEP = 50;
|
||
const int WIN_COL_L = 39;
|
||
const int WIN_COL_R = 341;
|
||
|
||
// --- 处理分辨率 ---
|
||
const int PROC_SIZE = 400;
|
||
|
||
// --- 梯度过滤 ---
|
||
const float GRAD_THRESHOLD = 30.0f;
|
||
|
||
// --- 方向直方图 ---
|
||
const int HIST_BIN_SIZE = 5;
|
||
const int HIST_ANGLE_MAX = 70;
|
||
const int HIST_BIN_COUNT = 14;
|
||
|
||
// --- 判定阈值 ---
|
||
const int ZEBRA_THRESHOLD = 1500;
|
||
|
||
// --- 摄像头 ---
|
||
const int CAM_WIDTH = 320;
|
||
const int CAM_HEIGHT = 240;
|
||
|
||
// ============================================================
|
||
// IPM 逆透视变换矩阵
|
||
//
|
||
// Python 源码中的变换逻辑:
|
||
// M = 原图 → BEV 的 3×3 透视变换矩阵
|
||
// iM = M.inv() = BEV → 原图的逆矩阵
|
||
//
|
||
// xy = 640×640 BEV 坐标网格 (px, py)
|
||
// ixy = perspectiveTransform(xy, iM) → 每个 BEV 像素在原图中的坐标
|
||
// remap(frame, mapx, mapy) → 采样原图得到 BEV 图像
|
||
//
|
||
// C++ 等价实现:
|
||
// iM = M.inv()
|
||
// warpPerspective(frame, bev, iM, Size(640,640), INTER_LINEAR)
|
||
//
|
||
// warpPerspective 的矩阵含义是 dst→src 映射, 恰好就是 iM
|
||
//
|
||
// 【标定方法】
|
||
// 1. 在小车前方地面放置一个已知尺寸的矩形标定板 (如 A4 纸或棋盘格)
|
||
// 2. 拍摄一张包含整个矩形的前视图
|
||
// 3. 获取矩形的 4 个角点在原图中的像素坐标 pts_src
|
||
// 4. 在 BEV 中定义矩形应有的 4 个角点坐标 pts_dst
|
||
// 5. M = cv::getPerspectiveTransform(pts_src, pts_dst)
|
||
// 6. 将 M 填入下方矩阵中
|
||
//
|
||
// 当前矩阵是 Python 项目的原始标定值，需替换为实车标定值!
|
||
// ============================================================
|
||
|
||
// Python 原始矩阵 M (前视图 → BEV), 来源: gd.py:255-257
|
||
// 元素按 (row, col) = (0,0) (0,1) (0,2)
|
||
// (1,0) (1,1) (1,2)
|
||
// (2,0) (2,1) (2,2)
|
||
static const double g_M_data[3][3] = {
|
||
{-1.86073726e-01, -5.02678929e-01, 4.72322899e+02},
|
||
{-1.39150388e-02, -1.50260445e+00, 1.00507430e+03},
|
||
{-1.77785988e-05, -1.65517173e-03, 1.00000000e+00}
|
||
};
|
||
|
||
/**
|
||
* 计算逆矩阵 (BEV → 前视图), 用于 warpPerspective
|
||
* 在启动时调用一次, 结果全局缓存
|
||
*/
|
||
static cv::Mat build_ipm_inverse_matrix()
|
||
{
|
||
cv::Mat M(3, 3, CV_64F);
|
||
for (int r = 0; r < 3; ++r)
|
||
for (int c = 0; c < 3; ++c)
|
||
M.at<double>(r, c) = g_M_data[r][c];
|
||
|
||
// iM = M.inv(): BEV 坐标 → 原始图像坐标
|
||
// warpPerspective 接受 dst→src 映射, 即 iM
|
||
cv::Mat iM = M.inv();
|
||
return iM;
|
||
}
|
||
|
||
// 全局变量: 在 main() 中计算一次, 之后每帧复用
|
||
static cv::Mat g_iM;
|
||
|
||
// ============================================================
|
||
// 全局状态 — 优雅退出机制
|
||
// ============================================================
|
||
|
||
static std::atomic<bool> g_running{true};
|
||
|
||
static void signal_handler(int)
|
||
{
|
||
g_running.store(false);
|
||
}
|
||
|
||
// ============================================================
|
||
// apply_ipm — 逆透视变换: 前视图 → 鸟瞰图 (BEV)
|
||
//
|
||
// 输入: 原始摄像头帧 (320×240 或任意分辨率)
|
||
// 预计算的逆矩阵 g_iM (BEV→原图)
|
||
// 输出: 640×640 鸟瞰图 (消除透视畸变, 斑马线恢复矩形)
|
||
//
|
||
// 等价于 Python 版:
|
||
// remap(frame, mapx, mapy, INTER_LINEAR) // 预计算映射表
|
||
//
|
||
// 为什么选 640×640 输出?
|
||
// 1. BEV 鸟瞰需要比原图更大的空间来容纳"拉平"后的路面
|
||
// 2. 640 是 2 的幂次方的近似值, 对 resize 和后续操作友好
|
||
// 3. Python 版使用此尺寸, 算法参数基于此调优
|
||
//
|
||
// 如果不开启 IPM, 此函数会被跳过, 直接用前视图进入处理管线
|
||
// ============================================================
|
||
static cv::Mat apply_ipm(const cv::Mat &frame)
|
||
{
|
||
cv::Mat bev;
|
||
cv::warpPerspective(frame, bev, g_iM,
|
||
cv::Size(IPM_SIZE, IPM_SIZE),
|
||
cv::INTER_LINEAR);
|
||
// warpPerspective 使用 INTER_LINEAR 双线性插值,
|
||
// 比 INTER_NEAREST 最近邻更平滑, 避免产生锯齿状伪边缘
|
||
return bev;
|
||
}
|
||
|
||
// ============================================================
|
||
// preprocess — 图像预处理 (蓝色通道 + 滤波 + 形态学)
|
||
//
|
||
// 输入: BGR 彩色图 (400×400)
|
||
// 输出: 预处理后的单通道图 (实际是蓝色通道处理结果)
|
||
//
|
||
// 五步处理:
|
||
// [1] extractChannel(bgr, blue, 0)
|
||
// 取出 B 通道 — 关键设计!
|
||
// 黄色减速带在 B 通道中亮度极低 (黄色≈R+G, 无B)
|
||
// 白色斑马线在 B 通道中亮度正常 (白色≈R=G=B)
|
||
// → B 通道天然过滤减速带, 保留斑马线
|
||
//
|
||
// [2] medianBlur(blue, 5)
|
||
// 中值滤波核大小=5
|
||
// 替代像素值为邻域中值 → 椒盐噪声/砖缝等独立噪点直接抹除
|
||
// 与均值滤波不同: 中值保持边缘处的实际梯度值不衰减
|
||
//
|
||
// [3] morphologyEx(MORPH_OPEN, 3×3, iter=4)
|
||
// 开运算 = Erode ×4 → Dilate ×4
|
||
// 3×3 矩形核较小, 但 4 次迭代累积效果强:
|
||
// - 窄车道线 (宽度 < 8px) → 在连续腐蚀中被完全消除
|
||
// - 宽斑马线 (宽度 >> 8px) → 腐蚀后仍有残留 → 膨胀恢复
|
||
// 本质: 保留"粗线条", 删除"细线条"
|
||
//
|
||
// [4] morphologyEx(MORPH_CLOSE, 5×5, iter=3)
|
||
// 闭运算 = Dilate ×3 → Erode ×3
|
||
// 5×5 较大核, 3 次迭代:
|
||
// - 先膨胀填平斑马线内部的小缺口/裂缝 (缺损斑马线恢复)
|
||
// - 后腐蚀恢复原始尺寸, 同时消除路面箭头碎片
|
||
// 注意: 如果闭运算过强, 会把相邻的细斑马线粘成一片 → iter=3 是经验平衡点
|
||
// ============================================================
|
||
static cv::Mat preprocess(const cv::Mat &bgr)
|
||
{
|
||
cv::Mat blue;
|
||
|
||
// [1] 提取 B 通道 (蓝底色 + 减弱黄减速带)
|
||
cv::extractChannel(bgr, blue, 0);
|
||
|
||
// [2] 中值滤波 5×5 → 去砖缝纹理/椒盐噪声/虚假小梯度点
|
||
cv::medianBlur(blue, blue, 5);
|
||
|
||
// [3] 开运算: 3×3 矩形核, 4 次迭代 → 去除细窄车道线
|
||
cv::Mat kernel_open = cv::getStructuringElement(cv::MORPH_RECT, cv::Size(3, 3));
|
||
cv::morphologyEx(blue, blue, cv::MORPH_OPEN, kernel_open, cv::Point(-1, -1), 4);
|
||
|
||
// [4] 闭运算: 5×5 矩形核, 3 次迭代 → 填补缺损 + 抑制箭头碎片
|
||
cv::Mat kernel_close = cv::getStructuringElement(cv::MORPH_RECT, cv::Size(5, 5));
|
||
cv::morphologyEx(blue, blue, cv::MORPH_CLOSE, kernel_close, cv::Point(-1, -1), 3);
|
||
|
||
return blue;
|
||
}
|
||
|
||
// ============================================================
|
||
// compute_gradient — Sobel 边缘梯度模值和方向
|
||
//
|
||
// 输入: Canny 边缘检测结果 (单通道, 像素值 0 或 255)
|
||
// 输出: Amplitude — 梯度模值 (浮点, 弱边缘 < 30 置零)
|
||
// theta — 梯度方向 0~90° (浮点)
|
||
//
|
||
// 方向定义:
|
||
// 对每个像素 (r, c) 计算 Sobel 梯度:
|
||
// gx = ∂I/∂c → 水平方向的变化率
|
||
// gy = ∂I/∂r → 垂直方向的变化率
|
||
//
|
||
// Amplitude = sqrt(gx² + gy²) → 边缘强度
|
||
// theta = atan2(|gy|, |gx|) → 边缘方向 (0~90°)
|
||
//
|
||
// 其中 atan2 取 abs 意味着不区分 "从左到右" 和 "从右到左" 的边缘,
|
||
// 也不区分 "白→黑" 和 "黑→白" 的方向, 统一归到 0~90°。
|
||
// 这对斑马线检测是合适的 — 我们只关心边缘的方向一致性, 不关心极性。
|
||
//
|
||
// 方向参考:
|
||
// 0° = 纯水平边缘 (dy→0, dx 大)
|
||
// 45° = 135° 角斜线
|
||
// 90° = 纯垂直边缘 (dx→0, dy 大)
|
||
// 斑马线纵线 ≈ 70~90°, 斜线 ≈ 30~60°
|
||
//
|
||
// 弱边缘过滤:
|
||
// Canny 之后仍有少量噪声梯度 (<30)
|
||
// 直接置零 → 后续直方图统计不参与
|
||
// ============================================================
|
||
static void compute_gradient(const cv::Mat &canny,
|
||
cv::Mat &litude, cv::Mat &theta)
|
||
{
|
||
cv::Mat dx, dy;
|
||
|
||
// Sobel 算子 3×3:
|
||
// dx = [-1 0 1; -2 0 2; -1 0 1] (x方向导数)
|
||
// dy = [-1 -2 -1; 0 0 0; 1 2 1] (y方向导数)
|
||
// CV_32F = 32 位浮点, 保留梯度方向亚度数精度
|
||
cv::Sobel(canny, dx, CV_32F, 1, 0, 3);
|
||
cv::Sobel(canny, dy, CV_32F, 0, 1, 3);
|
||
|
||
// 初始化输出矩阵为全零
|
||
amplitude = cv::Mat::zeros(canny.size(), CV_32F);
|
||
theta = cv::Mat::zeros(canny.size(), CV_32F);
|
||
|
||
// 逐像素计算模值和方向
|
||
// 逐像素循环对于 400×400 = 160K 像素来说不是瓶颈,
|
||
// 瓶颈在 medianBlur 和 morphologyEx 的核运算上
|
||
for (int r = 0; r < canny.rows; ++r)
|
||
{
|
||
for (int c = 0; c < canny.cols; ++c)
|
||
{
|
||
float gx = dx.at<float>(r, c);
|
||
float gy = dy.at<float>(r, c);
|
||
|
||
float amp = std::sqrt(gx * gx + gy * gy);
|
||
float ang = std::atan2(std::abs(gy), std::abs(gx) + 1e-10f)
|
||
* 180.0f / static_cast<float>(CV_PI);
|
||
|
||
// 弱边缘置零 (不参与后续方向统计)
|
||
amplitude.at<float>(r, c) = (amp >= GRAD_THRESHOLD) ? amp : 0.0f;
|
||
theta.at<float>(r, c) = ang;
|
||
}
|
||
}
|
||
}
|
||
|
||
// ============================================================
|
||
// is_zebra_window — 对单个滑动窗口做斑马线分类
|
||
//
|
||
// 输入: 窗口内的梯度模值图 (amp_win) 和方向图 (ang_win)
|
||
// 输出: true = 斑马线, false = 背景
|
||
//
|
||
// 判定逻辑:
|
||
// [1] 扫描窗口内每个像素
|
||
// - 跳过 amp ≤ 0 的像素 (弱边缘/噪声/平滑区域)
|
||
// - 跳过 ang ≥ 70° 的像素 (方向在统计范围外)
|
||
//
|
||
// [2] 统计方向直方图 hist[0..13]
|
||
// hist[k] = 方向在 [k*5, k*5+5) 的有效梯度点数量
|
||
// e.g. hist[0] = 方向 0~4.999° 的点数
|
||
//
|
||
// [3] 取峰值 bin 的点数 = peak
|
||
// peak 反映了窗口内 "某个特定方向上" 有多少条平行的边缘线
|
||
//
|
||
// [4] peak > ZEBRA_THRESHOLD (1500) → 斑马线
|
||
// 斑马线 = 同一方向上大量平行边缘 → peak 极高
|
||
// 普通路面 = 边缘方向杂乱 → 峰值被分散到不同 bin → 各 bin 点数低
|
||
//
|
||
// 在 IPM 开启时, 鸟瞰图将斑马线恢复为垂直矩形
|
||
// → 所有边缘方向集中在 90° 附近 → hist[13] 极高 → 检测更灵敏
|
||
// ============================================================
|
||
static bool is_zebra_window(const cv::Mat &_win, const cv::Mat &ang_win)
|
||
{
|
||
int hist[HIST_BIN_COUNT] = {0};
|
||
|
||
for (int r = 0; r < amp_win.rows; ++r)
|
||
{
|
||
for (int c = 0; c < amp_win.cols; ++c)
|
||
{
|
||
float amp = amp_win.at<float>(r, c);
|
||
if (amp <= 0.0f)
|
||
continue;
|
||
|
||
float ang = ang_win.at<float>(r, c);
|
||
if (ang < 0.0f || ang >= HIST_ANGLE_MAX)
|
||
continue;
|
||
|
||
int bin = static_cast<int>(ang) / HIST_BIN_SIZE;
|
||
if (bin >= 0 && bin < HIST_BIN_COUNT)
|
||
hist[bin]++;
|
||
}
|
||
}
|
||
|
||
int peak = 0;
|
||
for (int i = 0; i < HIST_BIN_COUNT; ++i)
|
||
if (hist[i] > peak)
|
||
peak = hist[i];
|
||
|
||
return peak > ZEBRA_THRESHOLD;
|
||
}
|
||
|
||
// ============================================================
|
||
// 帧缓冲显示 — 将 OpenCV BGR Mat 转为 RGB565 格式直写 /dev/fb0
|
||
//
|
||
// 硬件: SPI LCD 通过 /dev/fb0 (RGB565 格式)
|
||
//
|
||
// RGB565 编码:
|
||
// 16 位: [R4 R3 R2 R1 R0] [G5 G4 G3] [G2 G1 G0] [B4 B3 B2 B1 B0]
|
||
// R 取 高 5 位 (>>3, &0xF8)
|
||
// G 取 高 6 位 (>>2, &0xFC)
|
||
// B 取 高 5 位 (>>3)
|
||
// ============================================================
|
||
|
||
static uint16_t convertRGBToRGB565(uint8_t r, uint8_t g, uint8_t b)
|
||
{
|
||
return ((r & 0xF8) << 8) |
|
||
((g & 0xFC) << 3) |
|
||
(b >> 3);
|
||
}
|
||
|
||
static void display_on_fb(const cv::Mat &bgr, int /* fb_fd */,
|
||
uint16_t *fb_buf, int scr_w, int scr_h)
|
||
{
|
||
cv::Mat display;
|
||
cv::resize(bgr, display, cv::Size(scr_w, scr_h));
|
||
|
||
for (int y = 0; y < scr_h; ++y)
|
||
{
|
||
for (int x = 0; x < scr_w; ++x)
|
||
{
|
||
cv::Vec3b pixel = display.at<cv::Vec3b>(y, x);
|
||
fb_buf[y * scr_w + x] = convertRGBToRGB565(
|
||
pixel[2], pixel[1], pixel[0]);
|
||
}
|
||
}
|
||
}
|
||
|
||
// ============================================================
|
||
// main — 初始化 + 主检测循环 + 清理
|
||
// ============================================================
|
||
int main(int argc, char *argv[])
|
||
{
|
||
// ---- 命令行参数解析 ----
|
||
bool no_display = false;
|
||
bool use_ipm = (USE_IPM != 0); // 默认跟随编译宏
|
||
|
||
for (int i = 1; i < argc; ++i)
|
||
{
|
||
if (std::strcmp(argv[i], "--no-display") == 0)
|
||
no_display = true;
|
||
else if (std::strcmp(argv[i], "--no-ipm") == 0)
|
||
use_ipm = false;
|
||
}
|
||
|
||
// ---- 注册信号处理 ----
|
||
std::signal(SIGINT, signal_handler);
|
||
std::signal(SIGTERM, signal_handler);
|
||
|
||
// ---- 预计算 IPM 逆矩阵 (启用时) ----
|
||
if (use_ipm)
|
||
{
|
||
g_iM = build_ipm_inverse_matrix();
|
||
printf("IPM 逆透视变换: 已启用 (%dx%d BEV)\n", IPM_SIZE, IPM_SIZE);
|
||
printf(" 注意: 矩阵未针对实车标定, 效果可能不佳\n");
|
||
}
|
||
else
|
||
{
|
||
printf("IPM 逆透视变换: 已关闭 (使用前视图)\n");
|
||
}
|
||
|
||
// ========================================================
|
||
// 第 1 步: 打开 USB 摄像头
|
||
// ========================================================
|
||
cv::VideoCapture cap;
|
||
|
||
cap.open(0, cv::CAP_V4L2);
|
||
if (!cap.isOpened())
|
||
{
|
||
cap.open(0);
|
||
if (!cap.isOpened())
|
||
{
|
||
std::cerr << "无法打开摄像头 /dev/video0" << std::endl;
|
||
return 1;
|
||
}
|
||
}
|
||
|
||
cap.set(cv::CAP_PROP_FRAME_WIDTH, CAM_WIDTH);
|
||
cap.set(cv::CAP_PROP_FRAME_HEIGHT, CAM_HEIGHT);
|
||
cap.set(cv::CAP_PROP_FOURCC,
|
||
cv::VideoWriter::fourcc('M', 'J', 'P', 'G'));
|
||
|
||
printf("摄像头已打开 %dx%d\n",
|
||
(int)cap.get(cv::CAP_PROP_FRAME_WIDTH),
|
||
(int)cap.get(cv::CAP_PROP_FRAME_HEIGHT));
|
||
|
||
// ========================================================
|
||
// 第 2 步: 打开帧缓冲 /dev/fb0 → mmap 映射到用户空间
|
||
// ========================================================
|
||
int fb_fd = -1;
|
||
uint16_t *fb_buf = nullptr;
|
||
int scr_w = 0, scr_h = 0;
|
||
|
||
if (!no_display)
|
||
{
|
||
fb_fd = open("/dev/fb0", O_RDWR);
|
||
if (fb_fd < 0)
|
||
{
|
||
std::cerr << "警告: 无法打开 /dev/fb0，切换为无头模式" << std::endl;
|
||
no_display = true;
|
||
}
|
||
else
|
||
{
|
||
struct fb_var_screeninfo vinfo;
|
||
if (ioctl(fb_fd, FBIOGET_VSCREENINFO, &vinfo) < 0)
|
||
{
|
||
std::cerr << "无法获取屏幕信息" << std::endl;
|
||
close(fb_fd);
|
||
no_display = true;
|
||
}
|
||
else
|
||
{
|
||
scr_w = vinfo.xres;
|
||
scr_h = vinfo.yres;
|
||
size_t fb_size = vinfo.yres_virtual * vinfo.xres_virtual
|
||
* vinfo.bits_per_pixel / 8;
|
||
|
||
fb_buf = (uint16_t *)mmap(nullptr, fb_size,
|
||
PROT_READ | PROT_WRITE,
|
||
MAP_SHARED, fb_fd, 0);
|
||
if (fb_buf == MAP_FAILED)
|
||
{
|
||
std::cerr << "无法映射帧缓冲区" << std::endl;
|
||
close(fb_fd);
|
||
no_display = true;
|
||
}
|
||
else
|
||
{
|
||
printf("屏幕分辨率: %dx%d\n", scr_w, scr_h);
|
||
}
|
||
}
|
||
}
|
||
}
|
||
|
||
// ========================================================
|
||
// 第 3 步: 主循环
|
||
// ========================================================
|
||
cv::Mat frame, bev, proc, gray, canny, amplitude, theta, result;
|
||
int frame_count = 0;
|
||
int zebra_frames = 0;
|
||
bool was_zebra = false;
|
||
|
||
printf("\n开始斑马线检测 (按 Ctrl+C 退出)...\n");
|
||
|
||
while (g_running.load())
|
||
{
|
||
// ---- 3a. 摄像头采集一帧 ----
|
||
if (!cap.read(frame) || frame.empty())
|
||
{
|
||
usleep(5000);
|
||
continue;
|
||
}
|
||
|
||
// ---- 3b. [IPM] 逆透视变换: 前视图 → 鸟瞰图 ----
|
||
// 将倾斜透视拉平为俯瞰视角, 让斑马线恢复矩形平行特征
|
||
// 未启用 IPM 时直接使用原始帧
|
||
if (use_ipm)
|
||
{
|
||
bev = apply_ipm(frame);
|
||
}
|
||
else
|
||
{
|
||
bev = frame; // 使用前视图
|
||
}
|
||
|
||
// ---- 3c. 尺寸归一化: BEV/原图 → 400×400 ----
|
||
cv::resize(bev, proc, cv::Size(PROC_SIZE, PROC_SIZE));
|
||
|
||
// ---- 3d. 预处理: 蓝色通道 + 中值滤波 + 形态学 ----
|
||
gray = preprocess(proc);
|
||
|
||
// ---- 3e. Canny 边缘检测 (双阈值 30/90) ----
|
||
cv::Canny(gray, canny, 30, 90, 3);
|
||
|
||
// ---- 3f. 梯度模值与方向 ----
|
||
compute_gradient(canny, amplitude, theta);
|
||
|
||
// ---- 3g. 滑动窗口逐窗检测 ----
|
||
int img_h = proc.rows;
|
||
bool detected = false;
|
||
int z_ymin = img_h;
|
||
int z_ymax = 0;
|
||
|
||
for (int top = 0; top <= img_h - WIN_H; top += WIN_STEP)
|
||
{
|
||
cv::Rect roi(WIN_COL_L, top, WIN_W, WIN_H);
|
||
|
||
if (is_zebra_window(amplitude(roi), theta(roi)))
|
||
{
|
||
detected = true;
|
||
if (top < z_ymin) z_ymin = top;
|
||
if (top + WIN_H > z_ymax) z_ymax = top + WIN_H;
|
||
}
|
||
}
|
||
|
||
// ---- 3h. 写入状态文件 (仅在状态变化时写) ----
|
||
if (detected != was_zebra)
|
||
{
|
||
std::ofstream ofs("./zebra_detected", std::ios::trunc);
|
||
ofs << (detected ? "1" : "0");
|
||
was_zebra = detected;
|
||
}
|
||
|
||
// ---- 3i. 绘制调试叠加图 ----
|
||
result = proc.clone();
|
||
|
||
for (int top = 0; top <= img_h - WIN_H; top += WIN_STEP)
|
||
{
|
||
cv::line(result, cv::Point(WIN_COL_L, top),
|
||
cv::Point(WIN_COL_R, top),
|
||
cv::Scalar(128, 128, 128), 1);
|
||
}
|
||
|
||
if (detected)
|
||
{
|
||
cv::rectangle(result,
|
||
cv::Rect(WIN_COL_L, z_ymin, WIN_W, z_ymax - z_ymin),
|
||
cv::Scalar(255, 0, 255), 3);
|
||
}
|
||
|
||
if (detected)
|
||
{
|
||
cv::putText(result, "ZEBRA CROSSING!",
|
||
cv::Point(10, 30),
|
||
cv::FONT_HERSHEY_SIMPLEX, 0.8,
|
||
cv::Scalar(0, 0, 255), 2);
|
||
}
|
||
|
||
// ---- 3j. LCD 显示 ----
|
||
if (!no_display && fb_buf)
|
||
display_on_fb(result, fb_fd, fb_buf, scr_w, scr_h);
|
||
|
||
// ---- 3k. 统计 ----
|
||
frame_count++;
|
||
if (detected) zebra_frames++;
|
||
|
||
if (frame_count % 30 == 0)
|
||
{
|
||
printf("帧 %d | 斑马线: %s | 累计命中: %d/%d | IPM: %s\n",
|
||
frame_count,
|
||
detected ? "YES" : "NO ",
|
||
zebra_frames, frame_count,
|
||
use_ipm ? "ON" : "OFF");
|
||
}
|
||
}
|
||
|
||
// ========================================================
|
||
// 第 4 步: 清理
|
||
// ========================================================
|
||
cap.release();
|
||
|
||
if (!no_display && fb_buf && fb_buf != MAP_FAILED)
|
||
{
|
||
struct fb_var_screeninfo vinfo;
|
||
if (ioctl(fb_fd, FBIOGET_VSCREENINFO, &vinfo) == 0)
|
||
{
|
||
size_t fb_size = vinfo.yres_virtual * vinfo.xres_virtual
|
||
* vinfo.bits_per_pixel / 8;
|
||
munmap(fb_buf, fb_size);
|
||
}
|
||
close(fb_fd);
|
||
}
|
||
|
||
printf("\n退出。共处理 %d 帧，命中 %d 帧 (%.1f%%), IPM: %s\n",
|
||
frame_count, zebra_frames,
|
||
frame_count > 0 ? (100.0 * zebra_frames / frame_count) : 0.0,
|
||
use_ipm ? "ON" : "OFF");
|
||
|
||
return 0;
|
||
}
|