v10模型集成+去抖+架构重构: 单线程+背景采集, ControlUpdate单出口电机控制, I2C音频持久fd, 斑马线接近去抖
This commit is contained in:
spdis
Vendored
+4
@@ -0,0 +1,4 @@
|
||||
{
|
||||
"python-envs.defaultEnvManager": "ms-python.python:conda",
|
||||
"python-envs.defaultPackageManager": "ms-python.python:conda"
|
||||
}
|
||||
+7
-3
@@ -6,20 +6,21 @@ cmake_minimum_required(VERSION 3.5.0)
|
||||
|
||||
# 设置 C++ 标准
|
||||
set(CMAKE_CXX_STANDARD 17)
|
||||
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -O3 -pthread -Wall") # 对于 C++ 编译器
|
||||
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -O3 -pthread -Wall -march=loongarch64 -mtune=loongarch64 -ffast-math -funroll-loops -fomit-frame-pointer") # 对于 C++ 编译器
|
||||
set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -O3 -Wall") # 对于 C 编译器
|
||||
|
||||
# 定义项目名称和版本，并指定使用C和C++语言
|
||||
project(smartcar_demo2 VERSION 0.1.0 LANGUAGES C CXX)
|
||||
|
||||
# 设置OpenCV的安装路径
|
||||
set(OpenCV_DIR /home/ilikara/loongson/opencv-4.11.0/loongson)
|
||||
set(OpenCV_DIR /mnt/d/PPPProgram/smartcar/opencv_device/lib/cmake/opencv4)
|
||||
|
||||
# 查找OpenCV库，确保安装了所需的依赖
|
||||
find_package(OpenCV REQUIRED)
|
||||
|
||||
# 包含OpenCV的头文件路径
|
||||
include_directories(${OpenCV_INCLUDE_DIRS})
|
||||
include_directories(src)
|
||||
message(STATUS "OpenCV Include Directories: ${OpenCV_INCLUDE_DIRS}")
|
||||
|
||||
# 包含项目的自定义库路径
|
||||
@@ -43,4 +44,7 @@ add_subdirectory(jy62_demo)
|
||||
add_subdirectory(key_demo)
|
||||
add_subdirectory(wonderEcho_demo)
|
||||
add_subdirectory(udp_receive)
|
||||
add_subdirectory(image_test)
|
||||
add_subdirectory(image_test)
|
||||
# add_subdirectory(zebra_demo)
|
||||
add_subdirectory(gd13_demo)
|
||||
add_subdirectory(screenshot_demo)
|
||||
@@ -0,0 +1,18 @@
|
||||
#!/bin/bash
|
||||
export PATH=/opt/loongson-gnu-toolchain-8.3-x86_64-loongarch64-linux-gnu-rc1.6/bin:/usr/bin:/bin
|
||||
CC=loongarch64-linux-gnu-g++
|
||||
SRC_FW=/mnt/d/PPPProgram/smartcar/smartcar_framework
|
||||
SRC_CAR=/mnt/d/PPPProgram/smartcar/smartcar2
|
||||
INC="-I$SRC_FW/model/v10/release -I$SRC_CAR/src -I/mnt/d/PPPProgram/smartcar/opencv_device/include/opencv4"
|
||||
LIB="-L/mnt/d/PPPProgram/smartcar/opencv_device/lib -lopencv_core -lopencv_imgproc -lopencv_imgcodecs -lpthread -lrt"
|
||||
FLAGS="-O3 -std=c++17 -march=loongarch64 -mtune=loongarch64 -ffast-math -funroll-loops -fomit-frame-pointer"
|
||||
|
||||
echo "=== NOSIMD ==="
|
||||
$CC $FLAGS $INC $SRC_FW/tools/v10_demo.cpp $SRC_FW/model/v10/release/model_v10.cpp -o /tmp/vd_nosimd $LIB
|
||||
echo "rc=$?"
|
||||
|
||||
echo "=== LSX ==="
|
||||
$CC $FLAGS -mlsx $INC $SRC_FW/tools/v10_demo.cpp $SRC_FW/model/v10/release/model_v10.cpp -o /tmp/vd_simd $LIB
|
||||
echo "rc=$?"
|
||||
|
||||
ls -la /tmp/vd_nosimd /tmp/vd_simd 2>/dev/null || echo "some binaries missing"
|
||||
@@ -55,14 +55,14 @@ do_start() {
|
||||
echo "[demo] 启动..."
|
||||
echo 1 > "$DIR/start"
|
||||
cd "$DIR"
|
||||
LD_PRELOAD="$DIR/gpio_fix_final.so" nohup ./smartcar_demo2 > "$DIR/out.log" 2>&1 &
|
||||
LD_LIBRARY_PATH=/home/root/opencv/lib nohup ./smartcar_demo1 > "$DIR/out.log" 2>&1 &
|
||||
echo "[demo] PID=$!"
|
||||
}
|
||||
|
||||
do_stop() {
|
||||
echo "[demo] 停止..."
|
||||
echo 0 > "$DIR/start" 2>/dev/null
|
||||
killall -9 smartcar_demo2 2>/dev/null
|
||||
killall -9 smartcar_demo1 2>/dev/null
|
||||
sleep 0.3
|
||||
echo 0 > /sys/class/pwm/pwmchip${PWMCHIP}/pwm1/duty_cycle 2>/dev/null
|
||||
echo 0 > /sys/class/pwm/pwmchip${PWMCHIP}/pwm2/duty_cycle 2>/dev/null
|
||||
|
||||
@@ -0,0 +1,2 @@
|
||||
add_executable(gd13_demo gd13_demo.cpp)
|
||||
target_link_libraries(gd13_demo common_lib ${OpenCV_LIBS})
|
||||
@@ -0,0 +1,103 @@
|
||||
/*
|
||||
* gd13_demo — 斑马线检测演示 (调用 zebra_detect.h)
|
||||
*/
|
||||
#include <opencv2/opencv.hpp>
|
||||
#include <iostream>
|
||||
#include <cstring>
|
||||
#include <csignal>
|
||||
#include <atomic>
|
||||
|
||||
#include <fcntl.h>
|
||||
#include <unistd.h>
|
||||
#include <sys/mman.h>
|
||||
#include <sys/ioctl.h>
|
||||
#include <linux/fb.h>
|
||||
|
||||
#include "zebra_detect.h"
|
||||
|
||||
using namespace cv;
|
||||
using namespace std;
|
||||
|
||||
static atomic<bool> g_running{true};
|
||||
static void signal_handler(int) { g_running.store(false); }
|
||||
|
||||
static uint16_t rgb565(uint8_t r, uint8_t g, uint8_t b)
|
||||
{
|
||||
return ((r & 0xF8) << 8) | ((g & 0xFC) << 3) | (b >> 3);
|
||||
}
|
||||
|
||||
static void display_on_fb(const Mat &bgr, uint16_t *fb_buf, int scr_w, int scr_h)
|
||||
{
|
||||
Mat display;
|
||||
resize(bgr, display, Size(scr_w, scr_h));
|
||||
for (int y = 0; y < scr_h; ++y)
|
||||
for (int x = 0; x < scr_w; ++x)
|
||||
{
|
||||
Vec3b px = display.at<Vec3b>(y, x);
|
||||
fb_buf[y * scr_w + x] = rgb565(px[2], px[1], px[0]);
|
||||
}
|
||||
}
|
||||
|
||||
int main(int argc, char *argv[])
|
||||
{
|
||||
bool no_display = false;
|
||||
for (int i = 1; i < argc; ++i)
|
||||
if (strcmp(argv[i], "--no-display") == 0) no_display = true;
|
||||
|
||||
signal(SIGINT, signal_handler);
|
||||
signal(SIGTERM, signal_handler);
|
||||
|
||||
VideoCapture cap(0);
|
||||
cap.open(0, CAP_V4L2);
|
||||
if (!cap.isOpened()) cap.open(0);
|
||||
if (!cap.isOpened()) { cerr << "摄像头打开失败" << endl; return 1; }
|
||||
printf("摄像头已打开\n");
|
||||
|
||||
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)
|
||||
{
|
||||
struct fb_var_screeninfo vinfo;
|
||||
ioctl(fb_fd, FBIOGET_VSCREENINFO, &vinfo);
|
||||
scr_w = vinfo.xres; scr_h = vinfo.yres;
|
||||
size_t fb_size = vinfo.yres_virtual * vinfo.xres_virtual * 2;
|
||||
fb_buf = (uint16_t *)mmap(nullptr, fb_size, PROT_READ|PROT_WRITE,
|
||||
MAP_SHARED, fb_fd, 0);
|
||||
}
|
||||
}
|
||||
|
||||
Mat frame, result;
|
||||
printf("开始斑马线检测...\n");
|
||||
|
||||
while (g_running)
|
||||
{
|
||||
if (!cap.read(frame) || frame.empty()) { usleep(5000); continue; }
|
||||
|
||||
ZebraResult zr = detect_zebra_crossing(frame);
|
||||
|
||||
result = frame.clone();
|
||||
if (zr.detected)
|
||||
{
|
||||
putText(result, "ZEBRA! dist=" + to_string(zr.distance_px) + "px",
|
||||
Point(10, 30), FONT_HERSHEY_SIMPLEX, 0.8,
|
||||
Scalar(0, 0, 255), 2);
|
||||
printf("ZEBRA detected | distance=%d px\n", zr.distance_px);
|
||||
}
|
||||
else
|
||||
{
|
||||
printf("not detected | distance=%d\n", zr.distance_px);
|
||||
}
|
||||
|
||||
if (!no_display && fb_buf)
|
||||
display_on_fb(result, fb_buf, scr_w, scr_h);
|
||||
}
|
||||
|
||||
cap.release();
|
||||
if (fb_buf) munmap(fb_buf, scr_w * scr_h * 2);
|
||||
if (fb_fd >= 0) close(fb_fd);
|
||||
return 0;
|
||||
}
|
||||
+4
-12
@@ -1,11 +1,3 @@
|
||||
/*
|
||||
* @Author: ilikara 3435193369@qq.com
|
||||
* @Date: 2024-10-10 08:28:56
|
||||
* @LastEditors: ilikara 3435193369@qq.com
|
||||
* @LastEditTime: 2025-01-07 09:35:04
|
||||
* @FilePath: /smartcar/lib/camera.h
|
||||
* @Description: 这是默认设置,请设置`customMade`, 打开koroFileHeader查看配置 进行设置: https://github.com/OBKoro1/koro1FileHeader/wiki/%E9%85%8D%E7%BD%AE
|
||||
*/
|
||||
#ifndef CAMERA_H_
|
||||
#define CAMERA_H_
|
||||
|
||||
@@ -22,6 +14,7 @@
|
||||
#include <linux/fb.h>
|
||||
#include <algorithm>
|
||||
#include <chrono>
|
||||
#include <thread>
|
||||
|
||||
#include "image_cv.h"
|
||||
#include "PIDController.h"
|
||||
@@ -29,16 +22,15 @@
|
||||
#include "global.h"
|
||||
#include "frame_buffer.h"
|
||||
#include "serial.h"
|
||||
#include "control.h"
|
||||
|
||||
int CameraInit(uint8_t camera_id, double dest_fps, int width, int height);
|
||||
int CameraHandler(void);
|
||||
void streamCapture(void);
|
||||
void cameraDeInit(void);
|
||||
|
||||
extern bool streamCaptureRunning;
|
||||
|
||||
extern double kp;
|
||||
extern double ki;
|
||||
extern double kd;
|
||||
extern double g_steer_deviation;
|
||||
|
||||
#endif
|
||||
#endif
|
||||
|
||||
+5
-2
@@ -8,11 +8,14 @@
|
||||
#include "serial.h"
|
||||
|
||||
void ControlInit();
|
||||
void ControlMain();
|
||||
void ControlUpdate(double speed, bool zebra_block);
|
||||
void ControlExit();
|
||||
|
||||
extern double mortor_kp;
|
||||
extern double mortor_ki;
|
||||
extern double mortor_kd;
|
||||
|
||||
#endif
|
||||
extern GPIO mortorEN;
|
||||
extern MotorController *motorController[2];
|
||||
|
||||
#endif
|
||||
|
||||
@@ -18,6 +18,7 @@ const std::string start_file = "./start";
|
||||
const std::string showImg_file = "./showImg";
|
||||
const std::string destfps_file = "./destfps";
|
||||
const std::string foresee_file = "./foresee";
|
||||
const std::string zebrasee_file = "./zebrasee";
|
||||
const std::string saveImg_file = "./saveImg";
|
||||
const std::string speed_file = "./speed";
|
||||
const std::string deadband_file = "./deadband";
|
||||
|
||||
@@ -0,0 +1,16 @@
|
||||
/*
|
||||
* zebra_detect.h — 斑马线检测函数
|
||||
* ================================
|
||||
* 输入图片 → 返回是否有人行横道 + 距离
|
||||
*/
|
||||
#pragma once
|
||||
|
||||
#include <opencv2/opencv.hpp>
|
||||
|
||||
struct ZebraResult
|
||||
{
|
||||
bool detected; // true = 检测到斑马线
|
||||
int distance_px; // 斑马线下沿距图像下边框的像素距离, -1 = 未检测到
|
||||
};
|
||||
|
||||
ZebraResult detect_zebra_crossing(const cv::Mat &bgr_frame);
|
||||
+2
-2
@@ -1,4 +1,4 @@
|
||||
# 主程序
|
||||
add_executable(smartcar_demo2 main.cpp)
|
||||
add_executable(smartcar_demo1 main.cpp)
|
||||
|
||||
target_link_libraries(smartcar_demo2 common_lib ${OpenCV_LIBS})
|
||||
target_link_libraries(smartcar_demo1 common_lib ${OpenCV_LIBS})
|
||||
+35
-124
@@ -4,14 +4,12 @@
|
||||
#include <string>
|
||||
#include <csignal>
|
||||
#include <atomic>
|
||||
#include <chrono>
|
||||
#include <thread>
|
||||
|
||||
#include "global.h"
|
||||
#include "camera.h"
|
||||
#include "control.h"
|
||||
#include "Timer.h"
|
||||
#include "serial.h"
|
||||
#include "encoder.h"
|
||||
#include "video.h"
|
||||
|
||||
std::atomic<bool> running(true);
|
||||
void signalHandler(int signal)
|
||||
@@ -21,129 +19,42 @@ void signalHandler(int signal)
|
||||
|
||||
int main(void)
|
||||
{
|
||||
// Video video("test.mp4", 30);
|
||||
|
||||
// // 获取帧缓冲区设备信息
|
||||
// int fb = open("/dev/fb0", O_RDWR);
|
||||
// if (fb == -1)
|
||||
// {
|
||||
// std::cerr << "无法打开帧缓冲区设备" << std::endl;
|
||||
// return -1;
|
||||
// }
|
||||
// struct fb_var_screeninfo vinfo;
|
||||
// ioctl(fb, FBIOGET_VSCREENINFO, &vinfo);
|
||||
|
||||
// // 设置屏幕参数
|
||||
// int screenWidth = 160;
|
||||
// int screenHeight = 128;
|
||||
|
||||
// int newWidth, newHeight;
|
||||
|
||||
// // 创建帧缓冲区
|
||||
// uint16_t *fb_buffer = new uint16_t[screenWidth * screenHeight];
|
||||
|
||||
// int frameCount = 0;
|
||||
|
||||
// cv::Mat frame, resizedFrame, fbImage(screenHeight, screenWidth, CV_8UC3, cv::Scalar(0, 0, 0)); // 帧缓冲区图像
|
||||
|
||||
// int frame_count = 0;
|
||||
|
||||
// video.timer.start();
|
||||
|
||||
// while(1) {
|
||||
// video.frameMutex.lock();
|
||||
// if(newWidth == 0)
|
||||
// {
|
||||
// // 保持视频长宽比
|
||||
// int videoWidth = video.frame.cols;
|
||||
// int videoHeight = video.frame.rows;
|
||||
// double aspectRatio = static_cast<double>(videoWidth) / videoHeight;
|
||||
// // 根据屏幕大小计算缩放后的宽度和高度
|
||||
// if (screenWidth / static_cast<double>(screenHeight) > aspectRatio)
|
||||
// {
|
||||
// // 屏幕更宽，以高度为基准
|
||||
// newHeight = screenHeight;
|
||||
// newWidth = static_cast<int>(newHeight * aspectRatio);
|
||||
// }
|
||||
// else
|
||||
// {
|
||||
// // 屏幕更高，以宽度为基准
|
||||
// newWidth = screenWidth;
|
||||
// newHeight = static_cast<int>(newWidth / aspectRatio);
|
||||
// }
|
||||
// }
|
||||
|
||||
// // 缩放视频到新尺寸
|
||||
// cv::resize(video.frame, resizedFrame, cv::Size(newWidth, newHeight));
|
||||
// video.frameMutex.unlock();
|
||||
|
||||
// // 将缩放后的图像居中放置在帧缓冲区图像中，填充黑色边框
|
||||
// fbImage.setTo(cv::Scalar(0, 0, 0)); // 清空缓冲区（填充黑色）
|
||||
// cv::Rect roi((screenWidth - newWidth) / 2, (screenHeight - newHeight) / 2, newWidth, newHeight);
|
||||
// resizedFrame.copyTo(fbImage(roi));
|
||||
|
||||
// // 将帧缓冲区图像转换为RGB565格式
|
||||
// convertMatToRGB565(fbImage, fb_buffer, screenWidth, screenHeight);
|
||||
|
||||
// // 写入帧缓冲区
|
||||
// lseek(fb, 0, SEEK_SET);
|
||||
// write(fb, fb_buffer, screenWidth * screenHeight * 2);
|
||||
// std::this_thread::sleep_for(std::chrono::milliseconds(30));
|
||||
// }
|
||||
std::signal(SIGINT, signalHandler);
|
||||
|
||||
double dest_fps = readDoubleFromFile(destfps_file);
|
||||
int dest_frame_duration = CameraInit(0, dest_fps, 320, 240);
|
||||
printf("%d\n", dest_frame_duration);
|
||||
if (dest_frame_duration != -1)
|
||||
{
|
||||
streamCaptureRunning = true;
|
||||
std::thread camworker = std::thread(streamCapture);
|
||||
std::cout << "Stream Capture Service started!\n";
|
||||
ControlInit();
|
||||
std::cout << "Control Initialized!\n";
|
||||
if (dest_fps <= 0) dest_fps = 30.0;
|
||||
|
||||
Timer CameraTimer(dest_frame_duration, std::bind(CameraHandler));
|
||||
Timer MortorTimer(8, std::bind(ControlMain));
|
||||
CameraTimer.start();
|
||||
std::cout << "Camera Service started!\n";
|
||||
MortorTimer.start();
|
||||
std::cout << "Control Service started!\n";
|
||||
|
||||
// 主循环，直到用户输入 Ctrl+C
|
||||
while (running.load())
|
||||
{
|
||||
std::this_thread::sleep_for(std::chrono::milliseconds(500));
|
||||
target_speed = readDoubleFromFile(speed_file);
|
||||
|
||||
mortor_kp = readDoubleFromFile(mortor_kp_file);
|
||||
mortor_ki = readDoubleFromFile(mortor_ki_file);
|
||||
mortor_kd = readDoubleFromFile(mortor_kd_file);
|
||||
|
||||
kp = readDoubleFromFile(kp_file);
|
||||
ki = readDoubleFromFile(ki_file);
|
||||
kd = readDoubleFromFile(kd_file);
|
||||
// vofa_image(1, 160*120, 160, 120, Format_Grayscale8, (char*)IMG);
|
||||
}
|
||||
std::cout << "Stopping!\n";
|
||||
std::this_thread::sleep_for(std::chrono::milliseconds(500));
|
||||
|
||||
CameraTimer.stop();
|
||||
std::cout << "Camera Timer stopped!\n";
|
||||
MortorTimer.stop();
|
||||
std::cout << "Control Timer stopped!\n";
|
||||
|
||||
ControlExit();
|
||||
std::cout << "Control Service stopped!\n";
|
||||
|
||||
streamCaptureRunning = false;
|
||||
if (camworker.joinable())
|
||||
{
|
||||
camworker.join();
|
||||
}
|
||||
|
||||
cameraDeInit();
|
||||
std::cout << "Camera Service stopped!\n";
|
||||
if (CameraInit(0, dest_fps, 320, 240) < 0) {
|
||||
std::cerr << "CameraInit failed" << std::endl;
|
||||
return -1;
|
||||
}
|
||||
ControlInit();
|
||||
std::cout << "All services started (single-thread mode)" << std::endl;
|
||||
|
||||
int tick = 0;
|
||||
while (running.load())
|
||||
{
|
||||
if (CameraHandler() < 0) {
|
||||
std::this_thread::sleep_for(std::chrono::milliseconds(5));
|
||||
continue;
|
||||
}
|
||||
|
||||
if (++tick % 15 == 0)
|
||||
{
|
||||
target_speed = readDoubleFromFile(speed_file);
|
||||
mortor_kp = readDoubleFromFile(mortor_kp_file);
|
||||
mortor_ki = readDoubleFromFile(mortor_ki_file);
|
||||
mortor_kd = readDoubleFromFile(mortor_kd_file);
|
||||
kp = readDoubleFromFile(kp_file);
|
||||
ki = readDoubleFromFile(ki_file);
|
||||
kd = readDoubleFromFile(kd_file);
|
||||
}
|
||||
}
|
||||
|
||||
std::cout << "Stopping..." << std::endl;
|
||||
std::this_thread::sleep_for(std::chrono::milliseconds(500));
|
||||
ControlExit();
|
||||
cameraDeInit();
|
||||
std::cout << "Stopped." << std::endl;
|
||||
return 0;
|
||||
}
|
||||
}
|
||||
|
||||
Binary file not shown.
@@ -0,0 +1,2 @@
|
||||
add_executable(screenshot_demo screenshot_demo.cpp)
|
||||
target_link_libraries(screenshot_demo common_lib ${OpenCV_LIBS})
|
||||
@@ -0,0 +1,111 @@
|
||||
/*
|
||||
* screenshot_demo — 摄像头截图保存
|
||||
* ======================================
|
||||
* 每次运行截一张图, 保存到 ./screenshot/image_N.jpg
|
||||
* 用法:
|
||||
* ./screenshot_demo 截一张图 + LCD 显示
|
||||
* ./screenshot_demo --no-display 无 LCD
|
||||
*/
|
||||
|
||||
#include <opencv2/opencv.hpp>
|
||||
#include <iostream>
|
||||
#include <cstdio>
|
||||
#include <cstring>
|
||||
#include <sys/stat.h>
|
||||
#include <fcntl.h>
|
||||
#include <unistd.h>
|
||||
#include <sys/mman.h>
|
||||
#include <sys/ioctl.h>
|
||||
#include <linux/fb.h>
|
||||
|
||||
using namespace cv;
|
||||
using namespace std;
|
||||
|
||||
static uint16_t rgb565(uint8_t r, uint8_t g, uint8_t b)
|
||||
{
|
||||
return ((r & 0xF8) << 8) | ((g & 0xFC) << 3) | (b >> 3);
|
||||
}
|
||||
|
||||
int main(int argc, char *argv[])
|
||||
{
|
||||
bool no_display = false;
|
||||
for (int i = 1; i < argc; ++i)
|
||||
if (strcmp(argv[i], "--no-display") == 0)
|
||||
no_display = true;
|
||||
|
||||
// ---- 创建目录 ----
|
||||
mkdir("./screenshot", 0755);
|
||||
|
||||
// ---- 摄像头 ----
|
||||
VideoCapture cap;
|
||||
cap.open(0, CAP_V4L2);
|
||||
if (!cap.isOpened()) cap.open(0);
|
||||
if (!cap.isOpened()) { cerr << "无法打开摄像头" << endl; return 1; }
|
||||
cap.set(CAP_PROP_FRAME_WIDTH, 640);
|
||||
cap.set(CAP_PROP_FRAME_HEIGHT, 480);
|
||||
cap.set(CAP_PROP_FOURCC, VideoWriter::fourcc('M', 'J', 'P', 'G'));
|
||||
printf("摄像头: %dx%d\n",
|
||||
(int)cap.get(CAP_PROP_FRAME_WIDTH),
|
||||
(int)cap.get(CAP_PROP_FRAME_HEIGHT));
|
||||
|
||||
// ---- 丢弃前几帧 (曝光稳定) ----
|
||||
Mat frame;
|
||||
for (int i = 0; i < 10; ++i)
|
||||
cap.read(frame);
|
||||
usleep(100000);
|
||||
|
||||
// ---- 截图 ----
|
||||
if (!cap.read(frame) || frame.empty())
|
||||
{
|
||||
cerr << "截图失败" << endl;
|
||||
cap.release();
|
||||
return 1;
|
||||
}
|
||||
|
||||
// 自增文件名
|
||||
int idx = 0;
|
||||
char fname[64];
|
||||
struct stat st;
|
||||
do {
|
||||
snprintf(fname, sizeof(fname),
|
||||
"./screenshot/image_%05d.jpg", ++idx);
|
||||
} while (stat(fname, &st) == 0);
|
||||
|
||||
imwrite(fname, frame);
|
||||
printf("截图: %s (%dx%d)\n", fname, frame.cols, frame.rows);
|
||||
|
||||
// ---- LCD 显示 ----
|
||||
if (!no_display)
|
||||
{
|
||||
int fb_fd = open("/dev/fb0", O_RDWR);
|
||||
if (fb_fd >= 0)
|
||||
{
|
||||
struct fb_var_screeninfo vinfo;
|
||||
ioctl(fb_fd, FBIOGET_VSCREENINFO, &vinfo);
|
||||
int scr_w = vinfo.xres, scr_h = vinfo.yres;
|
||||
size_t fb_size = vinfo.yres_virtual * vinfo.xres_virtual * 2;
|
||||
uint16_t *fb_buf = (uint16_t *)mmap(nullptr, fb_size,
|
||||
PROT_READ|PROT_WRITE,
|
||||
MAP_SHARED, fb_fd, 0);
|
||||
|
||||
Mat disp;
|
||||
resize(frame, disp, Size(scr_w, scr_h));
|
||||
rectangle(disp, Point(0, 0), Point(disp.cols, disp.rows),
|
||||
Scalar(0, 255, 255), 3);
|
||||
|
||||
for (int y = 0; y < scr_h; ++y)
|
||||
for (int x = 0; x < scr_w; ++x)
|
||||
{
|
||||
Vec3b px = disp.at<Vec3b>(y, x);
|
||||
fb_buf[y * scr_w + x] = rgb565(px[2], px[1], px[0]);
|
||||
}
|
||||
|
||||
sleep(1);
|
||||
munmap(fb_buf, fb_size);
|
||||
close(fb_fd);
|
||||
}
|
||||
}
|
||||
|
||||
cap.release();
|
||||
return 0;
|
||||
}
|
||||
+267
-175
@@ -1,284 +1,376 @@
|
||||
#include "camera.h"
|
||||
#include "model_v10.hpp"
|
||||
|
||||
#include <fcntl.h>
|
||||
#include <unistd.h>
|
||||
#include <sys/ioctl.h>
|
||||
#include <linux/i2c-dev.h>
|
||||
#include <linux/i2c.h>
|
||||
|
||||
cv::VideoCapture cap;
|
||||
|
||||
double kp = 0;
|
||||
double ki = 0;
|
||||
double kd = 0;
|
||||
|
||||
int screenWidth, screenHeight;
|
||||
int newWidth, newHeight;
|
||||
double kp = 0, ki = 0, kd = 0;
|
||||
int screenWidth, screenHeight, newWidth, newHeight;
|
||||
int fb;
|
||||
// 创建帧缓冲区
|
||||
uint16_t *fb_buffer;
|
||||
PwmController servo(1, 0);
|
||||
|
||||
#define calc_scale 2
|
||||
|
||||
// ── 模型检测参数 ──
|
||||
#define ZEBRA_CLASS 3
|
||||
static float g_thresh[4] = {0.80f, 0.80f, 0.80f, 0.75f};
|
||||
|
||||
// ── 斑马线去抖: 远处→近处接近逻辑, 防反光误触发 ──
|
||||
#define ZEBRA_MIN_FRAMES 5 // 累计检测至少5帧
|
||||
#define ZEBRA_FAR_CY 50 // 必须在cy≤50处出现过(远处)
|
||||
|
||||
static int g_zc_frames = 0; // 当前接近episode中检测帧数
|
||||
static int g_zc_min_cy = 120; // 当前episode中最小cy(最远)
|
||||
|
||||
// ── 斑马线状态机 ──
|
||||
enum ZState { Z_NORMAL, Z_STOP, Z_COOLDOWN };
|
||||
static ZState g_zstate = Z_NORMAL;
|
||||
static time_t g_ztime = 0;
|
||||
static bool g_zebra_ever = false;
|
||||
|
||||
// ── 模型检测结果缓存 ──
|
||||
static DetectBoxV10 g_boxes[16];
|
||||
static int g_box_count = 0;
|
||||
static bool g_lcd_on = true;
|
||||
|
||||
double g_steer_deviation = 0;
|
||||
PIDController ServoControl(1.0, 0.0, 2.0, 0.0, POSITION, 1250000);
|
||||
|
||||
// ── 背景采集线程 (只做 cap.read, 不参与控制) ──
|
||||
static std::mutex frameMutex;
|
||||
static cv::Mat pubframe;
|
||||
static bool captureRunning;
|
||||
static std::thread captureWorker;
|
||||
|
||||
void streamCapture(void)
|
||||
{
|
||||
cv::Mat tmp;
|
||||
while (captureRunning) {
|
||||
cap.read(tmp);
|
||||
frameMutex.lock();
|
||||
pubframe = tmp;
|
||||
frameMutex.unlock();
|
||||
}
|
||||
}
|
||||
|
||||
// ── I2C 音频 (持久打开) ──
|
||||
static int i2c_audio_fd = -1;
|
||||
|
||||
static bool i2c_audio_open()
|
||||
{
|
||||
if (i2c_audio_fd >= 0) return true;
|
||||
i2c_audio_fd = open("/dev/i2c-2", O_RDWR);
|
||||
if (i2c_audio_fd < 0) {
|
||||
fprintf(stderr, "[ZEBRA] 无法打开 I2C-2: %s\n", strerror(errno));
|
||||
return false;
|
||||
}
|
||||
if (ioctl(i2c_audio_fd, I2C_SLAVE, 0x34) < 0) {
|
||||
fprintf(stderr, "[ZEBRA] 无法设置 I2C 从地址 0x34: %s\n", strerror(errno));
|
||||
close(i2c_audio_fd); i2c_audio_fd = -1;
|
||||
return false;
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
int CameraInit(uint8_t camera_id, double dest_fps, int width, int height)
|
||||
{
|
||||
servo.setPeriod(3000000);
|
||||
servo.setDutyCycle(1500000);
|
||||
servo.enable();
|
||||
|
||||
// 打开帧缓冲区设备
|
||||
fb = open("/dev/fb0", O_RDWR);
|
||||
if (fb == -1)
|
||||
{
|
||||
std::cerr << "无法打开帧缓冲区设备" << std::endl;
|
||||
return -1;
|
||||
}
|
||||
if (fb == -1) { std::cerr << "无法打开帧缓冲区设备" << std::endl; return -1; }
|
||||
|
||||
// 获取帧缓冲区设备信息
|
||||
struct fb_var_screeninfo vinfo;
|
||||
if (ioctl(fb, FBIOGET_VSCREENINFO, &vinfo) == -1)
|
||||
{
|
||||
std::cerr << "无法获取帧缓冲区信息" << std::endl;
|
||||
close(fb);
|
||||
return -1;
|
||||
if (ioctl(fb, FBIOGET_VSCREENINFO, &vinfo) == -1) {
|
||||
std::cerr << "无法获取帧缓冲区信息" << std::endl; close(fb); return -1;
|
||||
}
|
||||
|
||||
// 动态设置屏幕分辨率
|
||||
screenWidth = vinfo.xres;
|
||||
screenHeight = vinfo.yres;
|
||||
|
||||
// 计算帧缓冲区大小
|
||||
screenWidth = vinfo.xres; screenHeight = vinfo.yres;
|
||||
size_t fb_size = vinfo.yres_virtual * vinfo.xres_virtual * vinfo.bits_per_pixel / 8;
|
||||
|
||||
// 使用 mmap 映射帧缓冲区到内存
|
||||
fb_buffer = (uint16_t *)mmap(NULL, fb_size, PROT_READ | PROT_WRITE, MAP_SHARED, fb, 0);
|
||||
if (fb_buffer == MAP_FAILED)
|
||||
{
|
||||
std::cerr << "无法映射帧缓冲区到内存" << std::endl;
|
||||
close(fb);
|
||||
return -1;
|
||||
if (fb_buffer == MAP_FAILED) {
|
||||
std::cerr << "无法映射帧缓冲区到内存" << std::endl; close(fb); return -1;
|
||||
}
|
||||
|
||||
// 打开默认摄像头（设备编号 0）
|
||||
cap.open(0, cv::CAP_V4L2);
|
||||
if (!cap.isOpened()) cap.open(0);
|
||||
|
||||
cap.set(cv::CAP_PROP_FRAME_WIDTH, 320);
|
||||
cap.set(cv::CAP_PROP_FRAME_HEIGHT, 240);
|
||||
cap.set(cv::CAP_PROP_FOURCC, cv::VideoWriter::fourcc('M', 'J', 'P', 'G'));
|
||||
|
||||
// 检查摄像头是否成功打开
|
||||
if (!cap.isOpened())
|
||||
{
|
||||
if (!cap.isOpened()) {
|
||||
printf("无法打开摄像头\n");
|
||||
munmap(fb_buffer, fb_size);
|
||||
close(fb);
|
||||
return -1;
|
||||
munmap(fb_buffer, fb_size); close(fb); return -1;
|
||||
}
|
||||
cap.set(cv::CAP_PROP_FRAME_WIDTH, width);
|
||||
cap.set(cv::CAP_PROP_FRAME_HEIGHT, height);
|
||||
cap.set(cv::CAP_PROP_FOURCC, cv::VideoWriter::fourcc('M', 'J', 'P', 'G'));
|
||||
cap.set(cv::CAP_PROP_AUTO_EXPOSURE, -1);
|
||||
|
||||
cap.set(cv::CAP_PROP_FRAME_WIDTH, width); // 宽度
|
||||
cap.set(cv::CAP_PROP_FRAME_HEIGHT, height); // 高度
|
||||
cap.set(cv::CAP_PROP_FOURCC, cv::VideoWriter::fourcc('M', 'J', 'P', 'G')); // 视频流格式
|
||||
cap.set(cv::CAP_PROP_AUTO_EXPOSURE, -1); // 设置自动曝光
|
||||
|
||||
// 获取摄像头实际分辨率
|
||||
int cameraWidth = cap.get(cv::CAP_PROP_FRAME_WIDTH);
|
||||
int cameraHeight = cap.get(cv::CAP_PROP_FRAME_HEIGHT);
|
||||
printf("摄像头分辨率: %d x %d\n", cameraWidth, cameraHeight);
|
||||
|
||||
// 计算 newWidth 和 newHeight，确保图像适应屏幕
|
||||
double widthRatio = static_cast<double>(screenWidth) / cameraWidth;
|
||||
double heightRatio = static_cast<double>(screenHeight) / cameraHeight;
|
||||
double scale = std::min(widthRatio, heightRatio); // 选择较小的比例，确保图像不超出屏幕
|
||||
|
||||
double scale = std::min(widthRatio, heightRatio);
|
||||
newWidth = static_cast<int>(cameraWidth * scale);
|
||||
newHeight = static_cast<int>(cameraHeight * scale);
|
||||
printf("自适应分辨率: %d x %d\n", newWidth, newHeight);
|
||||
|
||||
// 计算帧率
|
||||
double fps = cap.get(cv::CAP_PROP_FPS);
|
||||
printf("Camera fps:%lf\n", fps);
|
||||
|
||||
line_tracking_width = newWidth / calc_scale;
|
||||
line_tracking_height = newHeight / calc_scale;
|
||||
|
||||
// 计算每帧的延迟时间（ms）
|
||||
if (!model_v10_init("./nanodetv10.bin")) {
|
||||
printf("[MODEL] 警告: 模型加载失败\n");
|
||||
} else {
|
||||
printf("[MODEL] v10 模型已加载\n");
|
||||
}
|
||||
|
||||
i2c_audio_open();
|
||||
|
||||
captureRunning = true;
|
||||
captureWorker = std::thread(streamCapture);
|
||||
|
||||
// 等待第一帧就绪
|
||||
for (int i = 0; i < 60 && pubframe.empty(); ++i) {
|
||||
std::this_thread::sleep_for(std::chrono::milliseconds(50));
|
||||
}
|
||||
if (pubframe.empty()) { printf("警告: 摄像头首帧超时\n"); }
|
||||
|
||||
return static_cast<int>(1000.0 / std::min(fps, dest_fps));
|
||||
}
|
||||
|
||||
void cameraDeInit(void)
|
||||
{
|
||||
captureRunning = false;
|
||||
cap.release();
|
||||
|
||||
// 获取帧缓冲区设备信息
|
||||
if (captureWorker.joinable()) captureWorker.join();
|
||||
struct fb_var_screeninfo vinfo;
|
||||
if (ioctl(fb, FBIOGET_VSCREENINFO, &vinfo) == -1)
|
||||
{
|
||||
std::cerr << "无法获取帧缓冲区信息" << std::endl;
|
||||
}
|
||||
else
|
||||
{
|
||||
// 计算帧缓冲区大小
|
||||
if (ioctl(fb, FBIOGET_VSCREENINFO, &vinfo) != -1) {
|
||||
size_t fb_size = vinfo.yres_virtual * vinfo.xres_virtual * vinfo.bits_per_pixel / 8;
|
||||
|
||||
// 取消映射
|
||||
munmap(fb_buffer, fb_size);
|
||||
}
|
||||
|
||||
close(fb);
|
||||
if (i2c_audio_fd >= 0) close(i2c_audio_fd);
|
||||
model_v10_deinit();
|
||||
}
|
||||
|
||||
int saved_frame_count = 0;
|
||||
bool saveCameraImage(cv::Mat frame, const std::string &directory)
|
||||
static bool saveCameraImage(cv::Mat frame, const std::string &directory)
|
||||
{
|
||||
if (frame.empty())
|
||||
{
|
||||
std::cerr << "Save Error: Frame is empty." << std::endl;
|
||||
return 0;
|
||||
}
|
||||
|
||||
// 构建文件名
|
||||
if (frame.empty()) return false;
|
||||
std::ostringstream filename;
|
||||
filename << directory << "/image_" << std::setw(5) << std::setfill('0') << saved_frame_count << ".jpg";
|
||||
|
||||
saved_frame_count++;
|
||||
// 保存图像
|
||||
return cv::imwrite(filename.str(), frame);
|
||||
}
|
||||
|
||||
std::mutex frameMutex;
|
||||
cv::Mat pubframe;
|
||||
bool streamCaptureRunning;
|
||||
void streamCapture(void)
|
||||
static void play_zebra_audio()
|
||||
{
|
||||
cv::Mat frame;
|
||||
while (streamCaptureRunning)
|
||||
{
|
||||
cap.read(frame);
|
||||
frameMutex.lock();
|
||||
pubframe = frame;
|
||||
frameMutex.unlock();
|
||||
if (!i2c_audio_open()) {
|
||||
printf("[ZEBRA] 语音失败: I2C 未打开\n");
|
||||
return;
|
||||
}
|
||||
return;
|
||||
|
||||
ioctl(i2c_audio_fd, I2C_SLAVE, 0x34); // 每次重设从地址
|
||||
|
||||
union i2c_smbus_data d;
|
||||
struct i2c_smbus_ioctl_data a;
|
||||
__u8 v[] = {0xFF, 0x10};
|
||||
d.block[0] = 2; d.block[1] = v[0]; d.block[2] = v[1];
|
||||
a.read_write = I2C_SMBUS_WRITE;
|
||||
a.command = 0x6E;
|
||||
a.size = I2C_SMBUS_I2C_BLOCK_DATA;
|
||||
a.data = &d;
|
||||
|
||||
if (ioctl(i2c_audio_fd, I2C_SMBUS, &a) < 0)
|
||||
printf("[ZEBRA] 语音失败: %s\n", strerror(errno));
|
||||
else
|
||||
printf("[ZEBRA] 语音播报已触发\n");
|
||||
}
|
||||
|
||||
// ===================================================
|
||||
// PIDController ServoControl(P=1.0, I=0, D=2.0, target=0, 位置式, 输出限幅=1,250,000)
|
||||
// 输出单位: 百分之一脉宽周期 (÷100 × period_ns → ns)
|
||||
// 实际等效线性增益: Kp=1.0 起主导, I=0 无积分, D=2.0 微分量抑制过冲
|
||||
// ===================================================
|
||||
double g_steer_deviation = 0; // 全局偏差, 供速度控制用
|
||||
|
||||
PIDController ServoControl(1.0, 0.0, 2.0, 0.0, POSITION, 1250000);
|
||||
int CameraHandler(void)
|
||||
{
|
||||
cv::Mat resizedFrame;
|
||||
|
||||
// ── 1. 取最新帧 (背景线程持续采集, 写全局 raw_frame 供 image_main 使用) ──
|
||||
frameMutex.lock();
|
||||
raw_frame = pubframe;
|
||||
frameMutex.unlock();
|
||||
if (raw_frame.empty()) { return -1; }
|
||||
|
||||
if (raw_frame.empty())
|
||||
{
|
||||
printf("无法捕获图像\n");
|
||||
return -1;
|
||||
}
|
||||
|
||||
if (readFlag(saveImg_file))
|
||||
{
|
||||
// ── 2. 保存图像 ──
|
||||
if (readFlag(saveImg_file)) {
|
||||
if (saveCameraImage(raw_frame, "./image"))
|
||||
{
|
||||
printf("图像%d已保存\n", saved_frame_count);
|
||||
}
|
||||
else
|
||||
{
|
||||
printf("图像保存失败\n");
|
||||
return -1;
|
||||
}
|
||||
|
||||
// ── 3. 视觉巡线 (禁止动) ──
|
||||
image_main();
|
||||
|
||||
// ── 4. 模型推理 (每2帧一次) ──
|
||||
static int infer_skip = 0;
|
||||
if (++infer_skip >= 2) {
|
||||
infer_skip = 0;
|
||||
g_box_count = 0;
|
||||
if (model_v10_ready()) {
|
||||
cv::Mat mInput;
|
||||
cv::resize(raw_frame, mInput, cv::Size(160, 120), 0, 0, cv::INTER_AREA);
|
||||
g_box_count = model_v10_detect(mInput.data, 160, 120, g_boxes, 16, g_thresh);
|
||||
}
|
||||
}
|
||||
|
||||
// ── 1. 视觉巡线 ──────────────────────────────────
|
||||
// image_main() 处理 raw_frame → 80×60 图
|
||||
// 产出: left_line[60], right_line[60], mid_line[60] (EMA 滤波后)
|
||||
{ // 图像计算
|
||||
image_main();
|
||||
}
|
||||
// ── 5. 斑马线去抖+停/走状态机 ──
|
||||
bool zebra_near = false;
|
||||
bool zebra_seen = false;
|
||||
int zebra_cy = 0;
|
||||
float zebra_cf = 0;
|
||||
g_zebra_ever = false;
|
||||
|
||||
// ── 2. 舵机转向控制 ──────────────────────────────
|
||||
// 仅在 start 文件为 1 时执行 (readFlag(start_file))
|
||||
// 否则保持上一次的脉宽 (不做任何转向)
|
||||
if (readFlag(start_file))
|
||||
{
|
||||
// 2a. 前瞻行号换算
|
||||
// foresee 是 newWidth×newHeight (160×120) 坐标系下的行号
|
||||
// calc_scale=2, 除以 2 得到 80×60 (line_tracking) 下的行号
|
||||
int foresee = readDoubleFromFile(foresee_file);
|
||||
int check_row = foresee / calc_scale;
|
||||
|
||||
// 2b. 单行偏差计算 (80×60 坐标系 → 160×120 像素偏差)
|
||||
if (check_row >= 0 && check_row < line_tracking_height && mid_line[check_row] != 255)
|
||||
{
|
||||
double deviation = mid_line[check_row] * calc_scale - newWidth / 2;
|
||||
double bias = readDoubleFromFile(center_bias_file); // 中位偏置(像素), 正=偏右
|
||||
deviation -= bias;
|
||||
double norm = deviation / (newWidth / 2.0); // 归一化到 ±1
|
||||
g_steer_deviation = norm; // 给速度控制用
|
||||
|
||||
// 2c. 死区 (像素)
|
||||
double deadband = readDoubleFromFile(deadband_file);
|
||||
if (std::abs(deviation) < deadband)
|
||||
{
|
||||
servo.setDutyCycle(1500000); // 中位 1.50ms
|
||||
for (int i = 0; i < g_box_count; ++i) {
|
||||
if (g_boxes[i].cls == ZEBRA_CLASS) {
|
||||
g_zebra_ever = true;
|
||||
zebra_cy = (int)g_boxes[i].cy;
|
||||
zebra_cf = g_boxes[i].conf;
|
||||
if (g_zstate == Z_NORMAL) {
|
||||
if (zebra_cy < g_zc_min_cy) g_zc_min_cy = zebra_cy;
|
||||
g_zc_frames++;
|
||||
}
|
||||
else
|
||||
{
|
||||
// 2d. 比例转向: 像素偏差 → 归一化 → 舵机脉宽
|
||||
zebra_seen = true;
|
||||
|
||||
int foresee = (int)readDoubleFromFile(zebrasee_file);
|
||||
if (zebra_cy > foresee)
|
||||
zebra_near = true;
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
if (!zebra_seen) {
|
||||
if (g_zc_frames > 0) g_zc_frames = std::max(0, g_zc_frames - 2);
|
||||
if (g_zc_frames == 0) g_zc_min_cy = 120;
|
||||
}
|
||||
|
||||
time_t now = time(nullptr);
|
||||
switch (g_zstate) {
|
||||
case Z_NORMAL:
|
||||
if (zebra_near) {
|
||||
bool enough = (g_zc_frames >= ZEBRA_MIN_FRAMES);
|
||||
bool from_far = (g_zc_min_cy <= ZEBRA_FAR_CY);
|
||||
if (enough && from_far) {
|
||||
play_zebra_audio();
|
||||
g_zstate = Z_STOP; g_ztime = now;
|
||||
printf("[ZEBRA] cy=%d cf=%.2f f=%d mc=%d 停车4s 冷却5s\n",
|
||||
zebra_cy, zebra_cf, g_zc_frames, g_zc_min_cy);
|
||||
g_zc_frames = 0; g_zc_min_cy = 120;
|
||||
} else {
|
||||
printf("[ZEBRA] cy=%d 拒绝: f=%d/%d mc=%d/%d\n",
|
||||
zebra_cy, g_zc_frames, ZEBRA_MIN_FRAMES, g_zc_min_cy, ZEBRA_FAR_CY);
|
||||
}
|
||||
}
|
||||
break;
|
||||
case Z_STOP:
|
||||
if (now - g_ztime >= 4) {
|
||||
g_zstate = Z_COOLDOWN; g_ztime = now;
|
||||
printf("[ZEBRA] 起步\n");
|
||||
}
|
||||
break;
|
||||
case Z_COOLDOWN:
|
||||
if (now - g_ztime >= 5) {
|
||||
g_zstate = Z_NORMAL;
|
||||
printf("[ZEBRA] 恢复\n");
|
||||
}
|
||||
break;
|
||||
}
|
||||
|
||||
// ── 6. 舵机 ──
|
||||
if (readFlag(start_file)) {
|
||||
int foresee = (int)readDoubleFromFile(foresee_file);
|
||||
int check_row = foresee / calc_scale;
|
||||
if (check_row >= 0 && check_row < line_tracking_height && mid_line[check_row] != 255) {
|
||||
double deviation = mid_line[check_row] * calc_scale - newWidth / 2;
|
||||
double bias = readDoubleFromFile(center_bias_file);
|
||||
deviation -= bias;
|
||||
g_steer_deviation = deviation / (newWidth / 2.0);
|
||||
double deadband = readDoubleFromFile(deadband_file);
|
||||
if (std::abs(deviation) < deadband) {
|
||||
servo.setDutyCycle(1500000);
|
||||
} else {
|
||||
double steer_gain = readDoubleFromFile(steer_gain_file);
|
||||
double norm = deviation / (newWidth / 2.0); // 归一化到 ±1
|
||||
double offset = norm * steer_gain * 300000; // 半行程 0.30ms
|
||||
double norm = deviation / (newWidth / 2.0);
|
||||
double offset = norm * steer_gain * 300000;
|
||||
double duty_ns = 1500000.0 + offset;
|
||||
duty_ns = std::clamp(duty_ns, 1200000.0, 1800000.0);
|
||||
servo.setDutyCycle(static_cast<unsigned int>(duty_ns));
|
||||
}
|
||||
}
|
||||
}
|
||||
else
|
||||
|
||||
// ── 7. 电机控制 (单出口) ──
|
||||
ControlUpdate(target_speed, g_zstate == Z_STOP);
|
||||
|
||||
// ── 8. LCD ──
|
||||
{
|
||||
// servo.setDutyCycle(1520000);
|
||||
static int lcd_check = 0;
|
||||
if (--lcd_check < 0) { g_lcd_on = readFlag(showImg_file); lcd_check = 10; }
|
||||
}
|
||||
|
||||
// 显示图片
|
||||
if (readFlag(showImg_file))
|
||||
{
|
||||
if (g_lcd_on) {
|
||||
cv::Mat fbImage(screenHeight, screenWidth, CV_8UC3, cv::Scalar(0, 0, 0));
|
||||
|
||||
// 缩放视频到新尺寸
|
||||
cv::Mat resizedFrame;
|
||||
cv::resize(track, resizedFrame, cv::Size(newWidth, newHeight));
|
||||
// 将单通道的二值化图像转换为三通道的彩色图像
|
||||
cv::Mat coloredResizedFrame;
|
||||
cv::cvtColor(resizedFrame, coloredResizedFrame, cv::COLOR_GRAY2BGR); // 转换为彩色图像
|
||||
cv::cvtColor(resizedFrame, coloredResizedFrame, cv::COLOR_GRAY2BGR);
|
||||
|
||||
// 将缩放后的图像居中放置在帧缓冲区图像中，填充黑色边框
|
||||
fbImage.setTo(cv::Scalar(0, 0, 0)); // 清空缓冲区（填充黑色）
|
||||
fbImage.setTo(cv::Scalar(0, 0, 0));
|
||||
cv::Rect roi((screenWidth - newWidth) / 2, (screenHeight - newHeight) / 2, newWidth, newHeight);
|
||||
coloredResizedFrame.copyTo(fbImage(roi));
|
||||
|
||||
// 绘制左右边界线和中线
|
||||
int scaledLeftX, scaledRightX, scaledMidX, scaledY;
|
||||
|
||||
for (int y = 0; y < line_tracking_height; y++)
|
||||
{
|
||||
// 根据缩放比例调整X坐标
|
||||
scaledLeftX = static_cast<int>(left_line[y] * calc_scale);
|
||||
scaledRightX = static_cast<int>(right_line[y] * calc_scale);
|
||||
scaledMidX = static_cast<int>(mid_line[y] * calc_scale);
|
||||
scaledY = static_cast<int>(y * calc_scale);
|
||||
|
||||
// 绘制左边界（红）
|
||||
cv::line(fbImage(roi), cv::Point(scaledLeftX, scaledY), cv::Point(scaledLeftX, scaledY), cv::Scalar(0, 0, 255), calc_scale);
|
||||
// 绘制右边界（绿）
|
||||
cv::line(fbImage(roi), cv::Point(scaledRightX, scaledY), cv::Point(scaledRightX, scaledY), cv::Scalar(0, 255, 0), calc_scale);
|
||||
// 绘制中线 （蓝）
|
||||
cv::line(fbImage(roi), cv::Point(scaledMidX, scaledY), cv::Point(scaledMidX, scaledY), cv::Scalar(255, 0, 0), calc_scale);
|
||||
for (int y = 0; y < line_tracking_height; y++) {
|
||||
int sLX=static_cast<int>(left_line[y]*calc_scale), sRX=static_cast<int>(right_line[y]*calc_scale);
|
||||
int sMX=static_cast<int>(mid_line[y]*calc_scale), sY=static_cast<int>(y*calc_scale);
|
||||
cv::line(fbImage(roi), cv::Point(sLX,sY), cv::Point(sLX,sY), cv::Scalar(0,0,255), calc_scale);
|
||||
cv::line(fbImage(roi), cv::Point(sRX,sY), cv::Point(sRX,sY), cv::Scalar(0,255,0), calc_scale);
|
||||
cv::line(fbImage(roi), cv::Point(sMX,sY), cv::Point(sMX,sY), cv::Scalar(255,0,0), calc_scale);
|
||||
}
|
||||
|
||||
// 将帧缓冲区图像转换为RGB565格式
|
||||
float bx=(float)newWidth/160.0f, by=(float)newHeight/120.0f;
|
||||
for (int i = 0; i < g_box_count; ++i) {
|
||||
if (g_boxes[i].cls == 1 || g_boxes[i].cls == 2) continue;
|
||||
int x1=(int)((g_boxes[i].cx-g_boxes[i].w/2)*bx), y1=(int)((g_boxes[i].cy-g_boxes[i].h/2)*by);
|
||||
int x2=(int)((g_boxes[i].cx+g_boxes[i].w/2)*bx), y2=(int)((g_boxes[i].cy+g_boxes[i].h/2)*by);
|
||||
x1=std::max(0,std::min(newWidth-1,x1)); y1=std::max(0,std::min(newHeight-1,y1));
|
||||
x2=std::max(0,std::min(newWidth-1,x2)); y2=std::max(0,std::min(newHeight-1,y2));
|
||||
cv::Scalar color(0,255,0);
|
||||
if (g_boxes[i].cls == ZEBRA_CLASS) color=cv::Scalar(255,0,255);
|
||||
cv::rectangle(fbImage(roi), cv::Point(x1,y1), cv::Point(x2,y2), color, 2);
|
||||
char lab[16]; std::snprintf(lab,16,"%d %.0f",g_boxes[i].cls,g_boxes[i].conf*100);
|
||||
cv::putText(fbImage(roi), lab, cv::Point(x1+2,y1+10), cv::FONT_HERSHEY_SIMPLEX,0.3,color,1);
|
||||
}
|
||||
|
||||
const char* ztxt="N";
|
||||
if (g_zstate==Z_STOP) ztxt="S";
|
||||
else if (g_zstate==Z_COOLDOWN) ztxt="C";
|
||||
cv::putText(fbImage(roi), ztxt, cv::Point(2,newHeight-4), cv::FONT_HERSHEY_SIMPLEX,0.4,cv::Scalar(0,255,255),1);
|
||||
|
||||
convertMatToRGB565(fbImage, fb_buffer, screenWidth, screenHeight);
|
||||
}
|
||||
|
||||
// ── 9. FPS ──
|
||||
{
|
||||
static int fc=0; static timespec t0; if(fc==0) clock_gettime(CLOCK_MONOTONIC,&t0);
|
||||
fc++;
|
||||
if(fc%15==0){ timespec t1; clock_gettime(CLOCK_MONOTONIC,&t1);
|
||||
double dt=(t1.tv_sec-t0.tv_sec)+(t1.tv_nsec-t0.tv_nsec)*1e-9;
|
||||
printf("fps=%.1f zc=%c lcd=%c \r", fc/dt, g_zebra_ever?'Y':' ', g_lcd_on?'Y':' ');
|
||||
fflush(stdout);
|
||||
}
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
|
||||
+33
-44
@@ -1,20 +1,12 @@
|
||||
/*
|
||||
* @Author: ilikara 3435193369@qq.com
|
||||
* @Date: 2024-10-10 09:02:10
|
||||
* @LastEditors: ilikara 3435193369@qq.com
|
||||
* @LastEditTime: 2025-03-21 10:40:10
|
||||
* @FilePath: /smartcar/src/control.cpp
|
||||
* @Description:
|
||||
*
|
||||
* Copyright (c) 2024 by ${git_name_email}, All Rights Reserved.
|
||||
*/
|
||||
#include "control.h"
|
||||
|
||||
#include "GPIO.h"
|
||||
extern double g_steer_deviation; // camera.cpp 输出的归一化偏差
|
||||
|
||||
extern double g_steer_deviation;
|
||||
|
||||
MotorController *motorController[2] = {nullptr, nullptr};
|
||||
|
||||
GPIO mortorEN(73);
|
||||
|
||||
double mortor_kp = 1000;
|
||||
double mortor_ki = 300;
|
||||
double mortor_kd = 0;
|
||||
@@ -28,46 +20,43 @@ void ControlInit()
|
||||
leftIn2.setDirection("out");
|
||||
leftIn2.setValue(1);
|
||||
|
||||
const int pwmchip[2] = {8, 8};
|
||||
const int pwmnum[2] = {2, 1};
|
||||
const int gpioNum[2] = {12, 13};
|
||||
const int encoder_pwmchip[2] = {0, 3};
|
||||
const int encoder_gpioNum[2] = {75, 72};
|
||||
const int encoder_dir[2] = {1, -1};
|
||||
const unsigned int period_ns = 50000; // 20 kHz
|
||||
const int pwmchip[2] = {8, 8};
|
||||
const int pwmnum[2] = {2, 1};
|
||||
const int gpioNum[2] = {12, 13};
|
||||
const int encoder_pwmchip[2] = {0, 3};
|
||||
const int encoder_gpioNum[2] = {75, 72};
|
||||
const int encoder_dir[2] = {1, -1};
|
||||
|
||||
const unsigned int period_ns = 50000;
|
||||
|
||||
for (int i = 0; i < 2; ++i)
|
||||
{
|
||||
motorController[i] = new MotorController(pwmchip[i], pwmnum[i], gpioNum[i], period_ns,
|
||||
mortor_kp, mortor_ki, mortor_kd, 0,
|
||||
encoder_pwmchip[i], encoder_gpioNum[i], encoder_dir[i]);
|
||||
motorController[i] = new MotorController(
|
||||
pwmchip[i], pwmnum[i], gpioNum[i], period_ns,
|
||||
mortor_kp, mortor_ki, mortor_kd, 0,
|
||||
encoder_pwmchip[i], encoder_gpioNum[i], encoder_dir[i]
|
||||
);
|
||||
}
|
||||
}
|
||||
|
||||
void ControlMain()
|
||||
void ControlUpdate(double speed, bool zebra_block)
|
||||
{
|
||||
if (readFlag(start_file))
|
||||
if (zebra_block || !readFlag(start_file))
|
||||
{
|
||||
// 弯道减速: 偏差大→速度低, 最低 30%
|
||||
double curve = 1.0 - std::abs(g_steer_deviation) * 0.7;
|
||||
if (curve < 0.3) curve = 0.3;
|
||||
double spd = target_speed * curve;
|
||||
for (int i = 0; i < 2; ++i)
|
||||
if (motorController[i]) motorController[i]->updateduty(0);
|
||||
if (!readFlag(start_file)) mortorEN.setValue(0);
|
||||
return;
|
||||
}
|
||||
|
||||
for (int i = 0; i < 2; ++i)
|
||||
{
|
||||
motorController[i]->updateduty(spd);
|
||||
}
|
||||
mortorEN.setValue(1);
|
||||
}
|
||||
else
|
||||
{
|
||||
for (int i = 0; i < 2; ++i)
|
||||
{
|
||||
motorController[i]->updateduty(0);
|
||||
}
|
||||
mortorEN.setValue(0);
|
||||
}
|
||||
return;
|
||||
double curve = 1.0 - std::abs(g_steer_deviation) * 0.4;
|
||||
if (curve < 0.6) curve = 0.6;
|
||||
double spd = speed * curve;
|
||||
|
||||
for (int i = 0; i < 2; ++i)
|
||||
if (motorController[i]) motorController[i]->updateduty(spd);
|
||||
|
||||
mortorEN.setValue(1);
|
||||
}
|
||||
|
||||
void ControlExit()
|
||||
@@ -75,7 +64,7 @@ void ControlExit()
|
||||
for (int i = 0; i < 2; ++i)
|
||||
{
|
||||
delete motorController[i];
|
||||
std::cout << "motor" << i << "deleted\n";
|
||||
std::cout << "motor" << i << " deleted\n";
|
||||
}
|
||||
mortorEN.setValue(0);
|
||||
}
|
||||
|
||||
+213
-45
@@ -4,79 +4,190 @@
|
||||
* @LastEditors: ilikara 3435193369@qq.com
|
||||
* @LastEditTime: 2025-03-13 08:15:10
|
||||
* @FilePath: /2k300_smartcar/src/image_cv.cpp
|
||||
* @Description: 这是默认设置,请设置`customMade`, 打开koroFileHeader查看配置 进行设置: https://github.com/OBKoro1/koro1FileHeader/wiki/%E9%85%8D%E7%BD%AE
|
||||
* @Description: 视觉巡线管线 —— HSV 二值化 → 洪泛填充 → 逐行边界搜索 → EMA 滤波
|
||||
*
|
||||
* 处理分辨率: 80×60（line_tracking_width × line_tracking_height）
|
||||
* 这是 2K0300 CPU 上的最优平衡点：再大 Otsu 不稳，再小细节丢失
|
||||
*/
|
||||
#include "image_cv.h"
|
||||
|
||||
cv::Mat raw_frame;
|
||||
cv::Mat grayFrame;
|
||||
cv::Mat binarizedFrame;
|
||||
cv::Mat morphologyExFrame;
|
||||
cv::Mat track;
|
||||
// ============================================================
|
||||
// 全局图像变量
|
||||
// ============================================================
|
||||
|
||||
std::vector<int> left_line; // 左边缘列号数组
|
||||
std::vector<int> right_line; // 右边缘列号数组
|
||||
std::vector<int> mid_line; // 中线列号数组
|
||||
std::vector<double> left_line_filtered; // 中线列号数组
|
||||
std::vector<double> right_line_filtered; // 中线列号数组
|
||||
std::vector<double> mid_line_filtered; // 中线列号数组
|
||||
cv::Mat raw_frame; // 摄像头原始帧 (320×240, BGR)
|
||||
cv::Mat grayFrame; // 灰度图 (未在当前管线中使用，保留)
|
||||
cv::Mat binarizedFrame; // HSV 双通道 Otsu 二值化结果
|
||||
cv::Mat morphologyExFrame; // 形态学开运算后的图像
|
||||
cv::Mat track; // 洪泛填充后的赛道区域蒙版
|
||||
|
||||
int line_tracking_width;
|
||||
int line_tracking_height;
|
||||
// ============================================================
|
||||
// 边界线数组 — 左/右边缘和中线的逐行列坐标
|
||||
//
|
||||
// 坐标系: row=0 = 图像顶部(远处), row=59 = 底部(近处)
|
||||
// 列坐标范围: 0 ~ line_tracking_width-1 (0~79)
|
||||
// 未检测到边界 = -1
|
||||
// ============================================================
|
||||
std::vector<int> left_line; // 左边缘列号 (逐行)
|
||||
std::vector<int> right_line; // 右边缘列号 (逐行)
|
||||
std::vector<int> mid_line; // 中线列号 = (left+right)/2
|
||||
std::vector<double> left_line_filtered; // 左边缘 EMA 滤波结果
|
||||
std::vector<double> right_line_filtered; // 右边缘 EMA 滤波结果
|
||||
std::vector<double> mid_line_filtered; // 中线滤波结果
|
||||
|
||||
int line_tracking_width; // 处理宽度 = 80
|
||||
int line_tracking_height; // 处理高度 = 60
|
||||
|
||||
// ============================================================
|
||||
// image_binerize — HSV 双通道 Otsu 二值化
|
||||
//
|
||||
// 输入: BGR 彩色帧 (80×60)
|
||||
// 输出: 二值图，赛道区域 = 白色(255)，边界/背景 = 黑色(0)
|
||||
//
|
||||
// 原理:
|
||||
// 蓝底赛道 → 蓝色区域饱和度高(S通道高)，色调集中(H通道100~130)
|
||||
// 灰度路面 → 饱和度低，色调分散
|
||||
//
|
||||
// 1. BGR → HSV 色彩空间转换
|
||||
// 2. H 通道 Otsu 阈值 → 分离蓝色与非蓝色
|
||||
// 3. S 通道 Otsu 阈值 → 分离高饱和(蓝)与低饱和(灰)
|
||||
// 4. 两通道做 bitwise_or 取并集 (THRESH_BINARY_INV 确保赛道=255)
|
||||
// 即只要 H 或 S 任一判定为赛道，就标记为赛道区域
|
||||
//
|
||||
// THRESH_BINARY_INV + THRESH_OTSU:
|
||||
// Otsu 自动计算最优阈值 T
|
||||
// pixel > T → 0 (黑色), pixel ≤ T → 255 (白色)
|
||||
// 赛道(蓝色/高饱和)通常偏向一侧，Otsu 将其归到低值区间
|
||||
// INV 反转后赛道 = 白色(前景, 255), 背景 = 黑色(0)
|
||||
// ============================================================
|
||||
cv::Mat image_binerize(cv::Mat &frame)
|
||||
{
|
||||
cv::Mat output;
|
||||
cv::Mat binarizedFrame;
|
||||
cv::Mat hsvImage;
|
||||
|
||||
// 1. BGR → HSV：分离色调(H)、饱和度(S)、明度(V) 三个通道
|
||||
cv::cvtColor(frame, hsvImage, cv::COLOR_BGR2HSV);
|
||||
|
||||
std::vector<cv::Mat> hsvChannels;
|
||||
cv::split(hsvImage, hsvChannels);
|
||||
// hsvChannels[0] = H (色调, 0~180)
|
||||
// hsvChannels[1] = S (饱和度, 0~255)
|
||||
// hsvChannels[2] = V (明度, 0~255)
|
||||
|
||||
cv::threshold(hsvChannels[0], binarizedFrame, 0, 255, cv::THRESH_BINARY_INV | cv::THRESH_OTSU);
|
||||
cv::threshold(hsvChannels[1], output, 0, 255, cv::THRESH_BINARY_INV | cv::THRESH_OTSU);
|
||||
// 2. H 通道 Otsu 二值化：区分蓝色与非蓝色
|
||||
cv::threshold(hsvChannels[0], binarizedFrame,
|
||||
0, 255, cv::THRESH_BINARY_INV | cv::THRESH_OTSU);
|
||||
|
||||
// 3. S 通道 Otsu 二值化：区分高饱和(蓝色赛道)与低饱和(灰色路面)
|
||||
cv::threshold(hsvChannels[1], output,
|
||||
0, 255, cv::THRESH_BINARY_INV | cv::THRESH_OTSU);
|
||||
|
||||
// 4. 双通道融合：任一通道认为"非赛道"就排除
|
||||
// bitwise_or: 两幅图同为赛道(255)才保留，否则变为背景(0)
|
||||
cv::bitwise_or(output, binarizedFrame, output);
|
||||
|
||||
return output;
|
||||
}
|
||||
|
||||
// ============================================================
|
||||
// find_road — 形态学去噪 + 洪泛填充提取连通赛道区域
|
||||
//
|
||||
// 输入: image_binerize 的输出 (赛道=255, 背景=0)
|
||||
// 输出: 仅保留与底边中点连通的赛道区域蒙版
|
||||
//
|
||||
// 处理步骤:
|
||||
// 1. 形态学开运算 (MORPH_OPEN): 先腐蚀后膨胀，消除孤立噪点
|
||||
// 核大小 2×2 十字形 — 极小核，避免吞没细弯
|
||||
// 2. 在底边中心上方偏移处放置种子点
|
||||
// 3. 洪泛填充 (floodFill):
|
||||
// - 从种子点出发，填充容差范围内的连通区域
|
||||
// - 填充值 = 128 (灰色)，区分于原始白色(255)
|
||||
// - loDiff/upDiff = 20: 像素值在 [108, 148] 范围内的像素被填充
|
||||
// - 邻域 8 连通
|
||||
// 4. 将蒙版 mask 的 ROI 区域复制到输出图像
|
||||
// - 赛道内部 = 128 (非零)，外部 = 0
|
||||
//
|
||||
// 为什么要洪泛填充？
|
||||
// 二值化后可能有多块白色区域（赛道 + 赛道外的反光/杂物）
|
||||
// 洪泛填充确保只处理"从车底到前方连通的"那块区域
|
||||
// 排除远处/边缘的伪赛道碎片
|
||||
// ============================================================
|
||||
cv::Mat find_road(cv::Mat &frame)
|
||||
{
|
||||
// 1. 形态学开运算去噪
|
||||
// MORPH_CROSS: 十字形结构元素，2×2
|
||||
// MORPH_OPEN: 先腐蚀(去除小白点) 再膨胀(恢复区域尺寸)
|
||||
cv::Mat kernel = cv::getStructuringElement(cv::MORPH_CROSS, cv::Size(2, 2));
|
||||
cv::morphologyEx(binarizedFrame, morphologyExFrame, cv::MORPH_OPEN, kernel);
|
||||
|
||||
// 2. 创建洪泛填充蒙版
|
||||
// 尺寸比原图各边大 2 像素 (floodFill 要求)
|
||||
cv::Mat mask = cv::Mat::zeros(line_tracking_height + 2, line_tracking_width + 2, CV_8UC1);
|
||||
|
||||
// 3. 种子点位置
|
||||
// X = 图像水平中心 (line_tracking_width/2)
|
||||
// Y = 距底部 10 行 (line_tracking_height-10)
|
||||
// 假设车在赛道中央附近，底部中心一定是赛道上
|
||||
cv::Point seedPoint(line_tracking_width / 2, line_tracking_height - 10);
|
||||
|
||||
// 4. 在种子点位置画一个白色实心圆，确保种子点落在赛道区域内
|
||||
// 避免因二值化偶尔在种子位置为黑色导致洪泛失败
|
||||
cv::circle(morphologyExFrame, seedPoint, 10, 255, -1);
|
||||
|
||||
cv::Scalar newVal(128);
|
||||
// 5. 洪泛填充参数
|
||||
cv::Scalar newVal(128); // 填充值 = 128 (标记为赛道内部)
|
||||
cv::Scalar loDiff = cv::Scalar(20); // 下界: 当前像素值 - 20 = 108
|
||||
cv::Scalar upDiff = cv::Scalar(20); // 上界: 当前像素值 + 20 = 148
|
||||
|
||||
cv::Scalar loDiff = cv::Scalar(20);
|
||||
cv::Scalar upDiff = cv::Scalar(20);
|
||||
|
||||
cv::floodFill(morphologyExFrame, mask, seedPoint, newVal, 0, loDiff, upDiff, 8);
|
||||
// 6. 执行洪泛填充
|
||||
// 从种子点向 8 邻域扩散，填充像素值在 [108, 148] 之间的所有连通像素
|
||||
// 填充结果直接写入 morphologyExFrame（in-place）
|
||||
// 蒙版 mask 记录被填充的像素位置
|
||||
cv::floodFill(morphologyExFrame, mask, seedPoint, newVal,
|
||||
nullptr, loDiff, upDiff, 8);
|
||||
|
||||
// 7. 从蒙版提取赛道区域
|
||||
// mask 的外扩边框(±1) 用于 floodFill 的内部计算，实际区域在(1,1)起
|
||||
// ROI 裁掉边框后即为赛道主体蒙版
|
||||
cv::Mat outputImage = cv::Mat::zeros(line_tracking_width, line_tracking_height, CV_8UC1);
|
||||
|
||||
mask(cv::Rect(1, 1, line_tracking_width, line_tracking_height)).copyTo(outputImage);
|
||||
|
||||
return outputImage;
|
||||
}
|
||||
|
||||
// ============================================================
|
||||
// image_main — 视觉巡线主函数，每帧调用一次
|
||||
//
|
||||
// 完整处理流水线：
|
||||
// raw_frame (320×240) → resize (80×60) → HSV二值化 → 形态学+洪泛
|
||||
// → 逐行最长连续段搜索 → 中线计算 → EMA 自底向上滤波 →
|
||||
// 输出 left_line[60], right_line[60], mid_line[60]
|
||||
//
|
||||
// 注意：坐标原点在左上角
|
||||
// - row=0 = 图像顶部（远处）
|
||||
// - row=59 = 图像底部（近处，车前方）
|
||||
// - 滤波从底部(row=59)往顶部(row=0)递推
|
||||
// ============================================================
|
||||
void image_main()
|
||||
{
|
||||
cv::Mat resizedFrame;
|
||||
|
||||
cv::resize(raw_frame, resizedFrame, cv::Size(line_tracking_width, line_tracking_height));
|
||||
// ── 1. 降采样 ──────────────────────────────────────
|
||||
// 320×240 → 80×60，大幅缩减计算量
|
||||
cv::resize(raw_frame, resizedFrame,
|
||||
cv::Size(line_tracking_width, line_tracking_height));
|
||||
|
||||
// ── 2. HSV 双通道 Otsu 二值化 ─────────────────────
|
||||
// 赛道区域 = 白色(255)，背景/边界 = 黑色(0)
|
||||
binarizedFrame = image_binerize(resizedFrame);
|
||||
|
||||
// ── 3. 洪泛填充提取赛道主体 ────────────────────────
|
||||
// 只保留与车底中点连通的赛道区域，排除伪赛道碎片
|
||||
// 输出: track = 赛道内部(128) / 赛道外部(0)
|
||||
track = find_road(binarizedFrame);
|
||||
|
||||
// ── 4. 初始化边界线数组 ────────────────────────────
|
||||
left_line.clear();
|
||||
right_line.clear();
|
||||
mid_line.clear();
|
||||
@@ -91,20 +202,33 @@ void image_main()
|
||||
right_line_filtered.resize(line_tracking_height, -1);
|
||||
mid_line_filtered.resize(line_tracking_height, -1);
|
||||
|
||||
uchar(*IMG)[line_tracking_width] = reinterpret_cast<uchar(*)[line_tracking_width]>(track.data);
|
||||
// ── 5. 逐行最长连续段搜索 ──────────────────────────
|
||||
//
|
||||
// 将 track 的 uchar* 数据解释为二维数组 IMG[row][col]
|
||||
// track 中 赛道内部 = 128(非零), 外部 = 0
|
||||
// 对每一行: 找到最长的连续非零段
|
||||
// → 段的起点 = left_line[row]
|
||||
// → 段的终点 = right_line[row]
|
||||
// → 无赛道段的行 = -1
|
||||
//
|
||||
// 这在低分辨率下比八邻域边界跟踪更高效：
|
||||
// 80×60 只有 4800 像素，逐行扫描 O(W×H) 已足够快
|
||||
uchar(*IMG)[line_tracking_width] =
|
||||
reinterpret_cast<uchar(*)[line_tracking_width]>(track.data);
|
||||
|
||||
for (int i = 0; i < line_tracking_height; ++i)
|
||||
for (int i = 0; i < line_tracking_height; ++i) // i = row
|
||||
{
|
||||
int max_start = -1;
|
||||
int max_end = -1;
|
||||
int current_start = -1;
|
||||
int current_length = 0;
|
||||
int max_length = 0;
|
||||
int max_start = -1; // 最长连续段的起始列
|
||||
int max_end = -1; // 最长连续段的结束列
|
||||
int current_start = -1; // 当前扫描中的段起始列
|
||||
int current_length = 0; // 当前扫描中的段长度
|
||||
int max_length = 0; // 已记录的最长段长度
|
||||
|
||||
for (int j = 0; j < line_tracking_width; ++j)
|
||||
for (int j = 0; j < line_tracking_width; ++j) // j = col
|
||||
{
|
||||
if (IMG[i][j])
|
||||
if (IMG[i][j]) // 非零 = 赛道内部像素
|
||||
{
|
||||
// 赛道段的第一个像素: 记录起点
|
||||
if (current_length == 0)
|
||||
{
|
||||
current_start = j;
|
||||
@@ -112,8 +236,11 @@ void image_main()
|
||||
}
|
||||
else
|
||||
{
|
||||
// 赛道段延续: 长度+1
|
||||
current_length++;
|
||||
}
|
||||
|
||||
// 更新最长记录 (≥ 而非 >, 取最后一个最长段，靠右)
|
||||
if (current_length >= max_length)
|
||||
{
|
||||
max_length = current_length;
|
||||
@@ -121,57 +248,98 @@ void image_main()
|
||||
max_end = j;
|
||||
}
|
||||
}
|
||||
else
|
||||
else // 零值 = 非赛道
|
||||
{
|
||||
// 赛道段结束: 重置
|
||||
current_length = 0;
|
||||
current_start = -1;
|
||||
}
|
||||
}
|
||||
|
||||
// 记录该行的左右边界
|
||||
if (max_length > 0)
|
||||
{
|
||||
left_line[i] = max_start;
|
||||
right_line[i] = max_end;
|
||||
left_line[i] = max_start; // 最长白段的左端点
|
||||
right_line[i] = max_end; // 最长白段的右端点
|
||||
}
|
||||
else
|
||||
{
|
||||
left_line[i] = -1;
|
||||
// 该行无赛道 → 后续由中线补全逻辑处理
|
||||
left_line[i] = -1;
|
||||
right_line[i] = -1;
|
||||
}
|
||||
}
|
||||
|
||||
double a = 0.4;
|
||||
// ── 6. 中线计算 + 丢线补全 → 自底向上 EMA 滤波 ─────
|
||||
//
|
||||
// 核心逻辑（从底部 row=59 往上到 row=10）:
|
||||
//
|
||||
// 6a. 如果当前行左右边界均有效 → 中线 = (left + right) / 2
|
||||
//
|
||||
// 6b. 如果当前行丢线（左右均无效）:
|
||||
// 用下一行(row+1)的中线补全:
|
||||
// → 中线下半区: 虚拟右边界在右边缘，左边界 = 下行中线
|
||||
// → 中线上半区: 虚拟左边界在左边缘，右边界 = 下行中线
|
||||
// 自动适应赛道偏左还是偏右的情况
|
||||
//
|
||||
// 6c. EMA 滤波 (指数移动平均):
|
||||
// row 本身的值权重 = a (0.4), 下行滤波值权重 = 1-a (0.6)
|
||||
// 从底部往顶部递推: 近处(底部)值稳定，远处(顶部)靠递推外推
|
||||
// a=0.4 → 近处值占主导，但保留过去趋势的惯性
|
||||
//
|
||||
double a = 0.4; // EMA 系数: 平衡当前测量与历史递推
|
||||
|
||||
for (int row = line_tracking_height - 1; row >= 10; --row)
|
||||
{
|
||||
// ── 6b. 丢线补全 ──────────────────────────────
|
||||
if (left_line[row] == -1 && right_line[row] == -1)
|
||||
{
|
||||
// 当前行完全丢线: 用下行(row+1)的中线来虚拟补线
|
||||
mid_line[row] = mid_line[row + 1];
|
||||
|
||||
if (mid_line[row] > line_tracking_width / 2)
|
||||
{
|
||||
// 中线偏右 → 实际赛道在图像右半区
|
||||
// 虚拟右边界 = 图像右边缘, 左边界 = 下行中线位置
|
||||
right_line[row] = line_tracking_width - 1;
|
||||
left_line[row] = mid_line[row + 1];
|
||||
left_line[row] = mid_line[row + 1];
|
||||
}
|
||||
else
|
||||
{
|
||||
left_line[row] = 0;
|
||||
// 中线偏左 → 实际赛道在图像左半区
|
||||
// 虚拟左边界 = 图像左边缘, 右边界 = 下行中线位置
|
||||
left_line[row] = 0;
|
||||
right_line[row] = mid_line[row + 1];
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
// 正常行: 中线 = 左右边界中点
|
||||
mid_line[row] = (left_line[row] + right_line[row]) / 2;
|
||||
}
|
||||
|
||||
// ── 6c. EMA 滤波 (自底向上) ────────────────────
|
||||
if (row == line_tracking_height - 1)
|
||||
{
|
||||
left_line_filtered[row] = left_line[row];
|
||||
// 最底行(最近处): 无下行参考，直接使用原始值
|
||||
left_line_filtered[row] = left_line[row];
|
||||
right_line_filtered[row] = right_line[row];
|
||||
mid_line_filtered[row] = mid_line[row];
|
||||
mid_line_filtered[row] = mid_line[row];
|
||||
}
|
||||
else
|
||||
{
|
||||
left_line_filtered[row] = a * left_line[row] + (1 - a) * left_line_filtered[row + 1];
|
||||
right_line_filtered[row] = a * right_line[row] + (1 - a) * right_line_filtered[row + 1];
|
||||
// mid_line_filtered[row] = a * mid_line[row] + (1 - a) * mid_line_filtered[row + 1];
|
||||
// 公式: filtered[row] = a * raw[row] + (1-a) * filtered[row+1]
|
||||
// a=0.4: 40% 当前行实测值 + 60% 下行滤波值的递推
|
||||
// 效果: 近处值稳定，越往远处越靠递推，杜绝抖动的概率传播
|
||||
left_line_filtered[row] = a * left_line[row]
|
||||
+ (1 - a) * left_line_filtered[row + 1];
|
||||
right_line_filtered[row] = a * right_line[row]
|
||||
+ (1 - a) * right_line_filtered[row + 1];
|
||||
|
||||
// 中线的滤波值为左右滤波边界的均值（不是对原始中线做 EMA）
|
||||
// 即: 先对左右边界各自滤波, 再求平均 → 减少中线突跳
|
||||
// 原注释行: mid_line_filtered[row] = a*mid_line[row] + (1-a)*mid_line_filtered[row+1]
|
||||
mid_line_filtered[row] = (left_line_filtered[row] + right_line_filtered[row]) / 2.0;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
@@ -0,0 +1,568 @@
|
||||
/*
|
||||
* NanoDetHeat v10: 3.2M MACs, 4-class
|
||||
* 优化: BN折叠 + conv_bn_relu融合 + 快速exp + 边界修正
|
||||
*/
|
||||
#include "model_v10.hpp"
|
||||
#include <cstdio>
|
||||
#include <cstdlib>
|
||||
#include <cstring>
|
||||
#include <cmath>
|
||||
#include <ctime>
|
||||
#include <new>
|
||||
#include <algorithm>
|
||||
#include <unordered_map>
|
||||
#include <string>
|
||||
#include <vector>
|
||||
|
||||
// ============================================================
|
||||
// 快速 exp — Schraudolph 方法 (IEEE 754 整数近似)
|
||||
// ============================================================
|
||||
static inline float fast_exp(float x) {
|
||||
// clamp to avoid overflow: exp(88) fits in float
|
||||
x = std::min(x, 88.0f);
|
||||
x = std::max(x, -87.0f);
|
||||
// exp(x) ~ 2^(x * log2(e))
|
||||
// float 按位: 2^23 * (127 + x*log2(e))
|
||||
union { float f; int i; } u;
|
||||
u.i = (int)(12102203.0f * x) + 1064866805; // 2^23 * log2(e) = 12102203
|
||||
return u.f;
|
||||
}
|
||||
static inline float fast_sigmoid(float x) {
|
||||
return 1.0f / (1.0f + fast_exp(-x));
|
||||
}
|
||||
|
||||
// ============================================================
|
||||
// 权重 (新增 BN alpha/beta 预计算)
|
||||
// ============================================================
|
||||
struct WT { char n[128]; int nd; int s[4]; float* d; };
|
||||
static int gn=0; static WT* gw=nullptr;
|
||||
|
||||
static float* wf(const char* k) {
|
||||
for(int i=0;i<gn;++i) if(!std::strcmp(gw[i].n,k)) return gw[i].d;
|
||||
return nullptr;
|
||||
}
|
||||
|
||||
static void init_lut();
|
||||
|
||||
// 加载后扫描 BN 参数，预计算 alpha/beta 存入权重表
|
||||
static void bn_precompute() {
|
||||
// 扫描所有权重，找 BN 三元组 (weight/bias/running_mean/running_var)
|
||||
std::unordered_map<std::string, int> base_idx;
|
||||
for (int i=0; i<gn; ++i) {
|
||||
const char* name = gw[i].n;
|
||||
// 匹配 xxx.weight (BN) 或 xxx.bias (BN) → base = "xxx"
|
||||
const char* dot = std::strrchr(name, '.');
|
||||
if (!dot) continue;
|
||||
std::string base(name, dot - name);
|
||||
int sz = gw[i].s[0];
|
||||
|
||||
if (std::strcmp(dot+1, "weight")==0 && sz>0 && sz <= 256) {
|
||||
base_idx[base] = i;
|
||||
}
|
||||
}
|
||||
|
||||
std::vector<WT> new_wts;
|
||||
for (auto& [base, wi] : base_idx) {
|
||||
// 找对应的 bias / running_mean / running_var
|
||||
std::string w_key = base + ".weight";
|
||||
std::string b_key = base + ".bias";
|
||||
std::string m_key = base + ".running_mean";
|
||||
std::string v_key = base + ".running_var";
|
||||
|
||||
float* w = wf(w_key.c_str());
|
||||
float* b = wf(b_key.c_str());
|
||||
float* m = wf(m_key.c_str());
|
||||
float* v = wf(v_key.c_str());
|
||||
|
||||
if (!w || !m || !v) continue; // cls_head/size_head 没有 BN
|
||||
int C = gw[wi].s[0];
|
||||
float* alpha = new float[C];
|
||||
float* beta = new float[C];
|
||||
const float eps = 1e-5f;
|
||||
for (int c=0; c<C; ++c) {
|
||||
alpha[c] = w[c] / std::sqrt(v[c] + eps);
|
||||
beta[c] = (b ? b[c] : 0.0f) - m[c] * alpha[c];
|
||||
}
|
||||
// 存为新的权重条目
|
||||
new_wts.push_back({});
|
||||
std::snprintf(new_wts.back().n, 128, "%s._alpha", base.c_str());
|
||||
new_wts.back().nd=1; new_wts.back().s[0]=C; new_wts.back().s[1]=1; new_wts.back().s[2]=1; new_wts.back().s[3]=1;
|
||||
new_wts.back().d = alpha;
|
||||
new_wts.push_back({});
|
||||
std::snprintf(new_wts.back().n, 128, "%s._beta", base.c_str());
|
||||
new_wts.back().nd=1; new_wts.back().s[0]=C; new_wts.back().s[1]=1; new_wts.back().s[2]=1; new_wts.back().s[3]=1;
|
||||
new_wts.back().d = beta;
|
||||
}
|
||||
|
||||
// 重新分配权重数组 (旧数组 + 新条目)
|
||||
WT* old_gw = gw;
|
||||
int old_gn = gn;
|
||||
int new_gn = old_gn + (int)new_wts.size();
|
||||
gw = new WT[new_gn];
|
||||
std::memcpy(gw, old_gw, old_gn * sizeof(WT));
|
||||
for (size_t i=0; i<new_wts.size(); ++i) {
|
||||
gw[old_gn + i] = new_wts[i];
|
||||
}
|
||||
gn = new_gn;
|
||||
delete[] old_gw;
|
||||
init_lut();
|
||||
}
|
||||
|
||||
// ============================================================
|
||||
// 内存
|
||||
// ============================================================
|
||||
struct M10 {
|
||||
float preproc[3*120*160]; // 预处理 buffer (静态, 避免每帧 new/delete)
|
||||
float stem[6*60*80];
|
||||
float b1[8*60*80];
|
||||
float b2[12*30*40];
|
||||
float b3[16*15*20];
|
||||
float b4[32*15*20];
|
||||
float b5[48*15*20];
|
||||
float b6[48*15*20];
|
||||
float sh[24*15*20];
|
||||
float cl[5*15*20];
|
||||
float sz[2*15*20];
|
||||
float dw_out[8*60*80];
|
||||
float se_buf[80];
|
||||
uint8 peak_mask[15*20];
|
||||
};
|
||||
static M10* m=nullptr;
|
||||
|
||||
// ============================================================
|
||||
// Relu
|
||||
// ============================================================
|
||||
static void relu_f(float* x, int N) {
|
||||
for (int i=0; i<N; ++i) if (x[i]<0) x[i]=0;
|
||||
}
|
||||
|
||||
// ============================================================
|
||||
// 融合: conv + BN + ReLU (使用预计算的 alpha/beta)
|
||||
// ============================================================
|
||||
static void conv_bn_relu(float* o, const float* in,
|
||||
const float* conv_w, const float* conv_b,
|
||||
const float* bn_alpha, const float* bn_beta,
|
||||
int H, int W, int iC, int oC, int K, int str, int grp, bool use_relu)
|
||||
{
|
||||
if (!conv_w || !in || !o) return;
|
||||
int Ho=H/str, Wo=W/str, pad=K/2;
|
||||
int icpg=iC/grp, ocpg=oC/grp;
|
||||
|
||||
if (K==1 && str==1) {
|
||||
// 1×1 fused: 指针递增避免 y*W+x
|
||||
for (int g=0; g<grp; ++g) {
|
||||
for (int oc=0; oc<ocpg; ++oc) {
|
||||
int occ = g*ocpg + oc;
|
||||
float* oo = o + occ*H*W;
|
||||
float cb = conv_b ? conv_b[occ] : 0.0f;
|
||||
const float* ww = conv_w + occ*icpg;
|
||||
float ba=bn_alpha?bn_alpha[occ]:1.0f, bb=bn_beta?bn_beta[occ]:0.0f;
|
||||
|
||||
for (int y=0; y<H; ++y) {
|
||||
float* oy = oo + y*W;
|
||||
const float* iy = in + y*W;
|
||||
for (int x=0; x<W; ++x) {
|
||||
float s = cb;
|
||||
const float* px = iy + x;
|
||||
for (int ic=0; ic<icpg; ++ic) {
|
||||
int icc = g*icpg + ic;
|
||||
s += ww[ic] * px[icc*H*W];
|
||||
}
|
||||
s = s*ba + bb;
|
||||
if (use_relu && s<0) s = 0;
|
||||
oy[x] = s;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
return;
|
||||
}
|
||||
|
||||
if (grp==iC && K==3 && str==1) {
|
||||
// 3×3 dw fused + interior unrolled / border with checks
|
||||
for (int c=0; c<iC; ++c) {
|
||||
float* const oo = o + c*H*W;
|
||||
const float* const ii = in + c*H*W;
|
||||
const float* const ww = conv_w + c*9;
|
||||
float cb = conv_b ? conv_b[c] : 0.0f;
|
||||
float ba=bn_alpha?bn_alpha[c]:1.0f, bb=bn_beta?bn_beta[c]:0.0f;
|
||||
float w0=ww[0], w1=ww[1], w2=ww[2], w3=ww[3], w4=ww[4], w5=ww[5], w6=ww[6], w7=ww[7], w8=ww[8];
|
||||
|
||||
if (H>=3 && W>=3) {
|
||||
// interior: 1≤y<H-1, 1≤x<W-1 (no boundary checks)
|
||||
for (int y=1; y<H-1; ++y) {
|
||||
float* const oy = oo + y*W + 1;
|
||||
const float* const i0 = ii + (y-1)*W;
|
||||
const float* const i1 = i0 + W;
|
||||
const float* const i2 = i1 + W;
|
||||
for (int x=1; x<W-1; ++x) {
|
||||
float s = cb
|
||||
+ w0*i0[x-1] + w1*i0[x] + w2*i0[x+1]
|
||||
+ w3*i1[x-1] + w4*i1[x] + w5*i1[x+1]
|
||||
+ w6*i2[x-1] + w7*i2[x] + w8*i2[x+1];
|
||||
s = s*ba + bb;
|
||||
if (use_relu && s<0) s = 0;
|
||||
oy[x-1] = s;
|
||||
}
|
||||
}
|
||||
// top row (y=0): boundary check — ky=-1 clamps to y=0
|
||||
{
|
||||
float* const oy = oo;
|
||||
const float* const i0 = ii; // y=0 (ky=-1 and ky=0)
|
||||
const float* const i1 = ii + W; // y=1 (ky=1)
|
||||
for (int x=0; x<W; ++x) {
|
||||
float s = cb;
|
||||
// ky=-1: use i0 (clamped), ky=0: use i0, ky=1: use i1
|
||||
int xl=(x>0)?x-1:x, xr=(x<W-1)?x+1:x;
|
||||
s += w0*i0[xl] + w1*i0[x] + w2*i0[xr] // ky=-1
|
||||
+ w3*i0[xl] + w4*i0[x] + w5*i0[xr] // ky=0
|
||||
+ w6*i1[xl] + w7*i1[x] + w8*i1[xr]; // ky=1
|
||||
s = s*ba + bb;
|
||||
if (use_relu && s<0) s = 0;
|
||||
oy[x] = s;
|
||||
}
|
||||
}
|
||||
// bottom row (y=H-1): boundary check — ky=1 clamps to y=H-1
|
||||
{
|
||||
float* const oy = oo + (H-1)*W;
|
||||
const float* const i0 = ii + (H-2)*W; // y=H-2 (ky=-1)
|
||||
const float* const i1 = ii + (H-1)*W; // y=H-1 (ky=0 and ky=1)
|
||||
for (int x=0; x<W; ++x) {
|
||||
float s = cb;
|
||||
int xl=(x>0)?x-1:x, xr=(x<W-1)?x+1:x;
|
||||
s += w0*i0[xl] + w1*i0[x] + w2*i0[xr] // ky=-1
|
||||
+ w3*i1[xl] + w4*i1[x] + w5*i1[xr] // ky=0
|
||||
+ w6*i1[xl] + w7*i1[x] + w8*i1[xr]; // ky=1
|
||||
s = s*ba + bb;
|
||||
if (use_relu && s<0) s = 0;
|
||||
oy[x] = s;
|
||||
}
|
||||
}
|
||||
// middle rows left border (1≤y<H-1, x=0)
|
||||
for (int y=1; y<H-1; ++y) {
|
||||
const float* const i0 = ii + (y-1)*W;
|
||||
const float* const i1 = i0 + W;
|
||||
const float* const i2 = i1 + W;
|
||||
float s = cb
|
||||
+ w1*i0[0] + w2*i0[1]
|
||||
+ w4*i1[0] + w5*i1[1]
|
||||
+ w7*i2[0] + w8*i2[1];
|
||||
s = s*ba + bb;
|
||||
if (use_relu && s<0) s = 0;
|
||||
oo[y*W] = s;
|
||||
}
|
||||
// middle rows right border (1≤y<H-1, x=W-1)
|
||||
for (int y=1; y<H-1; ++y) {
|
||||
const float* const i0 = ii + (y-1)*W + (W-2);
|
||||
const float* const i1 = i0 + W;
|
||||
const float* const i2 = i1 + W;
|
||||
float s = cb
|
||||
+ w0*i0[0] + w1*i0[1]
|
||||
+ w3*i1[0] + w4*i1[1]
|
||||
+ w6*i2[0] + w7*i2[1];
|
||||
s = s*ba + bb;
|
||||
if (use_relu && s<0) s = 0;
|
||||
oo[y*W+W-1] = s;
|
||||
}
|
||||
} else {
|
||||
// degenerate (small H/W): fallback
|
||||
for (int y=0; y<H; ++y) {
|
||||
for (int x=0; x<W; ++x) {
|
||||
float s = cb;
|
||||
for (int ky=0; ky<3; ++ky) {
|
||||
int iy = y + ky - 1;
|
||||
if (iy<0||iy>=H) continue;
|
||||
for (int kx=0; kx<3; ++kx) {
|
||||
int ix = x + kx - 1;
|
||||
if (ix<0||ix>=W) continue;
|
||||
s += ww[ky*3+kx] * ii[iy*W+ix];
|
||||
}
|
||||
}
|
||||
s = s*ba + bb;
|
||||
if (use_relu && s<0) s = 0;
|
||||
oo[y*W+x] = s;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
return;
|
||||
}
|
||||
|
||||
// 通用路径 — 常数外提 + iy*W 预计算
|
||||
for (int g=0; g<grp; ++g) {
|
||||
int ic_base = g*icpg;
|
||||
for (int oc=0; oc<ocpg; ++oc) {
|
||||
int occ = g*ocpg + oc;
|
||||
float* oo = o + occ*Ho*Wo;
|
||||
float cb = conv_b ? conv_b[occ] : 0.0f;
|
||||
float ba=bn_alpha?bn_alpha[occ]:1.0f, bb=bn_beta?bn_beta[occ]:0.0f;
|
||||
|
||||
for (int y=0; y<Ho; ++y) {
|
||||
int yi0 = y*str;
|
||||
for (int x=0; x<Wo; ++x) {
|
||||
float s = cb;
|
||||
int xi0 = x*str;
|
||||
for (int ic=0; ic<icpg; ++ic) {
|
||||
int icc = ic_base + ic;
|
||||
const float* ww = conv_w + ((occ*icpg+ic)*K*K);
|
||||
const float* ii = in + icc*H*W;
|
||||
for (int ky=0; ky<K; ++ky) {
|
||||
int iy = yi0 + ky - pad;
|
||||
if (iy<0||iy>=H) continue;
|
||||
int iy_off = iy*W;
|
||||
for (int kx=0; kx<K; ++kx) {
|
||||
int ix = xi0 + kx - pad;
|
||||
if (ix<0||ix>=W) continue;
|
||||
s += ww[ky*K+kx] * ii[iy_off + ix];
|
||||
}
|
||||
}
|
||||
}
|
||||
s = s*ba + bb;
|
||||
if (use_relu && s<0) s = 0;
|
||||
oo[y*Wo+x] = s;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// ============================================================
|
||||
// 纯 conv (无 BN/ReLU, cls_head / size_head / skip 的 conv 部分)
|
||||
// ============================================================
|
||||
static void conv2d(float* o, const float* in, const float* w, const float* b,
|
||||
int H, int W, int iC, int oC, int K, int str, int grp) {
|
||||
conv_bn_relu(o, in, w, b, nullptr, nullptr, H, W, iC, oC, K, str, grp, false);
|
||||
}
|
||||
|
||||
// ============================================================
|
||||
// SE 通道注意力 (使用快速 sigmoid)
|
||||
// ============================================================
|
||||
static void se_module(float* x, int C, int N,
|
||||
const float* w1, const float* b1,
|
||||
const float* w2, const float* b2,
|
||||
float* buf) {
|
||||
if (!w1||!w2) return;
|
||||
int mid=C/4;
|
||||
float* pool=buf;
|
||||
|
||||
// AdaptiveAvgPool
|
||||
for (int c=0; c<C; ++c) {
|
||||
float s=0;
|
||||
float* px = x+c*N, *end = px+N;
|
||||
while (px < end) { s += *px++; }
|
||||
pool[c]=s/(float)N;
|
||||
}
|
||||
// fc1: C→mid + ReLU
|
||||
float* fc1=buf+C;
|
||||
for (int o=0; o<mid; ++o) {
|
||||
float s=b1?b1[o]:0;
|
||||
const float* ww=w1+o*C;
|
||||
for (int ic=0; ic<C; ++ic) s+=pool[ic]*ww[ic];
|
||||
fc1[o]=s>0?s:0;
|
||||
}
|
||||
// fc2: mid→C + Sigmoid (fast)
|
||||
float* fc2=buf+C+mid;
|
||||
for (int o=0; o<C; ++o) {
|
||||
float s=b2?b2[o]:0;
|
||||
const float* ww=w2+o*mid;
|
||||
for (int ic=0; ic<mid; ++ic) s+=fc1[ic]*ww[ic];
|
||||
fc2[o]=fast_sigmoid(s);
|
||||
}
|
||||
for (int c=0; c<C; ++c) {
|
||||
float* xc=x+c*N; float sc=fc2[c];
|
||||
for (int i=0; i<N; ++i) xc[i]*=sc;
|
||||
}
|
||||
}
|
||||
|
||||
// ============================================================
|
||||
// SE-ResDW Block v10
|
||||
// ============================================================
|
||||
static void se_block_v10(float* o, const float* in, int iC, int oC, int H, int W, int str, const char* pfx) {
|
||||
int Ho=H/str, Wo=W/str, No=Ho*Wo;
|
||||
char na[128], wr[128], bi[128];
|
||||
|
||||
// dw conv + BN + ReLU (fused)
|
||||
std::snprintf(na,128,"%s.dw.0.weight",pfx);
|
||||
std::snprintf(wr,128,"%s.dw.1._alpha",pfx);
|
||||
std::snprintf(bi,128,"%s.dw.1._beta",pfx);
|
||||
conv_bn_relu(m->dw_out, in, wf(na), nullptr,
|
||||
wf(wr), wf(bi), H, W, iC, iC, 3, str, iC, true);
|
||||
|
||||
// pw conv + BN + ReLU (fused)
|
||||
std::snprintf(na,128,"%s.pw.0.weight",pfx);
|
||||
std::snprintf(wr,128,"%s.pw.1._alpha",pfx);
|
||||
std::snprintf(bi,128,"%s.pw.1._beta",pfx);
|
||||
conv_bn_relu(o, m->dw_out, wf(na), nullptr,
|
||||
wf(wr), wf(bi), Ho, Wo, iC, oC, 1, 1, 1, true);
|
||||
|
||||
// skip (BN fused, no ReLU)
|
||||
bool iden=(str==1 && iC==oC);
|
||||
float* skip=m->dw_out;
|
||||
if (iden) {
|
||||
std::memcpy(skip, in, iC*H*W*4);
|
||||
} else {
|
||||
std::snprintf(na,128,"%s.sk.0.weight",pfx);
|
||||
std::snprintf(wr,128,"%s.sk.1._alpha",pfx);
|
||||
std::snprintf(bi,128,"%s.sk.1._beta",pfx);
|
||||
conv_bn_relu(skip, in, wf(na), nullptr,
|
||||
wf(wr), wf(bi), H, W, iC, oC, 1, str, 1, false);
|
||||
}
|
||||
for (int i=0; i<oC*No; ++i) o[i]+=skip[i];
|
||||
|
||||
// SE
|
||||
std::snprintf(na,128,"%s.se.1.weight",pfx);
|
||||
std::snprintf(bi,128,"%s.se.1.bias",pfx);
|
||||
char se2[128], se2b[128];
|
||||
std::snprintf(se2,128,"%s.se.3.weight",pfx);
|
||||
std::snprintf(se2b,128,"%s.se.3.bias",pfx);
|
||||
se_module(o, oC, No, wf(na), wf(bi), wf(se2), wf(se2b), m->se_buf);
|
||||
|
||||
relu_f(o, oC*No);
|
||||
}
|
||||
|
||||
// ============================================================
|
||||
// 解码 — softmax + NMS (预计算 peak mask)
|
||||
// ============================================================
|
||||
static constexpr int OH=15, OW=20, STRIDE=8, NC=4;
|
||||
static constexpr float RS[NC][2] = {
|
||||
{56.85f,45.39f}, {101.44f,66.53f}, {96.75f,62.86f}, {66.01f,35.49f},
|
||||
};
|
||||
|
||||
static int decode(DetectBoxV10* boxes, int max, const float* th) {
|
||||
int N=OH*OW, cnt=0;
|
||||
|
||||
// 预计算 peak mask: 每个类别做一次 maxpool
|
||||
uint8* mask = m->peak_mask;
|
||||
for (int c=0; c<NC; ++c) {
|
||||
const float* cp = m->cl + c*N;
|
||||
uint8* mp = mask + c*N;
|
||||
for (int gy=0; gy<OH; ++gy) {
|
||||
for (int gx=0; gx<OW; ++gx) {
|
||||
if (cp[gy*OW+gx] < 0) { mp[gy*OW+gx]=0; continue; }
|
||||
bool peak=true;
|
||||
float val=cp[gy*OW+gx];
|
||||
for (int dy=-1; dy<=1&&peak; ++dy)
|
||||
for (int dx=-1; dx<=1&&peak; ++dx) {
|
||||
int ny=gy+dy, nx=gx+dx;
|
||||
if (ny<0||ny>=OH||nx<0||nx>=OW) continue;
|
||||
if (cp[ny*OW+nx] > val) peak=false;
|
||||
}
|
||||
mp[gy*OW+gx] = peak ? 1 : 0;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// softmax + check per cell
|
||||
for (int gy=0; gy<OH && cnt<max; ++gy) {
|
||||
for (int gx=0; gx<OW && cnt<max; ++gx) {
|
||||
int idx=gy*OW+gx;
|
||||
|
||||
// softmax over NC+1 classes (fast exp)
|
||||
float sm[5], mx=-1e9f, sum=0;
|
||||
for (int c=0; c<=NC; ++c) {
|
||||
float v=m->cl[c*N+idx];
|
||||
sm[c]=v; if(v>mx) mx=v;
|
||||
}
|
||||
for (int c=0; c<=NC; ++c) { sm[c]=fast_exp(sm[c]-mx); sum+=sm[c]; }
|
||||
|
||||
// best class (excluding bg=NC), per-class threshold
|
||||
int bc=-1; float bs=0;
|
||||
for (int c=0; c<NC; ++c) {
|
||||
if (!mask[c*N+idx]) continue;
|
||||
float p=sm[c]/sum;
|
||||
if (p>bs && p>=th[c]) { bs=p; bc=c; }
|
||||
}
|
||||
if (bc<0) continue;
|
||||
|
||||
float pw=std::max(1.0f, m->sz[0*N+idx]*RS[bc][0]);
|
||||
float ph=std::max(1.0f, m->sz[1*N+idx]*RS[bc][1]);
|
||||
boxes[cnt].cls=bc; boxes[cnt].conf=bs;
|
||||
boxes[cnt].cx=((float)gx+0.5f)*STRIDE;
|
||||
boxes[cnt].cy=((float)gy+0.5f)*STRIDE;
|
||||
boxes[cnt].w=pw; boxes[cnt].h=ph;
|
||||
cnt++;
|
||||
}
|
||||
}
|
||||
return cnt;
|
||||
}
|
||||
|
||||
// ============================================================
|
||||
// 预处理 LUT: uint8→float normalized
|
||||
// ============================================================
|
||||
static float g_lut[256];
|
||||
static void init_lut() {
|
||||
for (int i=0; i<256; ++i) g_lut[i] = (float)i / 255.0f;
|
||||
}
|
||||
|
||||
// ============================================================
|
||||
// 前向推理
|
||||
// ============================================================
|
||||
static void forward(const uint8* bgr) {
|
||||
float* in = m->preproc;
|
||||
for (int c=0; c<3; ++c) {
|
||||
int sc=2-c; float* ch=in+c*120*160;
|
||||
for (int y=0; y<120; ++y) {
|
||||
const uint8* row=bgr+y*160*3;
|
||||
for (int x=0; x<160; ++x) ch[y*160+x]=g_lut[row[x*3+sc]];
|
||||
}
|
||||
}
|
||||
|
||||
// stem: 3→6, s2 (fused)
|
||||
conv_bn_relu(m->stem, in, wf("s.0.weight"), nullptr,
|
||||
wf("s.1._alpha"), wf("s.1._beta"), 120, 160, 3, 6, 3, 2, 1, true);
|
||||
|
||||
// blocks
|
||||
se_block_v10(m->b1, m->stem, 6, 8, 60, 80, 1, "b1");
|
||||
se_block_v10(m->b2, m->b1, 8, 12, 60, 80, 2, "b2");
|
||||
se_block_v10(m->b3, m->b2, 12, 16, 30, 40, 2, "b3");
|
||||
se_block_v10(m->b4, m->b3, 16, 32, 15, 20, 1, "b4");
|
||||
se_block_v10(m->b5, m->b4, 32, 48, 15, 20, 1, "b5");
|
||||
se_block_v10(m->b6, m->b5, 48, 48, 15, 20, 1, "b6");
|
||||
|
||||
// shared: 48→24, 1×1 (fused)
|
||||
conv_bn_relu(m->sh, m->b6, wf("sh.0.weight"), nullptr,
|
||||
wf("sh.1._alpha"), wf("sh.1._beta"), 15, 20, 48, 24, 1, 1, 1, true);
|
||||
|
||||
// heads (pure conv, no BN)
|
||||
conv2d(m->cl, m->sh, wf("ch.weight"), wf("ch.bias"), 15, 20, 24, 5, 1, 1, 1);
|
||||
conv2d(m->sz, m->sh, wf("sz.weight"), wf("sz.bias"), 15, 20, 24, 2, 1, 1, 1);
|
||||
}
|
||||
|
||||
// ============================================================
|
||||
// 接口
|
||||
// ============================================================
|
||||
static bool g_rdy=false;
|
||||
|
||||
bool model_v10_init(const char* path) {
|
||||
FILE* f=fopen(path,"rb");
|
||||
if(!f) return false;
|
||||
fread(&gn,sizeof(int),1,f);
|
||||
gw=new WT[gn];
|
||||
for (int i=0; i<gn; ++i) {
|
||||
WT& t=gw[i]; int nl; fread(&nl,4,1,f); fread(t.n,1,nl,f); t.n[nl]=0;
|
||||
fread(&t.nd,4,1,f); int tot=1;
|
||||
for (int d=0; d<t.nd; ++d) { fread(&t.s[d],4,1,f); tot*=t.s[d]; }
|
||||
for (int d=t.nd; d<4; ++d) t.s[d]=1;
|
||||
t.d=new float[tot]; fread(t.d,4,tot,f);
|
||||
}
|
||||
fclose(f);
|
||||
bn_precompute();
|
||||
m=new(std::nothrow) M10();
|
||||
if(!m) { delete[] gw; gw=nullptr; return false; }
|
||||
std::memset(m,0,sizeof(M10));
|
||||
g_rdy=true;
|
||||
return true;
|
||||
}
|
||||
|
||||
int model_v10_detect(const uint8* bgr, int w, int h, DetectBoxV10* boxes, int max, const float* thresh) {
|
||||
if(!g_rdy||w!=160||h!=120) return 0;
|
||||
forward(bgr);
|
||||
return decode(boxes,max,thresh);
|
||||
}
|
||||
|
||||
void model_v10_deinit() {
|
||||
if(gw) { for(int i=0; i<gn; ++i) delete[] gw[i].d; delete[] gw; gw=nullptr; }
|
||||
delete m; m=nullptr; g_rdy=false;
|
||||
}
|
||||
|
||||
bool model_v10_ready() { return g_rdy; }
|
||||
@@ -0,0 +1,13 @@
|
||||
#pragma once
|
||||
#include "types_model.hpp"
|
||||
|
||||
struct DetectBoxV10 {
|
||||
int cls; // 0=锥 1=红灯 2=绿灯 3=行人
|
||||
float conf;
|
||||
float cx, cy, w, h;
|
||||
};
|
||||
|
||||
bool model_v10_init(const char* path);
|
||||
int model_v10_detect(const uint8* bgr, int w, int h, DetectBoxV10* boxes, int max, const float* thresh);
|
||||
void model_v10_deinit();
|
||||
bool model_v10_ready();
|
||||
@@ -0,0 +1,3 @@
|
||||
#pragma once
|
||||
#include <cstdint>
|
||||
using uint8 = uint8_t;
|
||||
@@ -0,0 +1,312 @@
|
||||
/*
|
||||
* zebra_detect.cpp — 斑马线检测实现
|
||||
* 算法对齐 gd13.py: 透视变换 → 白色掩码 → Sobel 梯度 → 滑窗判定
|
||||
*/
|
||||
#define ZEBRA_DEBUG 1
|
||||
|
||||
#include "zebra_detect.h"
|
||||
#include <cmath>
|
||||
#include <cstdio>
|
||||
|
||||
using namespace cv;
|
||||
using namespace std;
|
||||
|
||||
#if ZEBRA_DEBUG
|
||||
#define ZLOG(fmt, ...) fprintf(stderr, "[zebra] " fmt "\n", ##__VA_ARGS__)
|
||||
#else
|
||||
#define ZLOG(fmt, ...) ((void)0)
|
||||
#endif
|
||||
|
||||
// ============================================================
|
||||
// 参数 (对齐 gd13.py)
|
||||
// ============================================================
|
||||
static const int PROC_SIZE = 400;
|
||||
static const int WIN_H = 120;
|
||||
static const int WIN_W = 300;
|
||||
static const int WIN_STEP = 20;
|
||||
|
||||
static const float AMP_THRESH = 12.0f;
|
||||
static const int COUNT_THRESH = 150;
|
||||
static const float MIN_VRATIO = 0.45f;
|
||||
static const int MIN_VPIXELS = 300;
|
||||
|
||||
// ============================================================
|
||||
// 透视变换矩阵
|
||||
// ============================================================
|
||||
static Mat get_birdview_transform()
|
||||
{
|
||||
vector<Point2f> src = {
|
||||
{30.f, 380.f},
|
||||
{370.f, 380.f},
|
||||
{320.f, 180.f},
|
||||
{80.f, 180.f}
|
||||
};
|
||||
vector<Point2f> dst = {
|
||||
{50.f, 350.f},
|
||||
{350.f, 350.f},
|
||||
{350.f, 50.f},
|
||||
{50.f, 50.f}
|
||||
};
|
||||
return getPerspectiveTransform(src, dst);
|
||||
}
|
||||
|
||||
// ============================================================
|
||||
// 白色掩码 (HSV)
|
||||
// ============================================================
|
||||
static Mat get_white_zebra_mask(const Mat &bgr)
|
||||
{
|
||||
Mat hsv;
|
||||
cvtColor(bgr, hsv, COLOR_BGR2HSV);
|
||||
|
||||
Mat white_mask;
|
||||
inRange(hsv, Scalar(0, 0, 180), Scalar(180, 30, 255), white_mask);
|
||||
ZLOG(" 白掩码 H:[0,180] S:[0,30] V:[180,255] 白色像素=%d", countNonZero(white_mask));
|
||||
|
||||
Mat brown_mask;
|
||||
inRange(hsv, Scalar(10, 20, 80), Scalar(30, 100, 200), brown_mask);
|
||||
ZLOG(" 棕掩码 H:[10,30] S:[20,100] V:[80,200] 棕色像素=%d", countNonZero(brown_mask));
|
||||
|
||||
Mat not_brown;
|
||||
bitwise_not(brown_mask, not_brown);
|
||||
bitwise_and(white_mask, not_brown, white_mask);
|
||||
ZLOG(" 排除棕色后白色像素=%d", countNonZero(white_mask));
|
||||
|
||||
Mat kernel = getStructuringElement(MORPH_RECT, Size(2, 2));
|
||||
morphologyEx(white_mask, white_mask, MORPH_OPEN, kernel, Point(-1,-1), 1);
|
||||
morphologyEx(white_mask, white_mask, MORPH_CLOSE, kernel, Point(-1,-1), 1);
|
||||
ZLOG(" 开闭运算后白色像素=%d", countNonZero(white_mask));
|
||||
|
||||
return white_mask;
|
||||
}
|
||||
|
||||
// ============================================================
|
||||
// 预处理
|
||||
// ============================================================
|
||||
static Mat preprocess(const Mat &bgr)
|
||||
{
|
||||
Mat white_mask = get_white_zebra_mask(bgr);
|
||||
Mat gray;
|
||||
cvtColor(bgr, gray, COLOR_BGR2GRAY);
|
||||
bitwise_and(gray, gray, gray, white_mask);
|
||||
|
||||
medianBlur(gray, gray, 3);
|
||||
Mat kernel1 = getStructuringElement(MORPH_RECT, Size(3, 3));
|
||||
Mat kernel2 = getStructuringElement(MORPH_RECT, Size(5, 5));
|
||||
morphologyEx(gray, gray, MORPH_OPEN, kernel1, Point(-1,-1), 1);
|
||||
morphologyEx(gray, gray, MORPH_CLOSE, kernel2, Point(-1,-1), 1);
|
||||
|
||||
return gray;
|
||||
}
|
||||
|
||||
// ============================================================
|
||||
// 梯度计算
|
||||
// ============================================================
|
||||
static void compute_gradient(const Mat &gray, Mat &litude, Mat &theta)
|
||||
{
|
||||
Mat sobelx, sobely;
|
||||
Sobel(gray, sobelx, CV_32F, 1, 0, 3);
|
||||
Sobel(gray, sobely, CV_32F, 0, 1, 3);
|
||||
|
||||
phase(sobelx, sobely, theta, true);
|
||||
for (int r = 0; r < theta.rows; ++r)
|
||||
for (int c = 0; c < theta.cols; ++c)
|
||||
theta.at<float>(r, c) = fmod(theta.at<float>(r, c), 180.f);
|
||||
|
||||
magnitude(sobelx, sobely, amplitude);
|
||||
|
||||
// 统计过滤前
|
||||
int before = countNonZero(amplitude);
|
||||
for (int r = 0; r < amplitude.rows; ++r)
|
||||
for (int c = 0; c < amplitude.cols; ++c)
|
||||
if (amplitude.at<float>(r, c) < 10.f)
|
||||
amplitude.at<float>(r, c) = 0.f;
|
||||
|
||||
int after = countNonZero(amplitude);
|
||||
double amp_min, amp_max;
|
||||
minMaxLoc(amplitude, &_min, &_max);
|
||||
ZLOG(" 梯度: 过滤前=%d, 过滤后(amp>10)=%d, 幅值范围=[%.1f, %.1f]",
|
||||
before, after, amp_min, amp_max);
|
||||
}
|
||||
|
||||
// ============================================================
|
||||
// 单窗口判定 (无 vector 分配, 避免 OOM)
|
||||
// ============================================================
|
||||
static bool is_zebra_window(const Mat &_win, const Mat &ang_win, int win_top)
|
||||
{
|
||||
ZLOG("--- 窗口 top=%d [%dx%d] ---", win_top, amp_win.cols, amp_win.rows);
|
||||
|
||||
int total = 0; // amplitude > AMP_THRESH 的像素总数
|
||||
int hist[4] = {0}; // 水平边缘直方图 [80-85,85-90,90-95,95-100)
|
||||
int v_count = 0; // 垂直边缘计数 [0-10]∪[170-180]
|
||||
|
||||
// 单次遍历: 同时统计 total, hist, v_count
|
||||
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 <= AMP_THRESH)
|
||||
continue;
|
||||
|
||||
++total;
|
||||
float a = ang_win.at<float>(r, c);
|
||||
|
||||
if (a >= 80.f && a < 100.f)
|
||||
{
|
||||
int bin = (int)(a - 80.f) / 5;
|
||||
if (bin >= 0 && bin < 4)
|
||||
hist[bin]++;
|
||||
}
|
||||
|
||||
if (a <= 10.f || a >= 170.f)
|
||||
++v_count;
|
||||
}
|
||||
}
|
||||
|
||||
ZLOG(" 梯度像素(amplitude>%.0f)=%d", AMP_THRESH, total);
|
||||
if (total == 0)
|
||||
{
|
||||
ZLOG(" -> 丢弃: 无有效梯度像素");
|
||||
return false;
|
||||
}
|
||||
|
||||
// 找峰值 bin
|
||||
int peak = 0, peak_bin = 0;
|
||||
for (int i = 0; i < 4; ++i)
|
||||
if (hist[i] > peak) { peak = hist[i]; peak_bin = i; }
|
||||
|
||||
// 二次遍历: 统计峰值方向的像素数 (需要知道具体区间)
|
||||
int h_count = 0;
|
||||
float low_a = 80.f + peak_bin * 5.f;
|
||||
float high_a = low_a + 5.f;
|
||||
for (int r = 0; r < amp_win.rows; ++r)
|
||||
{
|
||||
for (int c = 0; c < amp_win.cols; ++c)
|
||||
{
|
||||
if (amp_win.at<float>(r, c) <= AMP_THRESH)
|
||||
continue;
|
||||
float a = ang_win.at<float>(r, c);
|
||||
if (a >= low_a && a <= high_a)
|
||||
++h_count;
|
||||
}
|
||||
}
|
||||
|
||||
ZLOG(" 水平边缘[80-100): hist=[%d,%d,%d,%d], 峰值bin=%d(%.0f-%.0f度), "
|
||||
"峰值像素=%d, 阈值=%d",
|
||||
hist[0], hist[1], hist[2], hist[3],
|
||||
peak_bin, low_a, high_a, h_count, COUNT_THRESH);
|
||||
|
||||
if (h_count < COUNT_THRESH)
|
||||
{
|
||||
ZLOG(" -> 丢弃: 水平边缘像素不足 (%d < %d)", h_count, COUNT_THRESH);
|
||||
return false;
|
||||
}
|
||||
|
||||
float v_ratio = (float)v_count / (float)(total + 1e-6);
|
||||
ZLOG(" 垂直边缘[0-10|170-180]: 像素=%d, 占比=%.3f, 要求: >=%d && 占比>=%.2f",
|
||||
v_count, v_ratio, MIN_VPIXELS, MIN_VRATIO);
|
||||
|
||||
if (v_count < MIN_VPIXELS)
|
||||
{
|
||||
ZLOG(" -> 丢弃: 垂直像素不足 (%d < %d)", v_count, MIN_VPIXELS);
|
||||
return false;
|
||||
}
|
||||
if (v_ratio < MIN_VRATIO)
|
||||
{
|
||||
ZLOG(" -> 丢弃: 垂直占比不足 (%.3f < %.2f)", v_ratio, MIN_VRATIO);
|
||||
return false;
|
||||
}
|
||||
|
||||
ZLOG(" -> 判定: 斑马线窗口!");
|
||||
return true;
|
||||
}
|
||||
|
||||
// ============================================================
|
||||
// detect_zebra_crossing — 主检测函数
|
||||
// ============================================================
|
||||
ZebraResult detect_zebra_crossing(const Mat &bgr_frame)
|
||||
{
|
||||
ZebraResult result;
|
||||
result.detected = false;
|
||||
result.distance_px = -1;
|
||||
|
||||
ZLOG("========================================");
|
||||
ZLOG("新帧开始: 输入尺寸=%dx%d", bgr_frame.cols, bgr_frame.rows);
|
||||
|
||||
if (bgr_frame.empty())
|
||||
{
|
||||
ZLOG("错误: 输入帧为空");
|
||||
return result;
|
||||
}
|
||||
|
||||
static Mat M = get_birdview_transform();
|
||||
static Mat proc, birdview, roi, gray, amplitude, theta;
|
||||
|
||||
// 1. 缩放 → 透视变换 → 裁剪 ROI(顶部30px) → 恢复 400×400
|
||||
resize(bgr_frame, proc, Size(PROC_SIZE, PROC_SIZE));
|
||||
ZLOG("步骤1-缩放: %dx%d -> %dx%d", bgr_frame.cols, bgr_frame.rows,
|
||||
PROC_SIZE, PROC_SIZE);
|
||||
|
||||
warpPerspective(proc, birdview, M, Size(PROC_SIZE, PROC_SIZE));
|
||||
roi = birdview(Rect(0, 30, PROC_SIZE, PROC_SIZE - 30));
|
||||
resize(roi, proc, Size(PROC_SIZE, PROC_SIZE));
|
||||
ZLOG("步骤1-透视+ROI: 裁剪顶部30px, 鸟瞰输出=%dx%d", proc.cols, proc.rows);
|
||||
|
||||
// 2. 预处理 + 梯度
|
||||
ZLOG("步骤2-预处理开始");
|
||||
gray = preprocess(proc);
|
||||
ZLOG("步骤2-预处理完成, 灰度非零像素=%d", countNonZero(gray));
|
||||
|
||||
ZLOG("步骤3-梯度计算开始");
|
||||
compute_gradient(gray, amplitude, theta);
|
||||
|
||||
int amp_nz = countNonZero(amplitude);
|
||||
ZLOG("步骤3-梯度完成, 总非零梯度像素=%d", amp_nz);
|
||||
|
||||
if (amp_nz == 0)
|
||||
{
|
||||
ZLOG("结果: 无梯度 → 未检测到斑马线");
|
||||
return result;
|
||||
}
|
||||
|
||||
// 3. 滑动窗口
|
||||
ZLOG("步骤4-滑动窗口: 窗口=%dx%d, 步长=%d, 遍历范围=[0,%d]",
|
||||
WIN_W, WIN_H, WIN_STEP, proc.rows - WIN_H);
|
||||
|
||||
int z_ymin = proc.rows;
|
||||
int z_ymax = 0;
|
||||
int valid = 0;
|
||||
int total_wins = 0;
|
||||
|
||||
for (int top = 0; top <= proc.rows - WIN_H; top += WIN_STEP)
|
||||
{
|
||||
++total_wins;
|
||||
Rect win(0, top, WIN_W, WIN_H);
|
||||
if (is_zebra_window(amplitude(win), theta(win), top))
|
||||
{
|
||||
++valid;
|
||||
if (top < z_ymin) z_ymin = top;
|
||||
if (top + WIN_H > z_ymax) z_ymax = top + WIN_H;
|
||||
}
|
||||
}
|
||||
|
||||
// 4. 判定
|
||||
int span = z_ymax - z_ymin;
|
||||
ZLOG("步骤5-汇总: 总窗口=%d, 有效=%d, "
|
||||
"斑马线区域: top=%d~%d, 跨度=%d, 判定阈值=%d",
|
||||
total_wins, valid, z_ymin, z_ymax, span, WIN_H);
|
||||
|
||||
if (span > WIN_H)
|
||||
{
|
||||
result.detected = true;
|
||||
result.distance_px = PROC_SIZE - z_ymax;
|
||||
ZLOG("结果: 检测到斑马线! 下沿距底部=%dpx (ymax=%d)",
|
||||
result.distance_px, z_ymax);
|
||||
}
|
||||
else
|
||||
{
|
||||
ZLOG("结果: 未检测到斑马线 (跨度%d <= 阈值%d)", span, WIN_H);
|
||||
}
|
||||
|
||||
return result;
|
||||
}
|
||||
@@ -0,0 +1,3 @@
|
||||
# zebra_demo - 斑马线检测测试
|
||||
add_executable(zebra_demo zebra_demo.cpp)
|
||||
target_link_libraries(zebra_demo common_lib ${OpenCV_LIBS})
|
||||
Binary file not shown.
|
After Width: | Height: | Size: 894 KiB |
@@ -0,0 +1,729 @@
|
||||
/*
|
||||
* 斑马线实时检测 — 龙芯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;
|
||||
}
|
||||
Reference in New Issue
Block a user