Fix vision pipeline: search-first IPM, IMU non-blocking, LCD inverse IPM, servo direction, model WIP

strategy/deviation.cpp

@@ -1,8 +1,73 @@
 #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;
@@ -44,8 +109,8 @@ float calc_deviation(const EdgeLines& lines)
     int mid_ref = IMAGE_WIDTH / 2;
 
     int total = IPM_ROW_COUNT;
-    int zone_far  = total / 5;        // 远: 上20%
-    int zone_mid  = total * 3 / 5;    // 中: 中间40%
+    int zone_far  = total / 5;
+    int zone_mid  = total * 3 / 5;
 
     for (int y = 0; y < total; ++y)
     {