摄像头测试代码

在main.c文件中img_processing();函数里pts_left和pts_right两个数组储存边线坐标
2023-12-17 16:11:35 +08:00
parent eaa7cc0eea
commit ce31d900ee
23 changed files with 1261 additions and 45 deletions
--- a/app/gl_tracking.c
+++ b/app/gl_tracking.c
@@ -0,0 +1,120 @@
+#include "zf_common_headfile.h"
+#include "gl_headfile.h"
+
+float (*mid_track)[2];
+int32_t mid_track_count;
+float near_angle;
+float pure_angle;
+float (*rpts)[2];
+int rpts_num;
+
+
+
+void tracking(){
+
+    if (pts_resample_left_count < pts_resample_right_count / 2 && pts_resample_left_count < 50) {
+            track_type = TRACK_RIGHT;
+        } else if (pts_resample_right_count < pts_resample_left_count / 2 && pts_resample_right_count < 58) {
+            track_type = TRACK_LEFT;
+        } else if (pts_resample_left_count < 20 && pts_resample_right_count > pts_resample_left_count) {
+            track_type = TRACK_RIGHT;
+        } else if (pts_resample_right_count < 20 && pts_resample_left_count > pts_resample_right_count) {
+            track_type = TRACK_LEFT;
+        }
+
+}
+
+
+
+void ElementJudge() {
+  CheckGarage();
+  if (garage_type == GARAGE_NONE) {
+    CheckCross();
+    if (cross_type == CROSS_NONE) {
+      CheckCircle();
+    }
+  }
+}
+
+void ElementRun() {
+  if (garage_type != GARAGE_NONE) {
+    RunGarage();
+  }
+
+  else if (cross_type != CROSS_NONE) {
+    RunCross();
+  }
+
+  else if (circle_type != CIRCLE_NONE) {
+    RunCircle();
+  }
+}
+
+void MidLineTrack() {
+  if (cross_type == CROSS_IN) {
+    if (track_type == TRACK_LEFT) {
+    }
+  } 
+  else {
+    if (track_type == TRACK_LEFT) {
+      mid_track = mid_left;
+      mid_track_count = mid_left_count;
+    } else {
+      mid_track = mid_right;
+      mid_track_count = mid_right_count;
+    }
+  }
+
+  // 车轮对应点(纯跟踪起始点)
+  float cx = UndistInverseMapW[(int) (IMAGE_H * 0.90f)][78];
+  float cy = UndistInverseMapH[(int) (IMAGE_H * 0.90f)][78];
+
+  // 找最近点(起始点中线归一化)
+  float min_dist = 1e10;
+
+  int begin_id = -1;
+  for (int i = 0; i < rpts_num; i++) {
+      float dx = rpts[i][1] - cx;
+      float dy = rpts[i][0] - cy;
+      float dist = sqrt(dx * dx + dy * dy);
+      if (dist < min_dist) {
+          min_dist = dist;
+          begin_id = i;
+      }
+  }
+
+  if (begin_id >= 0 && rpts_num - begin_id >= 3){
+    // 归一化中线
+    rpts[begin_id][0] = cy;
+    rpts[begin_id][1] = cx;
+    rptsn_num = sizeof(rptsn) / sizeof(rptsn[0]);
+    GetLinesResample(rpts + begin_id, rpts_num - begin_id, rptsn, &rptsn_num, RESAMPLEDIST * PIXPERMETER );
+
+    // 远预锚点位置
+    int aim_idx = clip(round(aim_distance / RESAMPLEDIST), 0, rptsn_num - 1);
+
+    //近锚点位置
+    int aim_idx_near = clip(round(0.25 / RESAMPLEDIST), 0, rptsn_num - 1);
+
+    // 计算远锚点偏差值
+    float dx = rptsn[aim_idx][1] - cx;
+    float dy = cy - rptsn[aim_idx][0] + 0.2 * PIXPERMETER;
+    float dn = sqrt(dx * dx + dy * dy);
+    //float error = -atan2f(dx, dy) * 180 / PI;
+
+    // 计算近锚点偏差值
+    float dx_near = rptsn[aim_idx_near][1] - cx;
+    float dy_near = cy - rptsn[aim_idx_near][0] + 0.2 * PIXPERMETER;
+    float dn_near = sqrt(dx_near * dx_near + dy_near *    dy_near);
+    //float error_near = -atan2f(dx_near, dy_near) * 180 / PI;
+
+    //考虑近点
+    near_angle = -atanf(PIXPERMETER * 2 * 0.2 * dx_near / dn_near / dn_near) / PI * 180 ;
+    //考虑远点
+    pure_angle = -atanf(PIXPERMETER * 2 * 0.2 * dx/ dn / dn) / PI * 180 ;  
+
+    
+
+  }
+}
+