73 lines
1.9 KiB
C++
73 lines
1.9 KiB
C++
#include "deviation.hpp"
|
|
#include <algorithm>
|
|
#include <cmath>
|
|
#include <cstring>
|
|
|
|
void fit_midline(EdgeLines& lines)
|
|
{
|
|
static EdgeLines prev;
|
|
static bool has_prev = false;
|
|
|
|
if (!has_prev)
|
|
{
|
|
std::memcpy(&prev, &lines, sizeof(EdgeLines));
|
|
has_prev = true;
|
|
return;
|
|
}
|
|
|
|
for (int y = 0; y < IPM_ROW_COUNT; ++y)
|
|
{
|
|
if (lines.left[y] == 0 || lines.right[y] == 0) continue;
|
|
|
|
if (std::abs(static_cast<int>(lines.mid[y]) - static_cast<int>(prev.mid[y])) > 20)
|
|
lines.mid[y] = prev.mid[y];
|
|
}
|
|
|
|
for (int y = 0; y < IPM_ROW_COUNT; ++y)
|
|
{
|
|
if (y >= 2 && y < IPM_ROW_COUNT - 2 && lines.mid[y] > 0)
|
|
{
|
|
int sum = 0, cnt = 0;
|
|
for (int dy = -2; dy <= 2; ++dy)
|
|
if (lines.mid[y + dy] > 0) { sum += lines.mid[y + dy]; ++cnt; }
|
|
if (cnt > 0) lines.mid[y] = sum / cnt;
|
|
}
|
|
}
|
|
|
|
std::memcpy(&prev, &lines, sizeof(EdgeLines));
|
|
}
|
|
|
|
float calc_deviation(const EdgeLines& lines)
|
|
{
|
|
float dev_near = 0, dev_mid = 0, dev_far = 0;
|
|
float cnt_near = 0, cnt_mid = 0, cnt_far = 0;
|
|
int mid_ref = IMAGE_WIDTH / 2;
|
|
|
|
int total = IPM_ROW_COUNT;
|
|
int zone_far = total / 5; // 远: 上20%
|
|
int zone_mid = total * 3 / 5; // 中: 中间40%
|
|
|
|
for (int y = 0; y < total; ++y)
|
|
{
|
|
if (lines.mid[y] == 0) continue;
|
|
float d = static_cast<float>(lines.mid[y]) - mid_ref;
|
|
|
|
if (y < zone_far)
|
|
{ dev_far += d; cnt_far += 1; }
|
|
else if (y < zone_mid)
|
|
{ dev_mid += d; cnt_mid += 1; }
|
|
else
|
|
{ dev_near += d; cnt_near += 1; }
|
|
}
|
|
|
|
if (cnt_far < 3) cnt_far = 3;
|
|
if (cnt_mid < 3) cnt_mid = 3;
|
|
if (cnt_near < 3) cnt_near = 3;
|
|
|
|
float dev = (dev_far / cnt_far) * 0.50f +
|
|
(dev_mid / cnt_mid) * 0.30f +
|
|
(dev_near / cnt_near) * 0.20f;
|
|
|
|
return std::clamp(dev / mid_ref, -1.0f, 1.0f);
|
|
}
|