【智能车Code review】—曲率计算、最小二乘法拟合

【1】曲率计算函数

选三个点计算曲率

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

【2】最小二乘法拟合直线函数（y=kx+b）

type:左中右三线
startline:计算的起始行
endline：计算的结束行
parameterB ：斜率
parameterA :截距

void regression(int type, int startline, int endline)
{
    int i = 0;
    int sumlines = endline - startline;
    int sumX = 0;
    int sumY = 0;
    float averageX = 0;
    float averageY = 0;
    float sumUp = 0;
    float sumDown = 0;
    if (type == 0)      //拟合中线
    {
        for (i = startline; i < endline; i++)
        {
            sumX += i;
            sumY += centerline[i];
        }
        if (sumlines != 0)
        {
            averageX = sumX / sumlines;     //x的平均值
            averageY = sumY / sumlines;     //y的平均值
        }
        else
        {
            averageX = 0;     //x的平均值
            averageY = 0;     //y的平均值
        }
        for (i = startline; i < endline; i++)
        {
            sumUp += (centerline[i] - averageY) * (i - averageX);
            sumDown += (i - averageX) * (i - averageX);
        }
        if (sumDown == 0) parameterB = 0;
        else parameterB = sumUp / sumDown;
        parameterA = averageY - parameterB * averageX;
    }
    else if (type == 1)//拟合左线
    {
        for (i = startline; i < endline; i++)
        {
            sumX += i;
            sumY += lefetline[i];
        }
        if (sumlines == 0) sumlines = 1;
        averageX = sumX / sumlines;     //x的平均值
        averageY = sumY / sumlines;     //y的平均值
        for (i = startline; i < endline; i++)
        {
            //SetText("lefetline"+i+" " +lefetline[i] + " averageY" +" "+ averageY);
            sumUp += (lefetline[i] - averageY) * (i - averageX);
            sumDown += (i - averageX) * (i - averageX);
        }
        if (sumDown == 0) parameterB = 0;
        else parameterB = sumUp / sumDown;
        parameterA = averageY - parameterB * averageX;
    }
    else if (type == 2)//拟合右线
    {
        for (i = startline; i < endline; i++)
        {
            sumX += i;
            sumY += rightline[i];
        }
        if (sumlines == 0) sumlines = 1;
        averageX = sumX / sumlines;     //x的平均值
        averageY = sumY / sumlines;     //y的平均值
        for (i = startline; i < endline; i++)
        {
            sumUp += (rightline[i] - averageY) * (i - averageX);
            sumDown += (i - averageX) * (i - averageX);
        }
        if (sumDown == 0) parameterB = 0;
        else parameterB = sumUp / sumDown;
        parameterA = averageY - parameterB * averageX;

    }
}

改进之后的函数，可以选取两段数据作为拟合

void advanced_regression(int type, int startline1, int endline1, int startline2, int endline2)
{
    int i = 0;
    int sumlines1 = endline1 - startline1;
    int sumlines2 = endline2 - startline2;
    int sumX = 0;
    int sumY = 0;
    float averageX = 0;
    float averageY = 0;
    float sumUp = 0;
    float sumDown = 0;
    if (type == 0)  //拟合中线
    {
        /**计算sumX sumY**/
        for (i = startline1; i < endline1; i++)
        {
            sumX += i;
            sumY += centerline[i];
        }
        for (i = startline2; i < endline2; i++)
        {
            sumX += i;
            sumY += centerline[i];
        }
        averageX = sumX / (sumlines1 + sumlines2);     //x的平均值
        averageY = sumY / (sumlines1 + sumlines2);     //y的平均值
        for (i = startline1; i < endline1; i++)
        {
            sumUp += (centerline[i] - averageY) * (i - averageX);
            sumDown += (i - averageX) * (i - averageX);
        }
        for (i = startline2; i < endline2; i++)
        {
            sumUp += (centerline[i] - averageY) * (i - averageX);
            sumDown += (i - averageX) * (i - averageX);
        }
        if (sumDown == 0) parameterB = 0;
        else parameterB = sumUp / sumDown;
        parameterA = averageY - parameterB * averageX;

    }
    else if (type == 1)     //拟合左线
    {
        /**计算sumX sumY**/
        for (i = startline1; i < endline1; i++)
        {
            sumX += i;
            sumY += lefetline[i];
        }
        for (i = startline2; i < endline2; i++)
        {
            sumX += i;
            sumY += lefetline[i];
        }
        averageX = sumX / (sumlines1 + sumlines2);     //x的平均值
        averageY = sumY / (sumlines1 + sumlines2);     //y的平均值
        for (i = startline1; i < endline1; i++)
        {
            sumUp += (lefetline[i] - averageY) * (i - averageX);
            sumDown += (i - averageX) * (i - averageX);
        }
        for (i = startline2; i < endline2; i++)
        {
            sumUp += (lefetline[i] - averageY) * (i - averageX);
            sumDown += (i - averageX) * (i - averageX);
        }
        if (sumDown == 0) parameterB = 0;
        else parameterB = sumUp / sumDown;
        parameterA = averageY - parameterB * averageX;
    }
    else if (type == 2)         //拟合右线
    {
        /**计算sumX sumY**/
        for (i = startline1; i < endline1; i++)
        {
            sumX += i;
            sumY += rightline[i];
        }
        for (i = startline2; i < endline2; i++)
        {
            sumX += i;
            sumY += rightline[i];
        }
        averageX = sumX / (sumlines1 + sumlines2);     //x的平均值
        averageY = sumY / (sumlines1 + sumlines2);     //y的平均值
        for (i = startline1; i < endline1; i++)
        {
            sumUp += (rightline[i] - averageY) * (i - averageX);
            sumDown += (i - averageX) * (i - averageX);
        }
        for (i = startline2; i < endline2; i++)
        {
            sumUp += (rightline[i] - averageY) * (i - averageX);
            sumDown += (i - averageX) * (i - averageX);
        }
        if (sumDown == 0) parameterB = 0;
        else parameterB = sumUp / sumDown;
        parameterA = averageY - parameterB * averageX;
    }

}

【3】最小二乘法拟合形如y=a^2+b的二次曲线

void LeastSquareCalc_Curve(uint8_t StartLine, uint8_t EndLine, uint8_t type)
{
    uint8_t i = 0;
    float Sum_X2 = 0, Sum_Y = 0, Sum_X4 = 0, Sum_YX2 = 0, Average_X2 = 0, Average_Y = 0, Average_X4 = 0, Average_YX2 = 0, Sum = 0;

    Sum = EndLine - StartLine;
    if (type == 1)
    {
        for (i = StartLine; i < EndLine; i++)
        {
            Sum_X2 += i * i;
            Sum_Y += lefetline[i];
            Sum_X4 += i * i * i * i;
            Sum_YX2 += lefetline[i] * i * i;
        }
    }
    else if (type == 2)
    {
        for (i = StartLine; i < EndLine; i++)
        {
            Sum_X2 += i * i;
            Sum_Y += rightline[i];
            Sum_X4 += i * i * i * i;
            Sum_YX2 += rightline[i] * i * i;
        }
    }
    else    //中线
    {
        for (i = StartLine; i < EndLine; i++)
        {
            Sum_X2 += i * i;
            Sum_Y += centerline[i];
            Sum_X4 += i * i * i * i;
            Sum_YX2 += centerline[i] * i * i;
        }
    }
    Average_X2 = Sum_X2 / Sum;
    Average_Y = Sum_Y / Sum;
    Average_X4 = Sum_X4 / Sum;
    Average_YX2 = Sum_YX2 / Sum;
    curve_a = (Average_YX2 - Average_Y * Average_X2) / (Average_X4 - Average_X2 * Average_X2);
    curve_b = Average_Y - curve_a * Average_X2;
}
void advanced_LeastSquareCalc_Curve(uint8_t type, uint8_t StartLine1, uint8_t EndLine1, uint8_t StartLine2, uint8_t EndLine2)
{
    uint8_t i = 0;
    float Sum_X2 = 0, Sum_Y = 0, Sum_X4 = 0, Sum_YX2 = 0, Average_X2 = 0, Average_Y = 0, Average_X4 = 0, Average_YX2 = 0, Sum = 0;

    Sum = EndLine1 - StartLine1 + EndLine2 - StartLine2;
    if (type == 1)  //左线
    {
        for (i = StartLine1; i < EndLine1; i++)
        {
            Sum_X2 += i * i;
            Sum_Y += lefetline[i];
            Sum_X4 += i * i * i * i;
            Sum_YX2 += lefetline[i] * i * i;
        }
        for (i = StartLine2; i < EndLine2; i++)
        {
            Sum_X2 += i * i;
            Sum_Y += lefetline[i];
            Sum_X4 += i * i * i * i;
            Sum_YX2 += lefetline[i] * i * i;
        }
    }
    else if (type == 2) //右线
    {
        for (i = StartLine1; i < EndLine1; i++)
        {
            Sum_X2 += i * i;
            Sum_Y += rightline[i];
            Sum_X4 += i * i * i * i;
            Sum_YX2 += rightline[i] * i * i;
        }
        for (i = StartLine2; i < EndLine2; i++)
        {
            Sum_X2 += i * i;
            Sum_Y += rightline[i];
            Sum_X4 += i * i * i * i;
            Sum_YX2 += rightline[i] * i * i;
        }
    }
    else    //中线
    {
        for (i = StartLine1; i < EndLine1; i++)
        {
            Sum_X2 += i * i;
            Sum_Y += centerline[i];
            Sum_X4 += i * i * i * i;
            Sum_YX2 += centerline[i] * i * i;
        }
        for (i = StartLine2; i < EndLine2; i++)
        {
            Sum_X2 += i * i;
            Sum_Y += centerline[i];
            Sum_X4 += i * i * i * i;
            Sum_YX2 += centerline[i] * i * i;
        }
    }
    Average_X2 = Sum_X2 / Sum;
    Average_Y = Sum_Y / Sum;
    Average_X4 = Sum_X4 / Sum;
    Average_YX2 = Sum_YX2 / Sum;
    curve_a = (Average_YX2 - Average_Y * Average_X2) / (Average_X4 - Average_X2 * Average_X2);
    curve_b = Average_Y - curve_a * Average_X2;
}

【4】最小二乘法拟合二元多次曲线

///

///用最小二乘法拟合二元多次曲线
///
///已知点的x坐标集合
///已知点的y坐标集合
///已知点的个数
///方程的最高次数
/**********/
public static double My_Pow(double a, int b)
{
    int i = 0;
    double result = a;
    for (i = 1; i <= b; i++)
    {
        result *= a;
    }
    return result;
}
public static double[] MultiLine(double[] arrX, double[] arrY, int length, int dimension)//二元多次线性方程拟合曲线
{
    int n = dimension + 1;                 //dimension次方程需要求 dimension+1个 系数
    double[,] Guass = new double[n, n + 1];     //高斯矩阵 例如：y=a0+a1*x+a2*x*x
    for (int i = 0; i < n; i++)
    {
        int j;
        for (j = 0; j < n; j++)
        {
            Guass[i, j] = SumArr(arrX, j + i, length);
        }
        Guass[i, j] = SumArr(arrX, i, arrY, 1, length);
    }
    return ComputGauss(Guass, n);
}
public static double SumArr(double[] arr, int n, int length) //求数组的元素的n次方的和
{
    double s = 0;
    for (int i = 0; i < length; i++)
    {
        if (arr[i] != 0 || n != 0)
            s = s + My_Pow(arr[i], n);
        else
            s = s + 1;
    }
    return s;
}
public static double SumArr(double[] arr1, int n1, double[] arr2, int n2, int length)
{
    double s = 0;
    for (int i = 0; i < length; i++)
    {
        if ((arr1[i] != 0 || n1 != 0) && (arr2[i] != 0 || n2 != 0))
            s = s + My_Pow(arr1[i], n1) * My_Pow(arr2[i], n2);
        else
            s = s + 1;
    }
    return s;

}
public static double[] ComputGauss(double[,] Guass, int n)
{
    int i, j;
    int k, m;
    double temp;
    double max;
    double s;
    double[] x = new double[n]; for (i = 0; i < n; i++) x[i] = 0.0;//初始化
    for (j = 0; j < n; j++)
    {
        max = 0; k = j;
        for (i = j; i < n; i++)
        {
            if (double_abs(Guass[i, j]) > max)
            {
                max = Guass[i, j];
                k = i;
            }
        }

        if (k != j)
        {
            for (m = j; m < n + 1; m++)
            {
                temp = Guass[j, m];
                Guass[j, m] = Guass[k, m];
                Guass[k, m] = temp;
            }
        }
        if (0 == max)
        {
            // "此线性方程为奇异线性方程"                return x;
        }
        for (i = j + 1; i < n; i++)
        {
            s = Guass[i, j];
            for (m = j; m < n + 1; m++)
            {
                Guass[i, m] = Guass[i, m] - Guass[j, m] * s / (Guass[j, j]);
            }
        }
    }//结束for (j=0;j
    for (i = n - 1; i >= 0; i--)
    {
        s = 0;
        for (j = i + 1; j < n; j++)
        {
            s = s + Guass[i, j] * x[j];
        }
        x[i] = (Guass[i, n] - s) / Guass[i, i];
    }
    return x;
}//返回值是函数的系数例如：y=a0+a1*x 返回值则为a0 a1例如：y=a0+a1*x+a2*x*x 返回值则为a0 a1 a2 剩下的就不用写了吧