LidarMeasure/verify/Verify_result.cpp

//
// Created by zx on 2020/1/10.
//

#include "Verify_result.h"
#include <glog/logging.h>


/*
 * 显示rotate rect
 */

void display(cv::RotatedRect rect,cv::Scalar color)
{
    return ;
    cv::Point2f points_src[4];
    rect.points(points_src);

    cv::Mat image=cv::Mat::zeros(1000,500,CV_8UC3);
    cv::RotatedRect t_rect=rect;
    t_rect.center=cv::Point2f(250,500);
    t_rect.size.width/=10.0;
    t_rect.size.height/=10.0;
    cv::Point2f points[4];
    t_rect.points(points);
    float min_x=10000,max_x=0;
    for(int i=0;i<4;++i)
    {
        if(points_src[i].x<min_x)
            min_x=points_src[i].x;
        if(points_src[i].x>max_x)
            max_x=points_src[i].x;
        cv::line(image, points[i%4],points[(i+1)%4],color,2);
    }
    char buf[255]={0};
    sprintf(buf,"(%.1f,%.1f)[%.1f,%.1f] c:%.1f",t_rect.size.height*10.0,t_rect.size.width*10.0,min_x,max_x,t_rect.angle);
    cv::putText(image,buf,cv::Point(10,50),1,1.0,color);
    cv::namedWindow("debug",0);
    cv::imshow("debug",image);
    cv::waitKey(0);
}


/*
     * 构造函数
     * parameter:硬件限制配置参数
     */
Verify_result::Verify_result(Hardware_limit::Hardware_parameter parameter)
{
    m_hardware_parameter=parameter;
}
/*
 * 检验硬件限制
 * rotate_rect:待检验的旋转矩形
 */
Error_manager Verify_result::verify(cv::RotatedRect rotate_rect,float height,bool verify_vertex)
{
    //-2300   3450
    cv::Point2f t_corners[4];
    rotate_rect.points(t_corners);

    //第一步,检验边界
    float corner_min_y=m_hardware_parameter.corner_min_y();
    float corner_max_y=m_hardware_parameter.corner_max_y();
    float center_minx=m_hardware_parameter.center_min_x();
    float center_maxx=m_hardware_parameter.center_max_x();
    float center_miny=m_hardware_parameter.center_min_y();
    float center_maxy=m_hardware_parameter.center_max_y();
    float center_x=rotate_rect.center.x;
    float center_y=rotate_rect.center.y;
    //左超界
    if(center_x<center_minx)
    {
        return Error_manager(HARDWARE_LIMIT_CENTER_X_LEFT,NORMAL,"left limit");
    }
    //右超界
    if(center_x>center_maxx)
    {
        return Error_manager(HARDWARE_LIMIT_CENTER_X_RIGHT,NORMAL,"right limit");
    }
    //前超界
    if(center_y>center_maxy)
    {
        return Error_manager(HARDWARE_LIMIT_CENTER_Y_TOP,NORMAL,"top limit");
    }
    for(int i=0;i<4;++i)
    {
        if(t_corners[i].y>corner_max_y)
        {
            return Error_manager(HARDWARE_LIMIT_CENTER_Y_TOP,NORMAL,"corner top limit");
        }
    }
    //后超界
    if(center_y<center_miny)
    {
        return Error_manager(HARDWARE_LIMIT_CENTER_Y_BOTTOM,NORMAL,"bottom limit");
    }
    for(int i=0;i<4;++i)
    {
        if(t_corners[i].y<corner_min_y)
        {
            return Error_manager(HARDWARE_LIMIT_CENTER_Y_BOTTOM,NORMAL,"corner bottom limit");
        }
    }
    //第二步,检验高度
    if(height>m_hardware_parameter.height())
    {
        char description[255]={0};
        sprintf(description,"locate height is out of range(%.2f),height:%.2f",m_hardware_parameter.height(),height);
        return Error_manager(HARDWARE_LIMIT_HEIGHT_OUT_RANGE,NORMAL,description);
    }
    //第三步,检验角度
    //计算角度到 0-180
    // 长边向量
    cv::Point2f vec;
    cv::Point2f vertice[4];
    rotate_rect.points(vertice);

    float len1 = pow(vertice[0].x - vertice[1].x, 2.0) + pow(vertice[0].y - vertice[1].y, 2.0);
    float len2 = pow(vertice[1].x - vertice[2].x, 2.0) + pow(vertice[1].y - vertice[2].y, 2.0);
    // 寻找长边，倾角为长边与x轴夹角
    if (len1 > len2)
    {
        vec.x = vertice[0].x - vertice[1].x;
        vec.y = vertice[0].y - vertice[1].y;
    }
    else
    {
        vec.x = vertice[1].x - vertice[2].x;
        vec.y = vertice[1].y - vertice[2].y;
    }
    float angle_x = 180.0 / M_PI * acos(vec.x / sqrt(vec.x * vec.x + vec.y * vec.y));
    bool tb_theta_verify=false;
    for(int i=0;i<m_hardware_parameter.theta_range_size();++i)
    {
        Hardware_limit::Theta_range theta_range=m_hardware_parameter.theta_range(i);
        float min_theta=theta_range.min_theta();
        float max_theta=theta_range.max_theta();
        //角度检验, 根据plc限制
        if( angle_x>=min_theta&&angle_x<=max_theta)
        {
            tb_theta_verify = true;
        }
    }
    if(tb_theta_verify==false)
    {
        char description[255]={0};
        sprintf(description,"locate theta is out of range,theta:%.2f",angle_x);
        return Error_manager(HARDWARE_LIMIT_ANGLE_OUT_RANGE,NORMAL,description);
    }
    //是否检验顶点与栏杆的碰撞
    if(verify_vertex==false)
    {
        return SUCCESS;
    }
    //第二步,检验栏杆是否碰撞
    //创建栏杆
    const int railing_count=m_hardware_parameter.railing_parameter_size();
    if(railing_count==0)
        return SUCCESS;

    for(int i=0;i<railing_count;++i)
    {
        Hardware_limit::Railing railing_parameter=m_hardware_parameter.railing_parameter(i);
        Railing* tp_railing=new Railing(railing_parameter.pa(),railing_parameter.pb(),railing_parameter.pc(),
            railing_parameter.railing_width());
        if(tp_railing!=NULL)
        {
            Error_manager code;
            code=tp_railing->verify(rotate_rect);
            delete tp_railing;
            if(code!=SUCCESS)
            {
                return code;
            }
        }
    }
    return SUCCESS;
}

/*
     * 检验硬件限制
     * rotate_rect:待检验的旋转矩形
     * height:输入高度
     * terminal_id:终端编号,终端对应的左右栏杆号为: id,id+1
     */
Error_manager Verify_result::verify(cv::RotatedRect rotate_rect,int terminal_id,int& code)
{
    display(rotate_rect,cv::Scalar(0,255,0));

    cv::Point2f t_corners[4];
    rotate_rect.points(t_corners);
    code=0x000000;
    //第一步,检验边界
    float corner_min_y=m_hardware_parameter.corner_min_y();
    float corner_max_y=m_hardware_parameter.corner_max_y();
    float center_minx=m_hardware_parameter.center_min_x();
    float center_maxx=m_hardware_parameter.center_max_x();
    float center_miny=m_hardware_parameter.center_min_y();
    float center_maxy=m_hardware_parameter.center_max_y();
    float center_x=rotate_rect.center.x;
    float center_y=rotate_rect.center.y;

    // 后超界
    if(center_y<center_miny)
    {
        code|=LIMIT_BACK_ERROR;
    }
    for(int i=0;i<4;++i)
    {
        if(t_corners[i].y>corner_max_y)
        {
            code|=LIMIT_BACK_ERROR;
        }
    }
    //前超界
    if(center_y>center_maxy)
    {
        code|=LIMIT_FRONT_ERROR;
    }
    for(int i=0;i<4;++i)
    {
        if(t_corners[i].y>corner_max_y)
        {
            code|=LIMIT_FRONT_ERROR;
        }
    }


    ////第二步,根据id,检验左右栏杆
    int left_id=terminal_id;
    int right_id=terminal_id+1;
    if(left_id<m_hardware_parameter.railing_parameter_size()&&left_id>=0
    &&right_id<m_hardware_parameter.railing_parameter_size()&&right_id>=0)
    {
        Hardware_limit::Railing railing_left_parameter=m_hardware_parameter.railing_parameter(left_id);
        Railing* tp_left_railing=new Railing(railing_left_parameter.pa(),railing_left_parameter.pb(),railing_left_parameter.pc(),
                                             railing_left_parameter.railing_width());
        Hardware_limit::Railing railing_right_parameter=m_hardware_parameter.railing_parameter(right_id);
        Railing* tp_right_railing=new Railing(railing_right_parameter.pa(),railing_right_parameter.pb(),railing_right_parameter.pc(),
                                              railing_right_parameter.railing_width());

        if(tp_left_railing->verify(rotate_rect)!=SUCCESS)
        {
            code|=LIMIT_LEFT_ERROR;
        }
        if(tp_right_railing->verify(rotate_rect)!=SUCCESS)
        {
            code|=LIMIT_RIGHT_ERROR;
        }
        delete tp_left_railing;
        delete tp_right_railing;
    }
//    LOG(INFO)<<"verify   end";
    if(code!=0x00000000)
        return ERROR;
    return SUCCESS;
}