[Smart Car Code review]-Finding the turning point

[Smart Car Code review]-Finding the turning point

Series of articles

[Smart car Code review] curvature calculation, least squares fitting
[Smart car Code review] ramp image and control processing
[Smart car Code review] Searching for inflection points
[Smart car Code review] Small S Judgment on the road with China and S
[Smart Car Code review] Determination of roundabouts and line-up operation
Smart car resumption diary [1] Menu index review
Smart car resumption diary [2] Common PID, variable structure PID, differential first PID, Fuzzy PID, Expert PID
Smart Car Resumption Diary [3]: Image Processing-Basic Scanning and Basic Feature Extraction and Cross Line Compensation
Smart Car Resumption Diary [4]: Summary of the adjustment problems of upper and lower computers
Smart Car Diary of the resumption of work [5]: Identification of the starting line and
resumption of work of the smart car in the garage [6]: Fuzzy PID record with bugs.
Diary of the resumption of work of the smart car [7]: Image problem of the meeting car.
Diary of the resumption of work of the smart car [N]: Image Handling-debug record around the island (continuous update)

CROSSROAD: Cross inflection point
ROUNDISLAND: island inflection
rightfindflag [i]: indicates whether the i-th row of the right line is found, one is found, 0 is not found
flag_find_huan_rightdown_point: Found Roundabout lower right inflection flag
flag_find_huan_rightmiddle_point: in the right knee island found flag
to find inflection Idea: Observe the trend of data points in a piece of data, and ensure that this piece of data is accurate. In the case of severe camera distortion, this method is no longer applicable
. For function calls, please see: blog.csdn.net/qq_42604176...

[1] Find the function of the lower right inflection point

//[Function5] Find the function of the lower right corner point//input: start point end point to determine the type of lower inflection point ( circular island or cross?) void find_rightdown_point (byte start_point, byte end_point, byte RoadName) { byte j; if (RoadName == CROSSROAD) { for (j = start_point; j <= end_point; j++) { //Find the right mutation (lower inflection point) if ( My_Abs (rightline[j- 1 ]-rightline[j- 2 ]) <= 3 && My_Abs (rightline[j]-rightline[j- 1 ]) <= 3 && ( rightline[j + 1 ]-rightline[j] <= -2 ) && rightfindflag[j- 2 ] == 1 && rightfindflag[j- 1 ] ==1 && rightfindflag[j] == 1 ) { right_turn_down[ 0 ] = j + 1 ; //There is no row 0 in the array right_turn_down[ 1 ] = rightline[j]; break ; } } } else if (RoadName == ROUNDISLAND) { for (j = start_point; j <= end_point; j++) { //Find the right mutation (lower inflection point) if (r_start <= 34 && My_Abs (rightline[j- 1 ]-rightline[j- 2 ]) <= 3 && My_Abs (rightline[j]-rightline[j- 1 ]) <= 3 && (rightline[j + 1 ]-rightline[j] <= -2 ) && rightfindflag[j- 2 ] == 1 && rightfindflag[j- 1 ] == 1 && rightfindflag[j] == 1 ) { right_turn_down[ 0 ] = j + 1 ; //There is no row 0 in the array right_turn_down[ 1 ] = rightline[j]; flag_find_huan_rightdown_point = 1 ; break ; } } //If the coordinates of the lower right corner is not empty and the roundabout status is not 4, it is determined that the lower right corner of the roundabout has been found if (right_turn_down[ 0 ] != 0 && huandao_memory != 4 ) flag_find_huan_rightdown_point = 1 ; } else { } } Copy code

[2] Find the function of the lower left corner

//Function6 Find the lower left corner function //input: start point end point to determine the type of lower corner ( circular island or cross?) void find_leftdown_point (byte start_point, byte end_point, byte RoadName) { byte j; if (RoadName == CROSSROAD) { for (j = start_point; j <= end_point; j++) { //Find the left mutation (lower inflection point); continuous and existing points () if (My_Abs(lefetline[j- 1 ]-lefetline[j- 2 ]) <= 3 && My_Abs(lefetline[j]-lefetline[j - . 1 ]) <= . 3 && (lefetline [J + . 1 ] - lefetline [J]> = . 3 ) && leftfindflag[j- 2 ] == 1 && leftfindflag[j- 1 ] == 1 && leftfindflag[j] == 1 ) { left_turn_down[ 0 ] = j + 1 ; //There is no row 0 in the array left_turn_down[ 1 ] = lefetline[j]; break ; } } } else if (RoadName == ROUNDISLAND) { //setText user-defined ("l_start" + l_start); for (j = start_point; j <= end_point; j++) { //Find the left mutation (lower inflection point); continuous and existing points () if (l_start <= 34 && My_Abs(lefetline[j- 1 ]-lefetline[j- 2 ]) <= 3 && My_Abs(lefetline[j ]-lefetline[j- 1 ]) <= 3 && (lefetline[j + 1 ]-lefetline[j] >= 3 ) && leftfindflag[j- 2 ] == 1 && leftfindflag[j- 1 ] == 1 && leftfindflag[j] == 1 ) { left_turn_down[ 0 ] = j + 1 ; //There is no row 0 in the array left_turn_down[ 1 ] = lefetline[j]; flag_find_huan_leftdown_point = 1 ; break ; } } if (left_turn_down[ 0 ] != 0 && huandao_memory != 4 ) flag_find_huan_leftdown_point = 1 ; } else { } } Copy code

[3] Find the right middle inflection point function

//Function7 Find the right middle inflection point function //input: start point end point void find_rightmiddle_point (byte start_point, byte end_point) { byte i; for (i = start_point; i <= end_point; i++) { //Find your turning point if (i >= 8 && (rightline[i- 5 ]-rightline[i- 1 ]) < 0 && (rightline[i- 4 ]-rightline[i- 1 ]) < 0 && (rightline [i- 3 ]-rightline[i- 1 ]) < 0 && (rightline[i- 5 ]-rightline[i- 4 ]) <= 0 && (rightline[i- 4 ]-rightline[i- 3 ]) <= 0 && (rightline[i- 3 ]-rightline[i- 2 ]) <= 0 && (rightline[i + 4 ]-rightline[i- 1 ]) <= 0 && (rightline[i + 3 ]-rightline[i- 1 ]) <= 0 && (rightline[i + 2 ]-rightline[i - . 1 ]) <= 0 && (rightline [I + . 4 ] - rightline [I + . 3 ]) <= 0 && (rightline [I + . 3 ] - rightline [I + 2 ]) <= 0 ) { right_turn_middle[ 0 ] = (byte)(i- 1 ); right_turn_middle[ 1 ] = rightline[i- 1 ]; flag_find_huan_rightmiddle_point = 1 ; break ; } } } Copy code

[4] Find the left middle inflection point function

//Function8 Find the left middle inflection point function //input: start point end point void find_leftmiddle_point (byte start_point, byte end_point) { byte i; for (i = start_point; i <= end_point; i++) { //Find the left turning point if (i >= 8 && (lefetline[i- 5 ]-lefetline[i- 1 ])> 0 && (lefetline[i- 4 ]-lefetline[i- 1 ])> 0 && (lefetline [i- 3 ]-lefetline[i- 1 ])> 0 && (lefetline[i- 5 ]-lefetline[i- 4 ]) >= 0 && (lefetline[i- 4 ]-lefetline[i- 3 ]) >= 0 && (lefetline[i- 3 ]-lefetline[i- 2 ]) >= 0 && (lefetline[i + 4 ]-lefetline[i- 1 ]) >= 0 && (lefetline[i + 3 ]-lefetline[i- 1 ]) >= 0 && (lefetline[i + 2 ]-lefetline[i - . 1 ])> = 0 //&& (lefetline [I +. 4] - lefetline [I +. 3])> 0 && (lefetline [I +. 3] - lefetline [I + 2])> = 0 ) { left_turn_middle[ 0 ] = (byte)(i- 1 ); left_turn_middle[ 1 ] = lefetline[i- 1 ]; flag_find_huan_leftmiddle_point = 1 ; break ; } } } Copy code

[5] Find the upper right corner function

//Function9 Find the upper right inflection point function //input: start point end point to determine the type of lower inflection point (circle or cross?) void find_rightup_point (byte start_point, byte end_point, byte RoadName) { byte j; if (RoadName == CROSSROAD) { for (j = start_point; j <= end_point; j++) { //If there are more than 45 consecutive lines, it is judged to be a long straight, and there is no turning point at this time if (flag_s >= 47 ) break ; //Upper right turning point if ((rightline[j- 1 ]-rightline[j- 2 ]) >= 3 && My_Abs (rightline[j]-rightline[j- 1 ]) <= 2 && My_Abs (rightline[j + 1 ]-rightline[j]) <= 2 && rightfindflag[j- 1 ] == 1 && rightfindflag[j] == 1 && rightfindflag[j + 1 ] == 1 ) { right_turn_up[ 0 ] = j + 1 ; right_turn_up[ 1 ] = rightline[j]; break ; } } } else if (RoadName == ROUNDISLAND) { for (j = start_point; j <= end_point; j++) { //If there are more than 45 consecutive lines, it is judged to be a long straight, and there is no inflection point at this time. //if (flag_s >= 47) break; //upper right inflection point if ((rightline[j- 1 ]-rightline[j - 2 ])> = . 4 && My_Abs (rightline [J] - rightline [J - . 1 ]) <= . 3 && My_Abs (rightline [J + . 1 ] - rightline [J]) <= . 3 && rightfindflag [J - . 1 ] == 1 && rightfindflag[j] == 1 && rightfindflag[j + 1 ] == 1 ) { right_turn_up[ 0 ] = j + 1 ; right_turn_up[ 1 ] = rightline[j]; flag_find_huan_rightup_point = 1 ; if (((right_turn_middle[ 0 ]> 0 && right_turn_middle[ 0 ] <= 50 && right_turn_up[ 1 ] <= right_turn_middle[ 1 ]) || right_turn_up[ 1 ] <= 15 ) && huandao_memory == 4 ) { } else { break ; } break ; } } } else { } } Copy code

[6] Find the upper left corner function

//Function10 Find the upper left corner function //input: start point end point to determine the type of lower inflection point ( circular island or cross?) void find_leftup_point (byte start_point, byte end_point, byte RoadName) { byte j; if (RoadName == CROSSROAD) { for (j = start_point; j <= end_point; j++) { //If there are more than 45 consecutive lines, it is judged as a long straight, and there is no inflection point at this time if (flag_s >= 47 ) break ; //upper left inflection point //lefetline[j-2]-lefetline[j-3] <= -4 && if (lefetline[j- 1 ]-lefetline[j- 2 ] <= -3 && My_Abs (lefetline[j]-lefetline[j- 1 ]) <= 2 && My_Abs (lefetline[j + 1 ]-lefetline[j]) <= 2 && leftfindflag[j- 1 ] == 1 && leftfindflag[j] == 1 &&leftfindflag[j + 1 ] == 1) { left_turn_up[ 0 ] = j + 1 ; //There is no row 0 in the array left_turn_up[ 1 ] = lefetline[j]; if (left_turn_up[ 1 ] < 120 ) { break ; } else { } } } } else if (RoadName == ROUNDISLAND) { for (j = start_point; j <= end_point; j++) { //If there are more than 45 consecutive lines, it is judged to be a long straight, and there is no turning point at this time. //if (flag_s >= 47) break; //upper left turning point if (lefetline[j- 1 ]-lefetline[j- 2 ] <= -4 && My_Abs (lefetline[j]-lefetline[j- 1 ]) <= 3 && My_Abs (lefetline[j + 1 ]-lefetline[j]) <= 3 && leftfindflag[j- 1 ] = = 1 && leftfindflag[j] == 1 && leftfindflag[j + 1 ] == 1 ) { left_turn_up[ 0 ] = j + 1 ; //There is no row 0 in the array left_turn_up[ 1 ] = lefetline[j]; flag_find_huan_leftup_point = 1 ; if (((left_turn_middle[ 0 ]> 0 && left_turn_middle[ 0 ] <= 50 && left_turn_up[ 1 ] >= left_turn_middle[ 1 ])|| left_turn_up[ 1 ]>= 170 ) && huandao_memory== 4 ) { } else { break ; } } } } else { } } Copy code