系列文章

智能车复工日记【1】——菜单索引回顾

智能车复工日记【2】——普通PID、变结构PID、微分先行PID、模糊PID、专家PID

智能车复工日记【3】：图像处理——基本扫线和基本特征提取和十字补线

智能车复工日记【4】：关于图像的上下位机的调整问题总结

智能车复工日记【5】：起跑线的识别与车库入库

智能车复工日记【6】：有bug的模糊PID记录

智能车复工日记【7】：关于会车的图像问题

智能车复工日记【N】：图像处理——环岛debug记录（持续更新）

【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;

    }
}

获取详情请订阅：https://blog.csdn.net/qq_42604176/category_9858434.html?spm=1001.2014.3001.5482