Initial commit: SmartCar Framework v0.1 — 龙芯2K0300智能车开发框架\n\n- HAL: GPIO/PWM/Encoder/Framebuffer 驱动\n- Control: PID/IMU/Motor/Servo 控制\n- Vision: HSV双Otsu→4点标定IPM→洪泛填充→逐行搜线\n- Strategy: 三区前瞻偏差+速度策略\n- Debug: 文件热调参+LCD预览+cv截帧\n- Scheduler: 5ms timerfd+epoll 中央调度器
This commit is contained in:
56
vision/camera.cpp
Normal file
56
vision/camera.cpp
Normal file
@@ -0,0 +1,56 @@
|
||||
#include "camera.hpp"
|
||||
#include <atomic>
|
||||
#include <mutex>
|
||||
#include <thread>
|
||||
|
||||
static cv::VideoCapture* cap = nullptr;
|
||||
static cv::Mat pubframe;
|
||||
static std::mutex frame_mutex;
|
||||
static std::atomic<bool> stream_running{false};
|
||||
|
||||
bool camera_init()
|
||||
{
|
||||
cap = new cv::VideoCapture(0);
|
||||
if (!cap->isOpened()) return false;
|
||||
|
||||
cap->set(cv::CAP_PROP_FOURCC, cv::VideoWriter::fourcc('M', 'J', 'P', 'G'));
|
||||
cap->set(cv::CAP_PROP_FRAME_WIDTH, CAMERA_WIDTH);
|
||||
cap->set(cv::CAP_PROP_FRAME_HEIGHT, CAMERA_HEIGHT);
|
||||
cap->set(cv::CAP_PROP_AUTO_EXPOSURE, -1);
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
void camera_deinit()
|
||||
{
|
||||
if (cap) { cap->release(); delete cap; cap = nullptr; }
|
||||
}
|
||||
|
||||
cv::Mat camera_capture()
|
||||
{
|
||||
std::lock_guard<std::mutex> lock(frame_mutex);
|
||||
return pubframe.clone();
|
||||
}
|
||||
|
||||
cv::Mat camera_resize(const cv::Mat& src)
|
||||
{
|
||||
cv::Mat resized;
|
||||
cv::resize(src, resized, cv::Size(IMAGE_WIDTH, IMAGE_HEIGHT));
|
||||
return resized;
|
||||
}
|
||||
|
||||
void camera_stream_start()
|
||||
{
|
||||
stream_running.store(true);
|
||||
std::thread([]() {
|
||||
cv::Mat tmp;
|
||||
while (stream_running.load())
|
||||
{
|
||||
if (cap && cap->read(tmp))
|
||||
{
|
||||
std::lock_guard<std::mutex> lock(frame_mutex);
|
||||
pubframe = tmp.clone();
|
||||
}
|
||||
}
|
||||
}).detach();
|
||||
}
|
||||
9
vision/camera.hpp
Normal file
9
vision/camera.hpp
Normal file
@@ -0,0 +1,9 @@
|
||||
#pragma once
|
||||
#include <opencv2/opencv.hpp>
|
||||
#include "types.hpp"
|
||||
|
||||
bool camera_init();
|
||||
void camera_deinit();
|
||||
cv::Mat camera_capture(); // 返回最新的 BGR 320x240 帧
|
||||
cv::Mat camera_resize(const cv::Mat& src); // 缩放到 160x120
|
||||
void camera_stream_start(); // 后台线程持续采集
|
||||
188
vision/element.cpp
Normal file
188
vision/element.cpp
Normal file
@@ -0,0 +1,188 @@
|
||||
#include "element.hpp"
|
||||
#include <algorithm>
|
||||
#include <cmath>
|
||||
#include <cstring>
|
||||
|
||||
ElementMachine g_elm;
|
||||
|
||||
namespace {
|
||||
static constexpr uint8 k_cross_confirm_frames = 3;
|
||||
static constexpr uint8 k_cross_exit_frames = 5;
|
||||
static constexpr uint8 k_lock_min_frames = 5;
|
||||
static constexpr float k_width_widen_ratio = 1.6f;
|
||||
static constexpr float k_width_normal_ratio = 1.3f;
|
||||
|
||||
static int bottom_width(const SearchResult& sr)
|
||||
{
|
||||
int y = IMAGE_HEIGHT - 1;
|
||||
if (sr.lines.left[y] > 0 && sr.lines.right[y] > 0)
|
||||
return sr.lines.right[y] - sr.lines.left[y];
|
||||
return -1;
|
||||
}
|
||||
|
||||
static uint8 line_lost_state(const SearchResult& sr)
|
||||
{
|
||||
uint8 st = 0;
|
||||
if (!sr.lines.left_valid) st |= 1;
|
||||
if (!sr.lines.right_valid) st |= 2;
|
||||
return st;
|
||||
}
|
||||
|
||||
static void calibrate_width(const SearchResult& sr)
|
||||
{
|
||||
int w = bottom_width(sr);
|
||||
if (w > 10 && w < IMAGE_WIDTH - 10)
|
||||
{
|
||||
g_elm.normal_width = (g_elm.normal_width * g_elm.calib_cnt + w) / (g_elm.calib_cnt + 1);
|
||||
if (g_elm.calib_cnt < 50) ++g_elm.calib_cnt;
|
||||
}
|
||||
}
|
||||
|
||||
static bool is_cross_entry(const SearchResult& sr)
|
||||
{
|
||||
if (g_elm.calib_cnt < 20) return false;
|
||||
|
||||
int w = bottom_width(sr);
|
||||
if (w < 0) return false;
|
||||
|
||||
bool too_wide = (w > g_elm.normal_width * k_width_widen_ratio);
|
||||
uint8 lost = line_lost_state(sr);
|
||||
bool both_lost_upper = (lost == 3);
|
||||
|
||||
if (too_wide || both_lost_upper) g_elm.confirm_cnt++;
|
||||
else g_elm.confirm_cnt = 0;
|
||||
|
||||
return g_elm.confirm_cnt >= k_cross_confirm_frames;
|
||||
}
|
||||
|
||||
static bool is_cross_exit(const SearchResult& sr)
|
||||
{
|
||||
int w = bottom_width(sr);
|
||||
if (w < 0) { g_elm.exit_cnt = 0; return false; }
|
||||
|
||||
bool width_normal = (w < g_elm.normal_width * k_width_normal_ratio);
|
||||
bool bounds_ok = (sr.lines.left_valid && sr.lines.right_valid);
|
||||
|
||||
if (width_normal && bounds_ok) g_elm.exit_cnt++;
|
||||
else g_elm.exit_cnt = 0;
|
||||
|
||||
return g_elm.exit_cnt >= k_cross_exit_frames;
|
||||
}
|
||||
|
||||
static void cross_patch_midline(SearchResult& sr)
|
||||
{
|
||||
int bottom_y = IMAGE_HEIGHT - 1;
|
||||
int mid_x = sr.lines.mid[bottom_y];
|
||||
if (mid_x <= 0) return;
|
||||
|
||||
int lx = mid_x - static_cast<int>(g_elm.normal_width / 2);
|
||||
int rx = mid_x + static_cast<int>(g_elm.normal_width / 2);
|
||||
lx = std::max(0, lx);
|
||||
rx = std::min(IMAGE_WIDTH - 1, rx);
|
||||
|
||||
for (int y = 0; y < IMAGE_HEIGHT; ++y)
|
||||
{
|
||||
if (sr.lines.left[y] == 0 && sr.lines.right[y] == 0)
|
||||
{
|
||||
sr.lines.left[y] = static_cast<uint8>(lx);
|
||||
sr.lines.right[y] = static_cast<uint8>(rx);
|
||||
sr.lines.mid[y] = static_cast<uint8>(mid_x);
|
||||
}
|
||||
}
|
||||
sr.lines.left_valid = 1;
|
||||
sr.lines.right_valid = 1;
|
||||
}
|
||||
}
|
||||
|
||||
void element_init()
|
||||
{
|
||||
std::memset(&g_elm, 0, sizeof(g_elm));
|
||||
g_elm.state = TRACK_STRAIGHT;
|
||||
g_elm.last_state = TRACK_STRAIGHT;
|
||||
g_elm.normal_width = 50.0f;
|
||||
}
|
||||
|
||||
bool element_is_cross()
|
||||
{
|
||||
return g_elm.state == TRACK_CROSS;
|
||||
}
|
||||
|
||||
void element_recognize(SearchResult& sr, TrackInfo& info)
|
||||
{
|
||||
calibrate_width(sr);
|
||||
|
||||
g_elm.last_state = g_elm.state;
|
||||
++g_elm.in_state_frames;
|
||||
|
||||
if (g_elm.lock_cnt > 0)
|
||||
{
|
||||
--g_elm.lock_cnt;
|
||||
info.scene = g_elm.state;
|
||||
return;
|
||||
}
|
||||
|
||||
switch (g_elm.state)
|
||||
{
|
||||
case TRACK_STRAIGHT:
|
||||
case TRACK_GENTLE_CURVE:
|
||||
if (is_cross_entry(sr))
|
||||
{
|
||||
int bottom_y = IMAGE_HEIGHT - 1;
|
||||
g_elm.entry_deviation = (sr.lines.mid[bottom_y] > 0)
|
||||
? (sr.lines.mid[bottom_y] - IMAGE_WIDTH / 2) / static_cast<float>(IMAGE_WIDTH / 2)
|
||||
: 0.0f;
|
||||
|
||||
g_elm.state = TRACK_CROSS;
|
||||
g_elm.lock_cnt = k_lock_min_frames;
|
||||
g_elm.in_state_frames = 0;
|
||||
g_elm.confirm_cnt = 0;
|
||||
g_elm.exit_cnt = 0;
|
||||
info.scene = TRACK_CROSS;
|
||||
}
|
||||
else
|
||||
{
|
||||
info.scene = sr.lines.left_valid || sr.lines.right_valid
|
||||
? TRACK_STRAIGHT : TRACK_LOST_LINE;
|
||||
}
|
||||
info.line_valid = (info.scene != TRACK_LOST_LINE);
|
||||
break;
|
||||
|
||||
case TRACK_CROSS:
|
||||
cross_patch_midline(sr);
|
||||
|
||||
if (is_cross_exit(sr))
|
||||
{
|
||||
g_elm.state = TRACK_STRAIGHT;
|
||||
g_elm.lock_cnt = 0;
|
||||
g_elm.in_state_frames = 0;
|
||||
g_elm.confirm_cnt = 0;
|
||||
g_elm.exit_cnt = 0;
|
||||
info.scene = TRACK_STRAIGHT;
|
||||
}
|
||||
else
|
||||
{
|
||||
info.scene = TRACK_CROSS;
|
||||
}
|
||||
info.line_valid = true;
|
||||
break;
|
||||
|
||||
case TRACK_LOST_LINE:
|
||||
g_elm.confirm_cnt = 0;
|
||||
if (sr.lines.left_valid && sr.lines.right_valid)
|
||||
{
|
||||
g_elm.state = TRACK_STRAIGHT;
|
||||
info.scene = TRACK_STRAIGHT;
|
||||
}
|
||||
else
|
||||
{
|
||||
info.scene = TRACK_LOST_LINE;
|
||||
}
|
||||
info.line_valid = (info.scene != TRACK_LOST_LINE);
|
||||
break;
|
||||
|
||||
default:
|
||||
info.scene = TRACK_STRAIGHT;
|
||||
info.line_valid = true;
|
||||
break;
|
||||
}
|
||||
}
|
||||
23
vision/element.hpp
Normal file
23
vision/element.hpp
Normal file
@@ -0,0 +1,23 @@
|
||||
#pragma once
|
||||
#include "types.hpp"
|
||||
#include "search.hpp"
|
||||
|
||||
struct ElementMachine {
|
||||
TrackScene state = TRACK_STRAIGHT;
|
||||
TrackScene last_state = TRACK_STRAIGHT;
|
||||
|
||||
uint8 lock_cnt = 0;
|
||||
uint8 confirm_cnt = 0;
|
||||
uint8 exit_cnt = 0;
|
||||
uint16 in_state_frames= 0;
|
||||
|
||||
float normal_width = 0.0f;
|
||||
int calib_cnt = 0;
|
||||
float entry_deviation= 0.0f;
|
||||
};
|
||||
|
||||
extern ElementMachine g_elm;
|
||||
|
||||
void element_init();
|
||||
void element_recognize(SearchResult& sr, TrackInfo& info);
|
||||
bool element_is_cross();
|
||||
131
vision/preprocess.cpp
Normal file
131
vision/preprocess.cpp
Normal file
@@ -0,0 +1,131 @@
|
||||
#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
|
||||
static cv::Mat g_ipm_matrix; // 3×3 透视变换矩阵
|
||||
|
||||
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);
|
||||
|
||||
// IPM 透视校正 (首次调用时计算矩阵)
|
||||
if (g_ipm_enabled)
|
||||
{
|
||||
if (g_ipm_matrix.empty())
|
||||
init_ipm_calibration();
|
||||
|
||||
cv::Mat ipm_out;
|
||||
cv::warpPerspective(bin, ipm_out, g_ipm_matrix,
|
||||
cv::Size(PROC_W, PROC_H),
|
||||
cv::INTER_LINEAR,
|
||||
cv::BORDER_CONSTANT, cv::Scalar(0));
|
||||
bin = ipm_out;
|
||||
}
|
||||
|
||||
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));
|
||||
}
|
||||
}
|
||||
10
vision/preprocess.hpp
Normal file
10
vision/preprocess.hpp
Normal file
@@ -0,0 +1,10 @@
|
||||
#pragma once
|
||||
#include <opencv2/opencv.hpp>
|
||||
#include "types.hpp"
|
||||
|
||||
void preprocess_init();
|
||||
void preprocess_run(const cv::Mat& bgr_frame); // 全彩 HSV 二值化 + IPM
|
||||
|
||||
extern uint8 g_ipm_image[IMAGE_HEIGHT][IMAGE_WIDTH];
|
||||
extern uint8 g_valid_l_bound[IMAGE_HEIGHT];
|
||||
extern uint8 g_valid_r_bound[IMAGE_HEIGHT];
|
||||
122
vision/search.cpp
Normal file
122
vision/search.cpp
Normal file
@@ -0,0 +1,122 @@
|
||||
#include "search.hpp"
|
||||
#include "preprocess.hpp"
|
||||
#include <algorithm>
|
||||
#include <cstring>
|
||||
|
||||
void search_init(SearchResult& r)
|
||||
{
|
||||
std::memset(&r, 0, sizeof(r));
|
||||
}
|
||||
|
||||
void search_run(SearchResult& r)
|
||||
{
|
||||
search_init(r);
|
||||
|
||||
int left_raw[IPM_ROW_COUNT] = {0};
|
||||
int right_raw[IPM_ROW_COUNT] = {0};
|
||||
int mid_raw[IPM_ROW_COUNT] = {0};
|
||||
|
||||
// ============================================================
|
||||
// 逐行最长白段搜索 (与 demo image_main() 完全一致)
|
||||
// ============================================================
|
||||
for (int y = 0; y < IMAGE_HEIGHT; ++y)
|
||||
{
|
||||
int best_start = -1, best_end = -1, best_len = 0;
|
||||
int cur_start = -1, cur_len = 0;
|
||||
|
||||
for (int x = 0; x < IMAGE_WIDTH; ++x)
|
||||
{
|
||||
if (g_ipm_image[y][x])
|
||||
{
|
||||
if (cur_len == 0) { cur_start = x; cur_len = 1; }
|
||||
else cur_len++;
|
||||
if (cur_len >= best_len)
|
||||
{
|
||||
best_len = cur_len;
|
||||
best_start = cur_start;
|
||||
best_end = x;
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
cur_len = 0;
|
||||
cur_start = -1;
|
||||
}
|
||||
}
|
||||
|
||||
if (best_len > 0)
|
||||
{
|
||||
left_raw[y] = best_start;
|
||||
right_raw[y] = best_end;
|
||||
}
|
||||
}
|
||||
|
||||
// ============================================================
|
||||
// 丢线补点 + 中线计算 (从底向上, 复用 demo 逻辑)
|
||||
// ============================================================
|
||||
for (int y = IMAGE_HEIGHT - 2; y >= 0; --y)
|
||||
{
|
||||
if (left_raw[y] == 0 && right_raw[y] == 0)
|
||||
{
|
||||
mid_raw[y] = mid_raw[y + 1];
|
||||
if (mid_raw[y] > IMAGE_WIDTH / 2)
|
||||
{
|
||||
right_raw[y] = IMAGE_WIDTH - 1;
|
||||
left_raw[y] = mid_raw[y + 1];
|
||||
}
|
||||
else
|
||||
{
|
||||
left_raw[y] = 0;
|
||||
right_raw[y] = mid_raw[y + 1];
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
mid_raw[y] = (left_raw[y] + right_raw[y]) / 2;
|
||||
}
|
||||
}
|
||||
if (left_raw[IMAGE_HEIGHT - 1] > 0 || right_raw[IMAGE_HEIGHT - 1] > 0)
|
||||
mid_raw[IMAGE_HEIGHT - 1] = (left_raw[IMAGE_HEIGHT - 1] + right_raw[IMAGE_HEIGHT - 1]) / 2;
|
||||
|
||||
// ============================================================
|
||||
// EMA 滤波 (从底向上, a=0.4, 与 demo 一致)
|
||||
// ============================================================
|
||||
float left_f[IPM_ROW_COUNT] = {0};
|
||||
float right_f[IPM_ROW_COUNT] = {0};
|
||||
float mid_f[IPM_ROW_COUNT] = {0};
|
||||
constexpr float a = 0.4f;
|
||||
int bot = IMAGE_HEIGHT - 1;
|
||||
|
||||
for (int y = bot; y >= 0; --y)
|
||||
{
|
||||
if (y == bot)
|
||||
{
|
||||
left_f[y] = static_cast<float>(left_raw[y]);
|
||||
right_f[y] = static_cast<float>(right_raw[y]);
|
||||
mid_f[y] = (left_f[y] + right_f[y]) * 0.5f;
|
||||
}
|
||||
else
|
||||
{
|
||||
left_f[y] = a * left_raw[y] + (1.0f - a) * left_f[y + 1];
|
||||
right_f[y] = a * right_raw[y] + (1.0f - a) * right_f[y + 1];
|
||||
mid_f[y] = (left_f[y] + right_f[y]) * 0.5f;
|
||||
}
|
||||
|
||||
int lv = static_cast<int>(left_f[y] + 0.5f);
|
||||
int rv = static_cast<int>(right_f[y] + 0.5f);
|
||||
int mv = static_cast<int>(mid_f[y] + 0.5f);
|
||||
|
||||
r.lines.left[y] = static_cast<uint8>(std::clamp(lv, 0, 255));
|
||||
r.lines.right[y] = static_cast<uint8>(std::clamp(rv, 0, 255));
|
||||
r.lines.mid[y] = static_cast<uint8>(std::clamp(mv, 0, 255));
|
||||
}
|
||||
|
||||
int left_cnt = 0, right_cnt = 0;
|
||||
for (int y = 0; y < IPM_ROW_COUNT; ++y)
|
||||
{
|
||||
if (r.lines.left[y] > 0) ++left_cnt;
|
||||
if (r.lines.right[y] > 0) ++right_cnt;
|
||||
}
|
||||
r.lines.left_valid = (left_cnt > 10) ? 1 : 0;
|
||||
r.lines.right_valid = (right_cnt > 10) ? 1 : 0;
|
||||
}
|
||||
9
vision/search.hpp
Normal file
9
vision/search.hpp
Normal file
@@ -0,0 +1,9 @@
|
||||
#pragma once
|
||||
#include "types.hpp"
|
||||
|
||||
struct SearchResult {
|
||||
EdgeLines lines;
|
||||
};
|
||||
|
||||
void search_init(SearchResult& r);
|
||||
void search_run(SearchResult& r);
|
||||
Reference in New Issue
Block a user