219 lines
8.8 KiB
C
219 lines
8.8 KiB
C
#include "zf_common_headfile.h"
|
||
#include "gl_headfile.h"
|
||
|
||
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;
|
||
|
||
void CheckCross() {
|
||
bool Xfound = Lpt0_found && Lpt1_found;
|
||
if (cross_type == CROSS_NONE && Xfound){
|
||
cross_type = CROSS_BEGIN;
|
||
}
|
||
}
|
||
|
||
void RunCross() {
|
||
bool Xfound = Lpt0_found && Lpt1_found;
|
||
aim_distance = 0.4;
|
||
//检测到十字，先按照近线走
|
||
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;
|
||
}
|
||
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_far_resample_right_count < 5) { track_type = TRACK_RIGHT; }
|
||
else if (Both_Boder_None_Cross > 0 && pts_far_resample_left_count < 5) { track_type = TRACK_LEFT; }
|
||
|
||
}
|
||
}
|
||
|
||
|
||
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;
|
||
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) {
|
||
far_x1 = w1 - cross_width;
|
||
break;
|
||
}
|
||
}
|
||
/*如果一行全为白色没写*/
|
||
if (far_x1 <= 40)
|
||
{
|
||
far_x1 = 40;
|
||
}
|
||
|
||
for (; h1 > 0; h1--) {
|
||
//先黑后白，先找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){
|
||
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;
|
||
|
||
int w2 = IMAGE_W / 2 + BEGINW_L, h2 = BEGINH_R;
|
||
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) {
|
||
far_x2 = w2 + cross_width;
|
||
break;
|
||
}
|
||
}
|
||
/*如果一行全为白色没写*/
|
||
if (far_x2 >= 140)
|
||
{
|
||
far_x2 = 140;
|
||
}
|
||
|
||
for (; h2 > 0; h2--) {
|
||
//先黑后白，先找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){
|
||
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;
|
||
|
||
//透视变换
|
||
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_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_count = pts_far_right_count;
|
||
|
||
// 边线滤波
|
||
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));
|
||
pts_far_filter_r_count = pts_far_inv_r_count;
|
||
|
||
// 边线等距采样
|
||
pts_far_resample_left_count = sizeof(pts_far_resample_left) / sizeof(pts_far_resample_left[0]);
|
||
GetLinesResample(pts_far_filter_l, pts_far_filter_l_count, pts_far_resample_left, &pts_far_resample_left_count, RESAMPLEDIST * PIXPERMETER);
|
||
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 ) {
|
||
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++;
|
||
}
|
||
}
|
||
pts_far_resample_left_count = valid_far_points_count;
|
||
|
||
valid_far_points_count = 0;
|
||
|
||
for (int i = 0; i < pts_far_resample_right_count; i++) {
|
||
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) {
|
||
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++;
|
||
}
|
||
}
|
||
|
||
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));
|
||
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));
|
||
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);
|
||
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);
|
||
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);
|
||
float conf = fabs(far_angle_left[i]) - (fabs(far_angle_left[im1]) + fabs(far_angle_left[ip1])) / 2;
|
||
|
||
//L角点阈值
|
||
if (far_Lpt0_found == false && (66. / 180. * PI) < conf && conf < (140. / 180. * PI) && i < 0.7 / RESAMPLEDIST) {
|
||
far_Lpt0_rpts0s_id = i;
|
||
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);
|
||
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. * PI) < conf && conf < 140. / 180. * PI && i < 0.7 / RESAMPLEDIST) {
|
||
far_Lpt1_rpts1s_id = i;
|
||
far_Lpt1_found = true;
|
||
}
|
||
|
||
}
|
||
|
||
} |