117 lines
4.0 KiB
C++
117 lines
4.0 KiB
C++
#include "preprocess.hpp"
|
||
#include <algorithm>
|
||
#include <cmath>
|
||
|
||
uint8 g_ipm_image[IMAGE_HEIGHT][IMAGE_WIDTH] = {{0}};
|
||
uint8 g_valid_l_bound[IMAGE_HEIGHT] = {0};
|
||
uint8 g_valid_r_bound[IMAGE_HEIGHT] = {0};
|
||
|
||
static constexpr int PROC_W = IMAGE_WIDTH / 2; // 80 — 与demo对齐
|
||
static constexpr int PROC_H = IMAGE_HEIGHT / 2; // 60
|
||
|
||
// ============================================================
|
||
// IPM 4 点标定 — 80×60 相机视角 ↔ 80×60 俯视输出
|
||
//
|
||
// 地面实况: 矩形赛道 40cm 宽 × 50cm 长, 近边距摄像头 55cm
|
||
// 摄像头垂直投影 ≈ 车头后 5cm, 车头距矩形 ≈ 50cm
|
||
// ============================================================
|
||
static constexpr float CAL_NEAR_Y_MM = 550.0f; // 矩形近边 Y (mm)
|
||
static constexpr float CAL_FAR_Y_MM = 1050.0f; // 矩形远边 Y (mm)
|
||
static constexpr float CAL_HALF_W_MM = 200.0f; // 矩形半宽 (mm)
|
||
|
||
static bool g_ipm_enabled = true; // 启用 IPM
|
||
cv::Mat g_ipm_matrix; // 3×3 透视变换矩阵 (extern)
|
||
|
||
void preprocess_init() {}
|
||
|
||
static void init_ipm_calibration()
|
||
{
|
||
// 四点: (相机 80×60 像素) → 标注的矩形角
|
||
std::vector<cv::Point2f> src = {
|
||
{19.f, 17.f}, // A 近左
|
||
{57.f, 15.f}, // B 近右
|
||
{27.f, 8.f}, // C 远左
|
||
{49.f, 7.f} // D 远右
|
||
};
|
||
|
||
// 目标: 俯视图中也保持矩形 (同宽), 近边 row=55, 远边 row=20
|
||
// mm/pixel ≈ 14.3, 半宽 200mm → 14px
|
||
const float cx = PROC_W / 2.0f; // 40
|
||
const float hw = 14.0f; // 半宽像素
|
||
std::vector<cv::Point2f> dst = {
|
||
{cx - hw, 55.f}, // A' 近左
|
||
{cx + hw, 55.f}, // B' 近右
|
||
{cx - hw, 20.f}, // C' 远左
|
||
{cx + hw, 20.f} // D' 远右
|
||
};
|
||
|
||
// getPerspectiveTransform(dst, src): 输出(俯视) → 输入(相机)
|
||
g_ipm_matrix = cv::getPerspectiveTransform(dst, src);
|
||
|
||
printf("[IPM] 4点标定完成\n");
|
||
printf(" 近边 Y=%.0f mm 远边 Y=%.0f mm 半宽=%.0f mm\n",
|
||
CAL_NEAR_Y_MM, CAL_FAR_Y_MM, CAL_HALF_W_MM);
|
||
}
|
||
|
||
static void update_bounds(const cv::Mat& binary);
|
||
|
||
static cv::Mat track_binarize(const cv::Mat& bgr)
|
||
{
|
||
cv::Mat hsv, thresh_h, thresh_s, result;
|
||
cv::cvtColor(bgr, hsv, cv::COLOR_BGR2HSV);
|
||
|
||
std::vector<cv::Mat> ch;
|
||
cv::split(hsv, ch);
|
||
|
||
cv::threshold(ch[0], thresh_h, 0, 255, cv::THRESH_BINARY_INV | cv::THRESH_OTSU);
|
||
cv::threshold(ch[1], thresh_s, 0, 255, cv::THRESH_BINARY_INV | cv::THRESH_OTSU);
|
||
cv::bitwise_or(thresh_h, thresh_s, result);
|
||
|
||
return result;
|
||
}
|
||
|
||
static cv::Mat flood_fill_track(const cv::Mat& binary)
|
||
{
|
||
cv::Mat kernel = cv::getStructuringElement(cv::MORPH_CROSS, cv::Size(2, 2));
|
||
cv::Mat opened;
|
||
cv::morphologyEx(binary, opened, cv::MORPH_OPEN, kernel);
|
||
|
||
cv::Mat mask = cv::Mat::zeros(PROC_H + 2, PROC_W + 2, CV_8UC1);
|
||
cv::Point seed(PROC_W / 2, PROC_H - 10);
|
||
cv::circle(opened, seed, 10, cv::Scalar(255), -1);
|
||
|
||
cv::floodFill(opened, mask, seed, cv::Scalar(128),
|
||
nullptr,
|
||
cv::Scalar(20), cv::Scalar(20), 8);
|
||
|
||
cv::Mat result = cv::Mat::zeros(PROC_H, PROC_W, CV_8UC1);
|
||
mask(cv::Rect(1, 1, PROC_W, PROC_H)).copyTo(result);
|
||
return result;
|
||
}
|
||
|
||
void preprocess_run(const cv::Mat& bgr_frame)
|
||
{
|
||
cv::Mat small;
|
||
cv::resize(bgr_frame, small, cv::Size(PROC_W, PROC_H));
|
||
|
||
cv::Mat bin = track_binarize(small);
|
||
cv::Mat track = flood_fill_track(bin);
|
||
cv::Mat full;
|
||
cv::resize(track, full, cv::Size(IMAGE_WIDTH, IMAGE_HEIGHT), 0, 0, cv::INTER_NEAREST);
|
||
|
||
update_bounds(full);
|
||
std::memcpy(g_ipm_image, full.data, IMAGE_WIDTH * IMAGE_HEIGHT);
|
||
}
|
||
|
||
static void update_bounds(const cv::Mat& binary)
|
||
{
|
||
for (int y = 0; y < IMAGE_HEIGHT; ++y)
|
||
{
|
||
int l = 0, r = IMAGE_WIDTH - 1;
|
||
while (l < IMAGE_WIDTH && binary.at<uint8>(y, l) == 0) ++l;
|
||
while (r >= 0 && binary.at<uint8>(y, r) == 0) --r;
|
||
g_valid_l_bound[y] = static_cast<uint8>(std::max(0, l));
|
||
g_valid_r_bound[y] = static_cast<uint8>(std::min(IMAGE_WIDTH - 1, r));
|
||
}
|
||
}
|