123 lines
3.8 KiB
C++
123 lines
3.8 KiB
C++
#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;
|
|
}
|