From 2db1937c897ca16f5d11d6b96bf69513827e4286 Mon Sep 17 00:00:00 2001 From: Glz <354338225@qq.com> Date: Tue, 13 Aug 2024 21:41:10 +0800 Subject: [PATCH] =?UTF-8?q?=E6=97=A5=E5=B8=B8=E6=9B=B4=E6=96=B0?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- app/gl_cross.c | 222 ++++++++++++++++++++++++++++++------------------- app/gl_cross.h | 1 + 2 files changed, 137 insertions(+), 86 deletions(-) diff --git a/app/gl_cross.c b/app/gl_cross.c index fdf4d28..cceb35f 100644 --- a/app/gl_cross.c +++ b/app/gl_cross.c @@ -5,134 +5,189 @@ enum cross_type_e cross_type = CROSS_NONE; bool far_Lpt0_found, far_Lpt1_found; int32_t Both_Boder_None_Cross; int far_Lpt0_rpts0s_id, far_Lpt1_rpts1s_id; +int cross_easy = 0; +int cross_open = 1; -void CheckCross() { +void CheckCross() +{ bool Xfound = Lpt0_found && Lpt1_found; - if (cross_type == CROSS_NONE && Xfound){ - cross_type = CROSS_BEGIN; + if (cross_open == 0) { + if (cross_type == CROSS_NONE && Xfound) { + cross_type = CROSS_BEGIN; + } + } + if (cross_open == 1) { + if (cross_type == CROSS_NONE && Xfound && cross_easy == 0) { + cross_type = CROSS_BEGIN; + } + if (cross_type == CROSS_NONE && cross_easy == 1 && (Lpt0_found || Lpt1_found)) { + cross_type = CROSS_BEGIN; + } } } -void RunCross() { +void RunCross() +{ bool Xfound = Lpt0_found && Lpt1_found; - //检测到十字，先按照近线走 - if (cross_type == CROSS_BEGIN) { - if (Lpt0_found) { - mid_left_count = pts_resample_left_count = Lpt0_rpts0s_id; - track_type = TRACK_LEFT; - } - else if(Lpt1_found) { - mid_right_count = pts_resample_right_count = Lpt1_rpts1s_id; - track_type = TRACK_RIGHT; - } - - //近角点过少，进入远线控制 - if ((Xfound && (Lpt0_rpts0s_id < 0.2 / RESAMPLEDIST && Lpt1_rpts1s_id < 0.2 / RESAMPLEDIST)) || (pts_resample_left_count <20 && pts_resample_right_count<20)) { - cross_type = CROSS_IN; - } - - } - //远线控制进十字,begin_y渐变靠近防丢线 - if (cross_type == CROSS_IN) { - //寻远线,算法与近线相同 - cross_farline(); - - if (pts_resample_left_count < 5 && pts_resample_right_count < 5) { - Both_Boder_None_Cross++; + // 检测到十字，先按照近线走 + if (cross_easy == 0) { + if (cross_type == CROSS_BEGIN) { + if (Lpt0_found) { + mid_left_count = pts_resample_left_count = Lpt0_rpts0s_id; + track_type = TRACK_LEFT; + } else if (Lpt1_found) { + mid_right_count = pts_resample_right_count = Lpt1_rpts1s_id; + track_type = TRACK_RIGHT; } - if (Both_Boder_None_Cross > 2 && pts_resample_left_count > 20 && pts_resample_right_count > 20) { - cross_type = CROSS_NONE; - Both_Boder_None_Cross = 0; - } - if (far_Lpt1_found) { track_type = TRACK_RIGHT; } - else if (far_Lpt0_found) { track_type = TRACK_LEFT; } - else if (Both_Boder_None_Cross > 0 && pts_resample_right_count < 5) { track_type = TRACK_RIGHT; } - else if (Both_Boder_None_Cross > 0 && pts_resample_left_count < 5) { track_type = TRACK_LEFT; } + // 近角点过少，进入远线控制 + if ((Xfound && (Lpt0_rpts0s_id < 0.2 / RESAMPLEDIST && Lpt1_rpts1s_id < 0.2 / RESAMPLEDIST)) || (pts_resample_left_count < 20 && pts_resample_right_count < 20)) { + cross_type = CROSS_IN; + } + } + // 远线控制进十字,begin_y渐变靠近防丢线 + if (cross_type == CROSS_IN) { + // 寻远线,算法与近线相同 + cross_farline(); + + if (pts_resample_left_count < 5 && pts_resample_right_count < 5) { + Both_Boder_None_Cross++; + } + if (Both_Boder_None_Cross > 2 && pts_resample_left_count > 20 && pts_resample_right_count > 20) { + cross_type = CROSS_NONE; + Both_Boder_None_Cross = 0; + cross_easy = 1; + } + if (far_Lpt1_found) { + track_type = TRACK_RIGHT; + } else if (far_Lpt0_found) { + track_type = TRACK_LEFT; + } else if (Both_Boder_None_Cross > 0 && pts_resample_right_count < 5) { + track_type = TRACK_RIGHT; + } else if (Both_Boder_None_Cross > 0 && pts_resample_left_count < 5) { + track_type = TRACK_LEFT; + } + } + } + + if (cross_easy == 1) { + if (cross_type == CROSS_BEGIN) { + if (Lpt0_found) { + mid_left_count = pts_resample_left_count = Lpt0_rpts0s_id; + track_type = TRACK_LEFT; + } else if (Lpt1_found) { + mid_right_count = pts_resample_right_count = Lpt1_rpts1s_id; + track_type = TRACK_RIGHT; + } + + // 近角点过少，进入远线控制 + if ((Xfound && (Lpt0_rpts0s_id < 0.2 / RESAMPLEDIST && Lpt1_rpts1s_id < 0.2 / RESAMPLEDIST)) || (pts_resample_left_count < 20 && pts_resample_right_count < 20)) { + cross_type = CROSS_IN; + } + } + // 远线控制进十字,begin_y渐变靠近防丢线 + if (cross_type == CROSS_IN) { + // 寻远线,算法与近线相同 + cross_farline(); + + if (pts_resample_left_count < 5 && pts_resample_right_count < 5) { + Both_Boder_None_Cross++; + } + if (Both_Boder_None_Cross > 2 && pts_resample_left_count > 20 && pts_resample_right_count > 20) { + cross_type = CROSS_NONE; + Both_Boder_None_Cross = 0; + cross_easy = 0; + } + if (far_Lpt1_found) { + track_type = TRACK_RIGHT; + } else if (far_Lpt0_found) { + track_type = TRACK_LEFT; + } else if (Both_Boder_None_Cross > 0 && pts_resample_right_count < 5) { + track_type = TRACK_RIGHT; + } else if (Both_Boder_None_Cross > 0 && pts_resample_left_count < 5) { + track_type = TRACK_LEFT; + } + } } } - -void cross_farline(){ +void cross_farline() +{ int cross_width = 4; int far_y1 = 0, far_y2 = 0; int far_x1 = 0, far_x2 = 0; - bool white_found = false; + bool white_found = false; pts_far_left_count = sizeof(pts_far_left) / sizeof(pts_far_left[0]); int w1 = IMAGE_W / 2 - BEGINW_R, h1 = BEGINH_L; - for(;w1>cross_width*2; w1--) - { - if(GET_PIX_1C(mt9v03x_image_copy[0], h1, w1 - 1) < FIX_BINTHRESHOLD) { + for (; w1 > cross_width * 2; w1--) { + if (GET_PIX_1C(mt9v03x_image_copy[0], h1, w1 - 1) < FIX_BINTHRESHOLD) { far_x1 = w1 - cross_width; break; - } + } } /*如果一行全为白色没写*/ - if (far_x1 <= 4) - { + if (far_x1 <= 4) { far_x1 = 4; } for (; h1 > 0; h1--) { - //先黑后白，先找white + // 先黑后白，先找white if (GET_PIX_1C(mt9v03x_image_copy[0], h1, far_x1) >= FIX_BINTHRESHOLD) { white_found = true; } if (GET_PIX_1C(mt9v03x_image_copy[0], h1 - 1, far_x1) < FIX_BINTHRESHOLD && white_found) { far_y1 = h1; break; } } - if (GET_PIX_1C(mt9v03x_image_copy[0], far_y1, far_x1) >= FIX_BINTHRESHOLD){ + if (GET_PIX_1C(mt9v03x_image_copy[0], far_y1, far_x1) >= FIX_BINTHRESHOLD) { SearchLineAdaptive_Left(mt9v03x_image_copy[0], SELFADAPT_KERNELSIZE, SELFADAPT_OFFSET, far_y1, far_x1, pts_far_left, &pts_far_left_count); - } - else pts_far_left_count = 0; + } else + pts_far_left_count = 0; int w2 = IMAGE_W / 2 + BEGINW_L, h2 = BEGINH_R; - white_found = false; + white_found = false; pts_far_right_count = sizeof(pts_far_right) / sizeof(pts_far_right[0]); - for(;w2 < IMAGE_W - cross_width*2; w2++) - { - if(GET_PIX_1C(mt9v03x_image_copy[0], h2, w2 + 1) < FIX_BINTHRESHOLD) { + for (; w2 < IMAGE_W - cross_width * 2; w2++) { + if (GET_PIX_1C(mt9v03x_image_copy[0], h2, w2 + 1) < FIX_BINTHRESHOLD) { far_x2 = w2 + cross_width; break; - } + } } /*如果一行全为白色没写*/ - if (far_x2 >= 136) - { + if (far_x2 >= 136) { far_x2 = 136; } for (; h2 > 0; h2--) { - //先黑后白，先找white + // 先黑后白，先找white if (GET_PIX_1C(mt9v03x_image_copy[0], h2, far_x2) >= FIX_BINTHRESHOLD) { white_found = true; } if (GET_PIX_1C(mt9v03x_image_copy[0], h2 - 1, far_x2) < FIX_BINTHRESHOLD && white_found) { far_y2 = h2; break; } } - if (GET_PIX_1C(mt9v03x_image_copy[0], far_y2, far_x2) >= FIX_BINTHRESHOLD){ + if (GET_PIX_1C(mt9v03x_image_copy[0], far_y2, far_x2) >= FIX_BINTHRESHOLD) { SearchLineAdaptive_Right(mt9v03x_image_copy[0], SELFADAPT_KERNELSIZE, SELFADAPT_OFFSET, far_y2, far_x2, pts_far_right, &pts_far_right_count); - } - else pts_far_right_count = 0; + } else + pts_far_right_count = 0; - //透视变换 + // 透视变换 for (int i = 0; i < pts_far_left_count; i++) { - pts_far_inv_l[i][1] = fclip(InverseMapW[pts_far_left[i][0]][pts_far_left[i][1]],0,IMAGE_W); - pts_far_inv_l[i][0] = fclip(InverseMapH[pts_far_left[i][0]][pts_far_left[i][1]],0,IMAGE_H); + pts_far_inv_l[i][1] = fclip(InverseMapW[pts_far_left[i][0]][pts_far_left[i][1]], 0, IMAGE_W); + pts_far_inv_l[i][0] = fclip(InverseMapH[pts_far_left[i][0]][pts_far_left[i][1]], 0, IMAGE_H); } pts_far_inv_l_count = pts_far_left_count; for (int i = 0; i < pts_far_right_count; i++) { - pts_far_inv_r[i][1] = fclip(InverseMapW[pts_far_right[i][0]][pts_far_right[i][1]],0,IMAGE_W); - pts_far_inv_r[i][0] = fclip(InverseMapH[pts_far_right[i][0]][pts_far_right[i][1]],0,IMAGE_H); + pts_far_inv_r[i][1] = fclip(InverseMapW[pts_far_right[i][0]][pts_far_right[i][1]], 0, IMAGE_W); + pts_far_inv_r[i][0] = fclip(InverseMapH[pts_far_right[i][0]][pts_far_right[i][1]], 0, IMAGE_H); } pts_far_inv_r_count = pts_far_right_count; // 边线滤波 - GetLinesFilter(pts_far_inv_l, pts_far_inv_l_count, pts_far_filter_l, (int) round(FILTER_KERNELSIZE)); + GetLinesFilter(pts_far_inv_l, pts_far_inv_l_count, pts_far_filter_l, (int)round(FILTER_KERNELSIZE)); pts_far_filter_l_count = pts_far_inv_l_count; - GetLinesFilter(pts_far_inv_r, pts_far_inv_r_count, pts_far_filter_r, (int) round(FILTER_KERNELSIZE)); + GetLinesFilter(pts_far_inv_r, pts_far_inv_r_count, pts_far_filter_r, (int)round(FILTER_KERNELSIZE)); pts_far_filter_r_count = pts_far_inv_r_count; // 边线等距采样 @@ -141,14 +196,13 @@ void cross_farline(){ pts_far_resample_right_count = sizeof(pts_far_resample_right) / sizeof(pts_far_resample_right[0]); GetLinesResample(pts_far_filter_r, pts_far_filter_r_count, pts_far_resample_right, &pts_far_resample_right_count, RESAMPLEDIST * PIXPERMETER); - int valid_far_points_count = 0; for (int i = 0; i < pts_far_resample_left_count; i++) { int x = pts_far_resample_left[i][1]; int y = pts_far_resample_left[i][0]; - if (y < IMAGE_H-3 && x > 3 && x < IMAGE_W-3 && y>=3 ) { + if (y < IMAGE_H - 3 && x > 3 && x < IMAGE_W - 3 && y >= 3) { pts_far_resample_left[valid_far_points_count][0] = pts_far_resample_left[i][0]; pts_far_resample_left[valid_far_points_count][1] = pts_far_resample_left[i][1]; valid_far_points_count++; @@ -162,7 +216,7 @@ void cross_farline(){ int x = pts_far_resample_right[i][1]; int y = pts_far_resample_right[i][0]; - if (y <= IMAGE_H-3 && x >= 3 && x <= IMAGE_W-3 && y>=3) { + if (y <= IMAGE_H - 3 && x >= 3 && x <= IMAGE_W - 3 && y >= 3) { pts_far_resample_right[valid_far_points_count][0] = pts_far_resample_right[i][0]; pts_far_resample_right[valid_far_points_count][1] = pts_far_resample_right[i][1]; valid_far_points_count++; @@ -172,47 +226,43 @@ void cross_farline(){ pts_far_resample_right_count = valid_far_points_count; // 边线角度变化率 - local_angle_points(pts_far_resample_left, pts_far_resample_left_count, far_angle_left, (int) round(ANGLEDIST / RESAMPLEDIST)); + local_angle_points(pts_far_resample_left, pts_far_resample_left_count, far_angle_left, (int)round(ANGLEDIST / RESAMPLEDIST)); far_angle_left_num = pts_far_resample_left_count; - local_angle_points(pts_far_resample_right, pts_far_resample_right_count, far_angle_right, (int) round(ANGLEDIST / RESAMPLEDIST)); + local_angle_points(pts_far_resample_right, pts_far_resample_right_count, far_angle_right, (int)round(ANGLEDIST / RESAMPLEDIST)); far_angle_right_num = pts_far_resample_right_count; // 角度变化率非极大值抑制 - nms_angle(far_angle_left, far_angle_left_num, far_angle_new_left, (int) round(ANGLEDIST / RESAMPLEDIST) * 2 + 1); + nms_angle(far_angle_left, far_angle_left_num, far_angle_new_left, (int)round(ANGLEDIST / RESAMPLEDIST) * 2 + 1); far_angle_new_left_num = far_angle_left_num; - nms_angle(far_angle_right, far_angle_right_num, far_angle_new_right, (int) round(ANGLEDIST / RESAMPLEDIST) * 2 + 1); + nms_angle(far_angle_right, far_angle_right_num, far_angle_new_right, (int)round(ANGLEDIST / RESAMPLEDIST) * 2 + 1); far_angle_new_right_num = far_angle_right_num; - + // 找远线上的L角点 far_Lpt0_found = far_Lpt1_found = false; // is_far_straight0 = pts_far_resample_left_count > 1.0 / RESAMPLEDIST; // is_far_straight1 = pts_far_resample_right_count > 1.0 / RESAMPLEDIST; for (int i = 0; i < pts_far_resample_left_count; i++) { if (far_angle_new_left[i] == 0) continue; - int im1 = clip(i - (int) round(ANGLEDIST / RESAMPLEDIST), 0, pts_far_resample_left_count - 1); - int ip1 = clip(i + (int) round(ANGLEDIST / RESAMPLEDIST), 0, pts_far_resample_left_count - 1); + int im1 = clip(i - (int)round(ANGLEDIST / RESAMPLEDIST), 0, pts_far_resample_left_count - 1); + int ip1 = clip(i + (int)round(ANGLEDIST / RESAMPLEDIST), 0, pts_far_resample_left_count - 1); float conf = fabs(far_angle_left[i]) - (fabs(far_angle_left[im1]) + fabs(far_angle_left[ip1])) / 2; - //L角点阈值 + // L角点阈值 if (far_Lpt0_found == false && (66. / 180. * PI32) < conf && conf < (140. / 180. * PI32) && i < 0.7 / RESAMPLEDIST) { far_Lpt0_rpts0s_id = i; - far_Lpt0_found = true; + far_Lpt0_found = true; } } - for (int i = 0; i < pts_far_resample_right_count; i++) { if (far_angle_new_right[i] == 0) continue; - int im1 = clip(i - (int) round(ANGLEDIST / RESAMPLEDIST), 0, pts_far_resample_right_count - 1); - int ip1 = clip(i + (int) round(ANGLEDIST / RESAMPLEDIST), 0, pts_far_resample_right_count - 1); + int im1 = clip(i - (int)round(ANGLEDIST / RESAMPLEDIST), 0, pts_far_resample_right_count - 1); + int ip1 = clip(i + (int)round(ANGLEDIST / RESAMPLEDIST), 0, pts_far_resample_right_count - 1); float conf = fabs(far_angle_right[i]) - (fabs(far_angle_right[im1]) + fabs(far_angle_right[ip1])) / 2; - if (far_Lpt1_found == false && (66. / 180. * PI32) < conf && conf < 140. / 180. * PI32 && i < 0.7 / RESAMPLEDIST) { far_Lpt1_rpts1s_id = i; - far_Lpt1_found = true; + far_Lpt1_found = true; } - } - } \ No newline at end of file diff --git a/app/gl_cross.h b/app/gl_cross.h index a0fa6eb..288bb05 100644 --- a/app/gl_cross.h +++ b/app/gl_cross.h @@ -10,6 +10,7 @@ enum cross_type_e { extern enum cross_type_e cross_type; extern int32_t Both_Boder_None_Cross; extern int far_Lpt0_rpts0s_id, far_Lpt1_rpts1s_id; +extern int cross_open; void CheckCross(void); void RunCross(void);