智能车摄像头灰度处理高效算法（下）

本篇文章分享全国大学生智能车竞赛中常用的一些摄像头图像处理算法。 

智能车摄像头灰度处理高效算法（上）-CSDN博客

一、八邻域边界初始化

/*---------------------------------------------------------------
 【函    数】clear_find_point
 【功    能】八邻域边界初始化
 【参    数】无
 【返 回 值】
 【注意事项】
 ----------------------------------------------------------------*/
void clear_find_point(void)
{
    for(int i = 0;i<L_edge_count;i++)
    {
        L_edge[i].row = 0;
        L_edge[i].col = 0;
        L_edge[i].flag = 0;
    }
    for(int i = 0;i<R_edge_count;i++)
    {
        R_edge[i].row = 0;
        R_edge[i].col = 0;
        R_edge[i].flag = 0;
    }
}

二、差比和

/*---------------------------------------------------------------
 【函    数】calc_diff
 【功    能】差比和
 【参    数】无
 【返 回 值】
 【注意事项】约放大128倍
 ----------------------------------------------------------------*/
int16 calc_diff(int16 x, int16 y)
{
    return ( ((x-y)<<7)/(x+y) );
}

三、限幅

//-------------------------------------------------------------------------------------------------------------------
//  @brief      限幅
//  @param      x               被限幅的数据
//  @param      y               限幅范围(数据会被限制在-y至+y之间)
//  @return     float           限幅之后的数据
//  Sample usage:               float dat = limit(500,300);//数据被限制在-300至+300之间  因此返回的结果是300
//-------------------------------------------------------------------------------------------------------------------
float limit(float x, int32 y)
{
    if(x>y)             return (float)y;
    else if(x<-y)       return (float)(-y);
    else                return x;
}

四、 拐点检测

/*---------------------------------------------------------------
 【函    数】get_turning_point
 【功    能】拐点检测
 【参    数】无
 【返 回 值】
 【注意事项】
 ----------------------------------------------------------------*/
int16 L_corner_flag = 0;//左拐点存在标志
int16 L_corner_row = 0;//左拐点所在行
int16 L_corner_col = 0;//左拐点所在列
int L_corner_angle = 0;//左拐点角度
int16 R_corner_flag = 0;//右拐点存在标志
int16 R_corner_row = 0;//右拐点所在行
int16 R_corner_col = 0;//右拐点所在列
int R_corner_angle = 0;//右拐点角度
uint8 enable_L_corner=1,enable_R_corner=1;
void get_turning_point(void)
{
    L_corner_flag = 0;// 初始化变量
    L_corner_row = 0;
    L_corner_col = 0;
    L_corner_angle = 0;
    if(enable_L_corner) //如果使能搜索左拐点
    {
        if(L_edge_count > 9&&L_start_y>=IMAGE_H/2&&left_lose<=IMAGE_H/2)
        {
            for(int i = 0; i<L_edge_count-9;i++)
            {
                if(L_edge[i+8].row>5)
                {
                    if((L_edge[i].col - L_edge[i + 4].col) * (L_edge[i + 8].col - L_edge[i + 4].col) +
                       (L_edge[i].row - L_edge[i + 4].row) * (L_edge[i + 8].row - L_edge[i + 4].row) >= 0) //初步确认为锐角或者直角 向量法
                    {
                        L_corner_angle = Get_angle(L_edge[i].col, L_edge[i].row, L_edge[i + 4].col, L_edge[i + 4].row, L_edge[i + 8].col, L_edge[i + 8].row); //求角度
                        if(L_edge[i+4].col>L_edge[i+8].col&&L_corner_angle>=28&&L_corner_angle<=110)
                        {
                            L_corner_flag = 1;
                            L_corner_row = L_edge[i+4].row;
                            L_corner_col = L_edge[i+4].col;
                            break;
                        }
                    }
                }
            }
        }
    }
    R_corner_flag = 0;//初始化变量
    R_corner_row = 0;
    R_corner_col = 0;
    R_corner_angle = 0;
    if(enable_R_corner)    //如果使能搜索右拐点
    {
        if(R_edge_count > 9&&R_start_y>=IMAGE_H/2&&right_lose<=IMAGE_H/2)
        {
            for(int i = 0; i<R_edge_count-9;i++)
            {
                if(R_edge[i+8].row>5)
                {
                    if((R_edge[i].col - R_edge[i + 4].col) * (R_edge[i + 8].col - R_edge[i + 4].col) +
                    (R_edge[i].row - R_edge[i + 4].row) * (R_edge[i + 8].row - R_edge[i + 4].row) >= 0) //初步确认为锐角或者直角 向量法
                    {
                        R_corner_angle = Get_angle(R_edge[i].col, R_edge[i].row, R_edge[i + 4].col, R_edge[i + 4].row, R_edge[i + 8].col, R_edge[i + 8].row); //求角度
                        if(R_edge[i+8].col>R_edge[i+4].col&&R_corner_angle>=28&&R_corner_angle<=110)
                        {
                            R_corner_flag = 1;
                            R_corner_row = R_edge[i+4].row;
                            R_corner_col = R_edge[i+4].col;
                            break;
                        }
                    }
                }
            }
        }
    }
}

五、逆透视知三点求形成的角度(以左上角为原点形成坐标系)

//-------------------------------------------------------------------------------------------------------------------
//  @brief      逆透视知三点求形成的角度(以左上角为原点形成坐标系)
//  @param      Ax，Ay      下边点
//  @param      Bx，By      要求角度的一点
//  @param      Cx，Cy      上边点
//  @return
//  @since      v1.0
//  Sample usage:
//-------------------------------------------------------------------------------------------------------------------
float hd[3][3] ={{-0.436025, 0.000000, 21.365217}, {0.424457, 0.487500, -22.048370}, {0.018944, -0.000000, 0.071739}};
//x=[18,18,69,69]  % 依次为A、B、D、C在摄像头获取的照片中的的纵坐标
//y=[70,121,70,121] % 依次为A、B、D、C在摄像头获取的照片中的的横坐标
float Get_angle(float Ax, float Ay, float Bx, float By, float Cx, float Cy)
{

    float BA = 0.00;//向量BA的模
    float BC = 0.00;
    float SBA_BC = 0.00;//向量点乘的值
    float angle = 0.00;

    float AX=((hd[0][0] * Ax + hd[0][1] * Ay + hd[0][2])/(hd[2][0] * Ax + hd[2][1] * Ay + hd[2][2]));
    float AY=((hd[1][0] * Ax + hd[1][1] * Ay + hd[1][2])/(hd[2][0] * Ax + hd[2][1] * Ay + hd[2][2]));
    float BX=((hd[0][0] * Bx + hd[0][1] * By + hd[0][2])/(hd[2][0] * Bx + hd[2][1] * By + hd[2][2]));
    float BY=((hd[1][0] * Bx + hd[1][1] * By + hd[1][2])/(hd[2][0] * Bx + hd[2][1] * By + hd[2][2]));
    float CX=((hd[0][0] * Cx + hd[0][1] * Cy + hd[0][2])/(hd[2][0] * Cx + hd[2][1] * Cy + hd[2][2]));
    float CY=((hd[1][0] * Cx + hd[1][1] * Cy + hd[1][2])/(hd[2][0] * Cx + hd[2][1] * Cy + hd[2][2]));

    BA = sqrt((AX-BX)*(AX-BX)+(AY-BY)*(AY-BY));
    BC = sqrt((CX-BX)*(CX-BX)+(CY-BY)*(CY-BY));

    SBA_BC = (AX-BX)*(CX-BX)+(AY-BY)*(CY-BY);

    angle =  acos(SBA_BC*1.00/(BA*BC));

    return angle*57.3;
}