138 lines
4.3 KiB
C++
138 lines
4.3 KiB
C++
#include "deviation.hpp"
|
|
#include "vision/preprocess.hpp"
|
|
#include <algorithm>
|
|
#include <cmath>
|
|
#include <cstring>
|
|
|
|
// ============================================================
|
|
// IPM 坐标矫正 — 相机透视 → 鸟瞰
|
|
// 搜线在原始相机视角完成 (准确), 这里只对坐标做透视变换
|
|
// ============================================================
|
|
void ipm_correct_lines(EdgeLines& lines)
|
|
{
|
|
if (g_ipm_matrix.empty() || g_ipm_matrix.type() != CV_64F) return;
|
|
|
|
cv::Mat H = g_ipm_matrix.inv(); // 相机→鸟瞰 (原矩阵 map 是鸟瞰→相机)
|
|
double* m = (double*)H.ptr<double>();
|
|
|
|
uint8 new_left[IPM_ROW_COUNT] = {0};
|
|
uint8 new_right[IPM_ROW_COUNT] = {0};
|
|
|
|
for (int y = 0; y < IPM_ROW_COUNT; ++y)
|
|
{
|
|
if (lines.left[y] > 0)
|
|
{
|
|
double sx = lines.left[y] * 0.5; // 160→80 缩放
|
|
double sy = y * 0.5;
|
|
double w = m[6]*sx + m[7]*sy + m[8];
|
|
if (std::abs(w) > 1e-9) {
|
|
double bx = (m[0]*sx + m[1]*sy + m[2]) / w;
|
|
double by = (m[3]*sx + m[4]*sy + m[5]) / w;
|
|
int nx = (int)(bx * 2.0 + 0.5); // 80→160 缩放
|
|
int ny = (int)(by * 2.0 + 0.5);
|
|
if (nx >= 0 && nx < 256 && ny >= 0 && ny < IPM_ROW_COUNT)
|
|
new_left[ny] = (uint8)nx;
|
|
}
|
|
}
|
|
if (lines.right[y] > 0)
|
|
{
|
|
double sx = lines.right[y] * 0.5;
|
|
double sy = y * 0.5;
|
|
double w = m[6]*sx + m[7]*sy + m[8];
|
|
if (std::abs(w) > 1e-9) {
|
|
double bx = (m[0]*sx + m[1]*sy + m[2]) / w;
|
|
double by = (m[3]*sx + m[4]*sy + m[5]) / w;
|
|
int nx = (int)(bx * 2.0 + 0.5);
|
|
int ny = (int)(by * 2.0 + 0.5);
|
|
if (nx >= 0 && nx < 256 && ny >= 0 && ny < IPM_ROW_COUNT)
|
|
new_right[ny] = (uint8)nx;
|
|
}
|
|
}
|
|
}
|
|
|
|
// 插值填充缺失行
|
|
for (int y = 1; y < IPM_ROW_COUNT - 1; ++y)
|
|
{
|
|
if (new_left[y] == 0 && new_left[y-1] > 0 && new_left[y+1] > 0)
|
|
new_left[y] = (new_left[y-1] + new_left[y+1]) / 2;
|
|
if (new_right[y] == 0 && new_right[y-1] > 0 && new_right[y+1] > 0)
|
|
new_right[y] = (new_right[y-1] + new_right[y+1]) / 2;
|
|
}
|
|
|
|
for (int y = 0; y < IPM_ROW_COUNT; ++y)
|
|
{
|
|
lines.left[y] = new_left[y];
|
|
lines.right[y] = new_right[y];
|
|
if (new_left[y] > 0 && new_right[y] > 0)
|
|
lines.mid[y] = (new_left[y] + new_right[y]) / 2;
|
|
}
|
|
}
|
|
|
|
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;
|
|
int zone_mid = total * 3 / 5;
|
|
|
|
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);
|
|
}
|