摄像头道路辅助判断函数（持续更新）

放一点写的辅助判断道路的函数，因为太多了就慢慢更吧。

//计算曲率
float process_curvity(int x1, int y1, int x2, int y2, int x3, int y3)
{
    float K;
    int S_of_ABC = ((x2 - x1) * (y3 - y1) - (x3 - x1) * (y2 - y1)) / 2;
    //面积的符号表示方向
    float q1 = (float)((x2 - x1) * (x2 - x1) + (y2 - y1) * (y2 - y1));
    float AB = sqrt(q1);
    q1 = (float)((x3 - x2) * (x3 - x2) + (y3 - y2) * (y3 - y2));
    float BC = sqrt(q1);
    q1 = (float)((x3 - x1) * (x3 - x1) + (y3 - y1) * (y3 - y1));
    float AC = sqrt(q1);
    if (AB * BC * AC == 0)
    {
        K = 0;
    }
    else
        K = (float)4 * S_of_ABC / (AB * BC * AC);
    return K;
}

这个函数是输入三个点，计算曲率，返回值的K就是计算得到的曲率，纯算法就不写注释了，直接就能拿去用。不过最好还是自己写，加深一下了解，也不难。

//最小二乘法拟合,拟合y=kx+b,直线函数
参数（拟合的边线类型，开