diff --git a/.vscode/settings.json b/.vscode/settings.json new file mode 100644 index 0000000..4b5a294 --- /dev/null +++ b/.vscode/settings.json @@ -0,0 +1,4 @@ +{ + "python-envs.defaultEnvManager": "ms-python.python:conda", + "python-envs.defaultPackageManager": "ms-python.python:conda" +} \ No newline at end of file diff --git a/CMakeLists.txt b/CMakeLists.txt index 5c90dd5..8f964c3 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -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) \ No newline at end of file +add_subdirectory(image_test) +# add_subdirectory(zebra_demo) +add_subdirectory(gd13_demo) +add_subdirectory(screenshot_demo) \ No newline at end of file diff --git a/build_lsx.sh b/build_lsx.sh new file mode 100644 index 0000000..91864a4 --- /dev/null +++ b/build_lsx.sh @@ -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" diff --git a/ctl.sh b/ctl.sh index eefbcf1..4d0ecab 100644 --- a/ctl.sh +++ b/ctl.sh @@ -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 diff --git a/gd13_demo/CMakeLists.txt b/gd13_demo/CMakeLists.txt new file mode 100644 index 0000000..7e2fb30 --- /dev/null +++ b/gd13_demo/CMakeLists.txt @@ -0,0 +1,2 @@ +add_executable(gd13_demo gd13_demo.cpp) +target_link_libraries(gd13_demo common_lib ${OpenCV_LIBS}) diff --git a/gd13_demo/gd13_demo.cpp b/gd13_demo/gd13_demo.cpp new file mode 100644 index 0000000..db1ea91 --- /dev/null +++ b/gd13_demo/gd13_demo.cpp @@ -0,0 +1,103 @@ +/* + * gd13_demo — 斑马线检测演示 (调用 zebra_detect.h) + */ +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +#include "zebra_detect.h" + +using namespace cv; +using namespace std; + +static atomic 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(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; +} diff --git a/lib/camera.h b/lib/camera.h index d1d9e8d..4bfcbdf 100644 --- a/lib/camera.h +++ b/lib/camera.h @@ -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 #include #include +#include #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 \ No newline at end of file +#endif diff --git a/lib/control.h b/lib/control.h index 6d393e2..8a998fe 100644 --- a/lib/control.h +++ b/lib/control.h @@ -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 \ No newline at end of file +extern GPIO mortorEN; +extern MotorController *motorController[2]; + +#endif diff --git a/lib/global.h b/lib/global.h index c675db8..03a3aa3 100644 --- a/lib/global.h +++ b/lib/global.h @@ -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"; diff --git a/lib/zebra_detect.h b/lib/zebra_detect.h new file mode 100644 index 0000000..226b30e --- /dev/null +++ b/lib/zebra_detect.h @@ -0,0 +1,16 @@ +/* + * zebra_detect.h — 斑马线检测函数 + * ================================ + * 输入图片 → 返回是否有人行横道 + 距离 + */ +#pragma once + +#include + +struct ZebraResult +{ + bool detected; // true = 检测到斑马线 + int distance_px; // 斑马线下沿距图像下边框的像素距离, -1 = 未检测到 +}; + +ZebraResult detect_zebra_crossing(const cv::Mat &bgr_frame); diff --git a/main/CMakeLists.txt b/main/CMakeLists.txt index e636278..745ce0c 100644 --- a/main/CMakeLists.txt +++ b/main/CMakeLists.txt @@ -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}) \ No newline at end of file +target_link_libraries(smartcar_demo1 common_lib ${OpenCV_LIBS}) \ No newline at end of file diff --git a/main/main.cpp b/main/main.cpp index 4051aa2..5544582 100644 --- a/main/main.cpp +++ b/main/main.cpp @@ -4,14 +4,12 @@ #include #include #include +#include +#include #include "global.h" #include "camera.h" #include "control.h" -#include "Timer.h" -#include "serial.h" -#include "encoder.h" -#include "video.h" std::atomic 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(videoWidth) / videoHeight; - // // 根据屏幕大小计算缩放后的宽度和高度 - // if (screenWidth / static_cast(screenHeight) > aspectRatio) - // { - // // 屏幕更宽，以高度为基准 - // newHeight = screenHeight; - // newWidth = static_cast(newHeight * aspectRatio); - // } - // else - // { - // // 屏幕更高，以宽度为基准 - // newWidth = screenWidth; - // newHeight = static_cast(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; -} \ No newline at end of file +} diff --git a/nanodetv10.bin b/nanodetv10.bin new file mode 100644 index 0000000..ea67541 Binary files /dev/null and b/nanodetv10.bin differ diff --git a/screenshot_demo/CMakeLists.txt b/screenshot_demo/CMakeLists.txt new file mode 100644 index 0000000..cac56a5 --- /dev/null +++ b/screenshot_demo/CMakeLists.txt @@ -0,0 +1,2 @@ +add_executable(screenshot_demo screenshot_demo.cpp) +target_link_libraries(screenshot_demo common_lib ${OpenCV_LIBS}) diff --git a/screenshot_demo/screenshot_demo.cpp b/screenshot_demo/screenshot_demo.cpp new file mode 100644 index 0000000..515fd96 --- /dev/null +++ b/screenshot_demo/screenshot_demo.cpp @@ -0,0 +1,111 @@ +/* + * screenshot_demo — 摄像头截图保存 + * ====================================== + * 每次运行截一张图, 保存到 ./screenshot/image_N.jpg + * 用法: + * ./screenshot_demo 截一张图 + LCD 显示 + * ./screenshot_demo --no-display 无 LCD + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +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(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; +} diff --git a/src/camera.cpp b/src/camera.cpp index 1d2d0c3..a4d2f74 100644 --- a/src/camera.cpp +++ b/src/camera.cpp @@ -1,284 +1,376 @@ #include "camera.h" +#include "model_v10.hpp" + +#include +#include +#include +#include +#include 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(screenWidth) / cameraWidth; double heightRatio = static_cast(screenHeight) / cameraHeight; - double scale = std::min(widthRatio, heightRatio); // 选择较小的比例，确保图像不超出屏幕 - + double scale = std::min(widthRatio, heightRatio); newWidth = static_cast(cameraWidth * scale); newHeight = static_cast(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(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(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(left_line[y] * calc_scale); - scaledRightX = static_cast(right_line[y] * calc_scale); - scaledMidX = static_cast(mid_line[y] * calc_scale); - scaledY = static_cast(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(left_line[y]*calc_scale), sRX=static_cast(right_line[y]*calc_scale); + int sMX=static_cast(mid_line[y]*calc_scale), sY=static_cast(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; } diff --git a/src/control.cpp b/src/control.cpp index 43ee195..7072152 100644 --- a/src/control.cpp +++ b/src/control.cpp @@ -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); } diff --git a/src/image_cv.cpp b/src/image_cv.cpp index e3e5628..ee04ce1 100644 --- a/src/image_cv.cpp +++ b/src/image_cv.cpp @@ -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 left_line; // 左边缘列号数组 -std::vector right_line; // 右边缘列号数组 -std::vector mid_line; // 中线列号数组 -std::vector left_line_filtered; // 中线列号数组 -std::vector right_line_filtered; // 中线列号数组 -std::vector 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 left_line; // 左边缘列号 (逐行) +std::vector right_line; // 右边缘列号 (逐行) +std::vector mid_line; // 中线列号 = (left+right)/2 +std::vector left_line_filtered; // 左边缘 EMA 滤波结果 +std::vector right_line_filtered; // 右边缘 EMA 滤波结果 +std::vector 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 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(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(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; } } -} \ No newline at end of file +} diff --git a/src/model_v10.cpp b/src/model_v10.cpp new file mode 100644 index 0000000..72f4a3c --- /dev/null +++ b/src/model_v10.cpp @@ -0,0 +1,568 @@ +/* + * NanoDetHeat v10: 3.2M MACs, 4-class + * 优化: BN折叠 + conv_bn_relu融合 + 快速exp + 边界修正 + */ +#include "model_v10.hpp" +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +// ============================================================ +// 快速 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 base_idx; + for (int i=0; i0 && sz <= 256) { + base_idx[base] = i; + } + } + + std::vector 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=3 && W>=3) { + // interior: 1≤y0)?x-1:x, xr=(x0)?x-1:x, xr=(x=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=H) continue; + int iy_off = iy*W; + for (int kx=0; kx=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; c0?s:0; + } + // fc2: mid→C + Sigmoid (fast) + float* fc2=buf+C+mid; + for (int o=0; odw_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; ise_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; ccl + c*N; + uint8* mp = mask + c*N; + for (int gy=0; gy=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; gycl[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; cbs && 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 +using uint8 = uint8_t; diff --git a/src/zebra_detect.cpp b/src/zebra_detect.cpp new file mode 100644 index 0000000..966c2c1 --- /dev/null +++ b/src/zebra_detect.cpp @@ -0,0 +1,312 @@ +/* + * zebra_detect.cpp — 斑马线检测实现 + * 算法对齐 gd13.py: 透视变换 → 白色掩码 → Sobel 梯度 → 滑窗判定 + */ +#define ZEBRA_DEBUG 1 + +#include "zebra_detect.h" +#include +#include + +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 src = { + {30.f, 380.f}, + {370.f, 380.f}, + {320.f, 180.f}, + {80.f, 180.f} + }; + vector 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(r, c) = fmod(theta.at(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(r, c) < 10.f) + amplitude.at(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(r, c); + if (amp <= AMP_THRESH) + continue; + + ++total; + float a = ang_win.at(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(r, c) <= AMP_THRESH) + continue; + float a = ang_win.at(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; +} diff --git a/zebra_demo/CMakeLists.txt b/zebra_demo/CMakeLists.txt new file mode 100644 index 0000000..abcd2dc --- /dev/null +++ b/zebra_demo/CMakeLists.txt @@ -0,0 +1,3 @@ +# zebra_demo - 斑马线检测测试 +add_executable(zebra_demo zebra_demo.cpp) +target_link_libraries(zebra_demo common_lib ${OpenCV_LIBS}) diff --git a/zebra_demo/test.jpg b/zebra_demo/test.jpg new file mode 100644 index 0000000..7d0f35e Binary files /dev/null and b/zebra_demo/test.jpg differ diff --git a/zebra_demo/zebra_demo.cpp b/zebra_demo/zebra_demo.cpp new file mode 100644 index 0000000..af0320c --- /dev/null +++ b/zebra_demo/zebra_demo.cpp @@ -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 +#include +#include +#include +#include +#include +#include + +// Linux 特定: 帧缓冲屏 + mmap + ioctl +#include +#include +#include +#include +#include + +// ============================================================ +// 可调参数 — 与 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(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 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(r, c); + float gy = dy.at(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(CV_PI); + + // 弱边缘置零 (不参与后续方向统计) + amplitude.at(r, c) = (amp >= GRAD_THRESHOLD) ? amp : 0.0f; + theta.at(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(r, c); + if (amp <= 0.0f) + continue; + + float ang = ang_win.at(r, c); + if (ang < 0.0f || ang >= HIST_ANGLE_MAX) + continue; + + int bin = static_cast(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(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; +}