【智能车Code review】——小S与中S道路判断

基本思想：
找到中线的左拐点和右拐点，并且计算出两点之间的空间距离。通过限制空间距离，分为中S和小S。
注意这种情况：当20行以下的中线斜率比较陡，此时可以通过人为限制设定，将第0行的中线设置为拐点。

 if (flag_centre_right_point == 0 && flag_centre_left_point == 1 && parameterB >= 0.5 && centre_left_point[0] >= 33)
 {
     centre_right_point[0] = 0;
     centre_right_point[1] = centerline[0];
     flag_centre_right_point = 1;
     SetText("中线右拐点：" + centre_right_point[0] + "  " + centre_right_point[1]);
 }
 if (flag_centre_left_point == 0 && flag_centre_right_point == 1 && parameterB <= -0.5 && centre_right_point[0] >= 33)
 {
     centre_left_point[0] = 0;
     centre_left_point[1] = centerline[0];
     flag_centre_left_point = 1;
     SetText("中线左拐点：" + centre_left_point[0] + "  " + centre_left_point[1]);
 }

完整的判定：（当然也有许多误判和漏判，此处只是提供一种思路）

#region[中S和小S]
void find_guaidian()
{
    byte i = 0;
    flag_centre_right_point = 0;
    flag_centre_left_point = 0;
    centre_left_point[0] = 0;
    centre_left_point[1] = 0;
    centre_right_point[0] = 0;
    centre_right_point[1] = 0;
    regression(0, 0, 20);
    SetText("0-20行的趋势" + parameterB);
    //找后11行的拐点
    for (i = 10; i <= 55; i++)
    {
        //中线右拐点
        if (i > 9 && flag_centre_right_point == 0
        && (centerline[i - 5] - centerline[i - 4]) >= 0 && (centerline[i - 6] - centerline[i - 4]) >= 0 && (centerline[i - 7] - centerline[i - 4]) >= 0 && (centerline[i - 8] - centerline[i - 4]) >= 0 && (centerline[i - 8] - centerline[i - 4]) <= 20      //判断j-4是否为中线拐点，拐点在左边的那个
        && (centerline[i - 3] - centerline[i - 4]) >= 0 && (centerline[i - 2] - centerline[i - 4]) >= 0 && (centerline[i - 2] - centerline[i - 4]) <= 20 && (centerline[i - 1] - centerline[i - 4]) >= 1 && (centerline[i] - centerline[i - 4]) >= 1
        && centerline[i - 4] > 20 && centerline[i - 4] < 165 && double_abs(parameterB) >= 0.2)
        {
            centre_right_point[0] = (byte)(i - 3);
            centre_right_point[1] = centerline[i - 4];
            flag_centre_right_point = 1;
        }
        //中线左拐点
        if (i > 9 && flag_centre_left_point == 0
        && (centerline[i - 5] - centerline[i - 4]) <= 0 && (centerline[i - 6] - centerline[i - 4]) <= 0 && (centerline[i - 7] - centerline[i - 4]) <= 0 && (centerline[i - 8] - centerline[i - 4]) <= 0 && (centerline[i - 8] - centerline[i - 4]) >= -20       //判断j-4是否为中线拐点，拐点在左边的那个
        && (centerline[i - 3] - centerline[i - 4]) <= 0 && (centerline[i - 2] - centerline[i - 4]) <= 0 && (centerline[i - 2] - centerline[i - 4]) >= -20 && (centerline[i - 1] - centerline[i - 4]) <= -1 && (centerline[i] - centerline[i - 4]) <= -1
        && centerline[i - 4] > 20 && centerline[i - 4] < 165 && double_abs(parameterB) >= 0.2)
        {
            centre_left_point[0] = (byte)(i - 3);
            centre_left_point[1] = centerline[i - 4];
            flag_centre_left_point = 1;
        }
    }
    if (flag_centre_left_point == 1)
    {
        SetText("中线左拐点：" + centre_left_point[0] + "  " + centre_left_point[1]);
    }
    if (flag_centre_right_point == 1)
    {
        SetText("中线右拐点：" + centre_right_point[0] + "  " + centre_right_point[1]);
    }
    if (flag_centre_right_point == 0 && flag_centre_left_point == 1 && parameterB >= 0.5 && centre_left_point[0] >= 33)
    {
        centre_right_point[0] = 0;
        centre_right_point[1] = centerline[0];
        flag_centre_right_point = 1;
        SetText("中线右拐点：" + centre_right_point[0] + "  " + centre_right_point[1]);
    }
    if (flag_centre_left_point == 0 && flag_centre_right_point == 1 && parameterB <= -0.5 && centre_right_point[0] >= 33)
    {
        centre_left_point[0] = 0;
        centre_left_point[1] = centerline[0];
        flag_centre_left_point = 1;
        SetText("中线左拐点：" + centre_left_point[0] + "  " + centre_left_point[1]);
    }
    Middle_S_flag = 0;
    flag_small_S = 0;
    if (flag_centre_left_point == 1 && flag_centre_right_point == 1 && break_hangshu >= 55)
    {
        SetText("中线拐点的竖直差值： " + My_Abs(centre_left_point[0] - centre_right_point[0]));
        SetText("中线拐点的水平差值： " + (centre_left_point[1] - centre_right_point[1]));
        if ((My_Abs(centre_left_point[0] - centre_right_point[0]) >= 8) && (My_Abs(centre_left_point[0] - centre_right_point[0]) <= 28) && (centre_left_point[1] - centre_right_point[1]) >= 8 && (centre_left_point[1] - centre_right_point[1]) <= 35)
        {
            SetText("符合小S的中线拐点特征");
            //setText用户自定义("符合小S的中线拐点特征");
            flag_small_S = 1;
        }
        else
        {
            SetText("符合中S的中线拐点特征");
           // setText用户自定义("符合中S的中线拐点特征");
            Middle_S_flag = 1;
        }
    }
}
#endregion

示意图：