LidarMeasure/wj_lidar/region_detect.cpp

//
// Created by zx on 2019/12/6.
//
#include "region_detect.h"

/**
 * 有参构造函数
 * */
Region_detector::Region_detector(int id, wj::Region region) : m_region_id(-1)
{
    m_region_param.CopyFrom(region);
    m_region_id = id;
}

/**
 * 析构函数
 * */
Region_detector::~Region_detector()
{
}

/**
 * 获取区域id
 * */
int Region_detector::get_region_id()
{
    return m_region_id;
}

/**
 * 预处理，xy直通滤波与离群点过滤
 * 返回 PARAMETER_ERROR，WJ_REGION_EMPTY_CLOUD 或 SUCCESS
 * */
Error_manager Region_detector::preprocess(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_in, pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_out)
{
    // 1.参数合法性检验
    if (!m_region_param.has_maxx() || !m_region_param.has_maxy() || !m_region_param.has_minx() || !m_region_param.has_miny())
        return Error_manager(Error_code::PARAMETER_ERROR);
    if (cloud_in->size() <= 0)
        return Error_manager(Error_code::WJ_REGION_EMPTY_CLOUD);
    cloud_out->clear();
    pcl::copyPointCloud(*cloud_in, *cloud_out);
    if (cloud_out->size() <= 0)
        return Error_manager(Error_code::WJ_REGION_EMPTY_CLOUD);

    // 2.直通滤波, 筛选xy
    pcl::PassThrough<pcl::PointXYZ> pass;
    pass.setInputCloud(cloud_out);
    pass.setFilterFieldName("x");                                       //设置想在哪个坐标轴上操作
    pass.setFilterLimits(m_region_param.minx(), m_region_param.maxx()); //将x轴的0到1范围内
    pass.setFilterLimitsNegative(false);                                //保留（true就是删除，false就是保留而删除此区间外的）
    pass.filter(*cloud_out);                                            //输出到结果指针

    if (cloud_out->size() <= 0)
        return Error_manager(Error_code::WJ_REGION_EMPTY_CLOUD);

    pass.setInputCloud(cloud_out);
    pass.setFilterFieldName("y");                                       //设置想在哪个坐标轴上操作
    pass.setFilterLimits(m_region_param.miny(), m_region_param.maxy()); //将x轴的0到1范围内
    pass.setFilterLimitsNegative(false);                                //保留（true就是删除，false就是保留而删除此区间外的）
    pass.filter(*cloud_out);                                            //输出到结果指针

    if (cloud_out->size() <= 0)
        return Error_manager(Error_code::WJ_REGION_EMPTY_CLOUD);

    // 3.离群点过滤
    pcl::StatisticalOutlierRemoval<pcl::PointXYZ> sor;
    sor.setInputCloud(cloud_out);
    sor.setMeanK(15);            //K近邻搜索点个数
    sor.setStddevMulThresh(3.0); //标准差倍数
    sor.setNegative(false);      //保留未滤波点（内点）
    sor.filter(*cloud_out);      //保存滤波结果到cloud_filter

    if (cloud_out->size() <= 0)
        return Error_manager(Error_code::WJ_REGION_EMPTY_CLOUD);
    else
        return Error_manager(Error_code::SUCCESS);
}

/**
 * 返回二维点之间距离
 * */
double distance(cv::Point2f p1, cv::Point2f p2)
{
    return sqrt(pow(p1.x - p2.x, 2.0) + pow(p1.y - p2.y, 2.0));
}

/**
 * 判断是否符合标准矩形，
 * 传入矩形四个角点，以及是否打印
 * 返回 WJ_REGION_RECTANGLE_ANGLE_ERROR, WJ_REGION_RECTANGLE_SIZE_ERROR, 
 * WJ_REGION_RECTANGLE_SYMMETRY_ERROR, WJ_REGION_CLUSTER_SIZE_ERROR, SUCCESS
 * */
Error_manager Region_detector::isRect(std::vector<cv::Point2f> &points, bool print)
{
    if (points.size() == 4)
    {
        double L[3] = {0.0};
        L[0] = distance(points[0], points[1]);
        L[1] = distance(points[1], points[2]);
        L[2] = distance(points[0], points[2]);
        double max_l = L[0];
        double l1 = L[1];
        double l2 = L[2];
        int max_index = 0;
        cv::Point2f ps = points[0], pt = points[1];
        // 顶点
        cv::Point2f pc = points[2];
        for (int i = 1; i < 3; ++i)
        {
            if (L[i] > max_l)
            {
                max_index = i;
                max_l = L[i];
                l1 = L[abs(i + 1) % 3];
                l2 = L[abs(i + 2) % 3];
                ps = points[i % 3];
                pt = points[(i + 1) % 3];
                pc = points[(i + 2) % 3];
            }
        }
        // 顶角大小
        double cosa = (l1 * l1 + l2 * l2 - max_l * max_l) / (2.0 * l1 * l2);
        // 顶角相对于90度过大或小
        if (fabs(cosa) >= 0.15 /* || std::min(l1, l2) > 2.0 || std::max(l1, l2) > 3.3*/)
        {
            if (print)
            {
                LOG(WARNING) << " angle cos >0.13 =" << cosa << "  i=" << max_index;
                // LOG(WARNING) << "L1:" << l1 << "  L2:" << l2 << "  L3:" << max_l;
            }
            return Error_manager(Error_code::WJ_REGION_RECTANGLE_ANGLE_ERROR);
        }

        float width = std::min(l1, l2);
        float length = std::max(l1, l2);
        // 车宽应位于[1.35, 2.0]，车长应位于[2.2, 3.0]
        if (width < 1.350 || width > 2.000 || length > 3.000 || length < 2.200)
        {
            if (print)
            {
                LOG(WARNING) << "\t width<1350 || width >2100 || length >3300 ||length < 2100 "
                             << "  length:" << length << "  width:" << width;
            }
            return Error_manager(Error_code::WJ_REGION_RECTANGLE_SIZE_ERROR);
        }

        double d = distance(pc, points[3]);
        cv::Point2f center1 = (ps + pt) * 0.5;
        cv::Point2f center2 = (pc + points[3]) * 0.5;
        // 对角线形变超过0.15倍，或中心点距离偏差0.15m
        if (fabs(d - max_l) > max_l * 0.15 || distance(center1, center2) > 0.150)
        {
            if (print)
            {
                LOG(WARNING) << "d:" << d << " maxl:" << max_l << "  center1:" << center1 << "  center2:" << center2
                             << "  center distance=" << distance(center1, center2);
            }
            return Error_manager(Error_code::WJ_REGION_RECTANGLE_SYMMETRY_ERROR);
        }
        if (print)
        {
            LOG(INFO) << " rectangle verify OK  cos angle=" << cosa << "  length off=" << fabs(d - max_l)
                         << "  center distance=" << distance(center1, center2);
        }
        return Error_manager(Error_code::SUCCESS);
    }
    // 以三个点进行验证
    else if (points.size() == 3)
    {
        double L[3] = {0.0};
        L[0] = distance(points[0], points[1]);
        L[1] = distance(points[1], points[2]);
        L[2] = distance(points[0], points[2]);
        double max_l = L[0];
        double l1 = L[1];
        double l2 = L[2];
        int max_index = 0;
        cv::Point2f ps = points[0], pt = points[1];
        cv::Point2f pc = points[2];
        for (int i = 1; i < 3; ++i)
        {
            if (L[i] > max_l)
            {
                max_index = i;
                max_l = L[i];
                l1 = L[abs(i + 1) % 3];
                l2 = L[abs(i + 2) % 3];
                ps = points[i % 3];
                pt = points[(i + 1) % 3];
                pc = points[(i + 2) % 3];
            }
        }
        double cosa = (l1 * l1 + l2 * l2 - max_l * max_l) / (2.0 * l1 * l2);
        // 顶角应接近90度
        if (fabs(cosa) >= 0.15)
        {
            if (print)
            {
                LOG(WARNING) << "3 wheels angle cos >0.12 =" << cosa << "  i=" << max_index;
                LOG(WARNING) << "L1:" << l1 << "  L2:" << l2 << "  L3:" << max_l;
            }
            return Error_manager(Error_code::WJ_REGION_RECTANGLE_ANGLE_ERROR);
        }

        double length = std::max(l1, l2);
        double width = std::min(l1, l2);
        // 车宽应位于[1.4, 2.0]，车长应位于[2.2, 3.0]
        if (length > 2.100 && length < 3.000 && width > 1.400 && width < 2.100)
        {
            //生成第四个点
            cv::Point2f vec1 = ps - pc;
            cv::Point2f vec2 = pt - pc;
            cv::Point2f point4 = (vec1 + vec2) + pc;
            points.push_back(point4);
            if (print)
            {
                LOG(WARNING) << "3 wheels rectangle verify OK  cos angle=" << cosa << "  L=" << length
                             << "  w=" << width;
            }
            return Error_manager(Error_code::SUCCESS);
        }
        else
        {
            if (print)
            {
                LOG(WARNING) << "3 wheels rectangle verify Failed  cos angle=" << cosa << "  L=" << length
                             << "  w=" << width;
            }
            return Error_manager(Error_code::WJ_REGION_RECTANGLE_SIZE_ERROR);
        }
    }
    else
    {
        return Error_manager(Error_code::WJ_REGION_CLUSTER_SIZE_ERROR);
    }
}

/*
 * 仅仅聚类
 */
Error_manager Region_detector::clustering_only(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_in,
        std::vector<pcl::PointCloud<pcl::PointXYZ>> &seg_clouds, bool print)
{
    // 1.判断参数合法性
    if (cloud_in->size() <= 0)
        return Error_manager(Error_code::WJ_REGION_EMPTY_CLOUD);

    // 2.聚类
    pcl::search::KdTree<pcl::PointXYZ>::Ptr kdtree(new pcl::search::KdTree<pcl::PointXYZ>);
    kdtree->setInputCloud(cloud_in);

    pcl::EuclideanClusterExtraction<pcl::PointXYZ> clustering;
    // 设置聚类的最小值 2cm (small values may cause objects to be divided
    // in several clusters, whereas big values may join objects in a same cluster).
    clustering.setClusterTolerance(0.5);
    // 设置聚类的小点数和最大点云数
    clustering.setMinClusterSize(10);
    clustering.setMaxClusterSize(500);
    clustering.setSearchMethod(kdtree);
    clustering.setInputCloud(cloud_in);
    std::vector<pcl::PointIndices> clusters;
    clustering.extract(clusters);
    if(clusters.size() <= 0)
        return Error_manager(Error_code::WJ_REGION_EMPTY_CLOUD);

    for (int i = 0; i < clusters.size(); ++i)
    {
        seg_clouds.push_back(pcl::PointCloud<pcl::PointXYZ>());
    }

    int j = 0;
    for (std::vector<pcl::PointIndices>::const_iterator it = clusters.begin(); it != clusters.end(); ++it)
    {
        pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_cluster(new pcl::PointCloud<pcl::PointXYZ>);
        //创建新的点云数据集cloud_cluster，将所有当前聚类写入到点云数据集中
        for (std::vector<int>::const_iterator pit = it->indices.begin(); pit != it->indices.end(); ++pit)
        {
            cloud_cluster->points.push_back(cloud_in->points[*pit]);
            cloud_cluster->width = cloud_cluster->points.size();
            cloud_cluster->height = 1;
            cloud_cluster->is_dense = true;
        }
        seg_clouds[j] = *cloud_cluster;
        j++;
    }

    if (print)
    {
        LOG(INFO) << " cluster classes:" << clusters.size();
    }

    return SUCCESS;
}


/**
 * 聚类并通过矩形判断函数
 * 传入原始点云，传出角点与聚类出的点云
 * 返回 WJ_REGION_EMPTY_CLOUD, WJ_REGION_RECTANGLE_ANGLE_ERROR, WJ_REGION_RECTANGLE_SIZE_ERROR, 
 * WJ_REGION_RECTANGLE_SYMMETRY_ERROR, WJ_REGION_CLUSTER_SIZE_ERROR, SUCCESS
 * */
Error_manager Region_detector::clustering(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_in, std::vector<cv::Point2f> &corner_points, std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> &seg_clouds, bool print)
{
    // 1.判断参数合法性
    if (cloud_in->size() <= 0)
        return Error_manager(Error_code::WJ_REGION_EMPTY_CLOUD);

    // 2.聚类
    pcl::search::KdTree<pcl::PointXYZ>::Ptr kdtree(new pcl::search::KdTree<pcl::PointXYZ>);
    kdtree->setInputCloud(cloud_in);

    pcl::EuclideanClusterExtraction<pcl::PointXYZ> clustering;
    // 设置聚类的最小值 2cm (small values may cause objects to be divided
    // in several clusters, whereas big values may join objects in a same cluster).
    clustering.setClusterTolerance(0.2);
    // 设置聚类的小点数和最大点云数
    clustering.setMinClusterSize(10);
    clustering.setMaxClusterSize(500);
    clustering.setSearchMethod(kdtree);
    clustering.setInputCloud(cloud_in);
    std::vector<pcl::PointIndices> clusters;
    clustering.extract(clusters);
    if(clusters.size() <= 0)
        return Error_manager(Error_code::WJ_REGION_EMPTY_CLOUD);

    for (int i = 0; i < clusters.size(); ++i)
    {
        seg_clouds.push_back(pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>));
    }

    int j = 0;
    for (std::vector<pcl::PointIndices>::const_iterator it = clusters.begin(); it != clusters.end(); ++it)
    {
        pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_cluster(new pcl::PointCloud<pcl::PointXYZ>);
        //创建新的点云数据集cloud_cluster，将所有当前聚类写入到点云数据集中
        for (std::vector<int>::const_iterator pit = it->indices.begin(); pit != it->indices.end(); ++pit)
        {
            cloud_cluster->points.push_back(cloud_in->points[*pit]);
            cloud_cluster->width = cloud_cluster->points.size();
            cloud_cluster->height = 1;
            cloud_cluster->is_dense = true;
        }
        seg_clouds[j] = cloud_cluster;
        j++;
    }

    if (print)
    {
        LOG(INFO) << " cluster classes:" << clusters.size();
    }

    // 3.分别计算每团点云几何中心，作为矩形角点
    corner_points.clear();
    for (int i = 0; i < clusters.size(); ++i)
    {
        if (seg_clouds.size() <= i || seg_clouds[i]->size() == 0)
            continue;
        float sumX = 0, sumY = 0;
        for (int j = 0; j < seg_clouds[i]->size(); ++j)
        {
            sumX += seg_clouds[i]->points[j].x;
            sumY += seg_clouds[i]->points[j].y;
        }

        float center_x = sumX / float(seg_clouds[i]->size());
        float center_y = sumY / float(seg_clouds[i]->size());
        corner_points.push_back(cv::Point2f(center_x, center_y));
    }
    /*char buf[255]={0};
    for (int k = 0; k < corner_points.size(); ++k) {
        sprintf(buf+strlen(buf), "point %d: (%.2f, %.2f)__", k, corner_points[k].x, corner_points[k].y);
    }
    LOG(WARNING) << buf;*/

    cv::RotatedRect t_rect=cv::minAreaRect(corner_points);
    //display(t_rect,cv::Scalar(255,0,0));
    return isRect(corner_points, print);
}
/**
 * 省略传出参数的四轮检测算法函数
 * 传入待检测点云
 * 返回值检测结果
 * */
Error_manager Region_detector::detect(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_in)
{
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered = pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>);
    // 1.预处理
    Error_manager result = preprocess(cloud_in, cloud_filtered);
    if (result == SUCCESS)
    {
        std::vector<cv::Point2f> corner_points;
        std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> seg_clouds;
        // 2.聚类与矩形校验
        result = clustering(cloud_filtered, corner_points, seg_clouds);
    }
    return result;
}


/**
 * ceres优化
 * 优化变量:中心点、角度、轮距与宽度的四轮点云检测函数
 * */
Error_manager Region_detector::detect(pcl::PointCloud<pcl::PointXYZ>::Ptr &cloud_in, double &x, double &y, double &c,
        double &wheelbase, double &width, double& front_wheel_theta,bool print)
{
    Error_manager result = Error_manager(Error_code::SUCCESS);
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered(pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>));
    // 1.预处理
    result = preprocess(cloud_in, cloud_filtered);
    if (result != SUCCESS)
        return result;

    // 2.聚类
    std::vector<pcl::PointCloud<pcl::PointXYZ>> seg_clouds;
    result = clustering_only(cloud_filtered, seg_clouds, print);
    if (result != SUCCESS)
        return result;
    
    cloud_in->clear();
    cloud_in->operator+=(*cloud_filtered);

    detect_wheel_ceres::Detect_result detect_result{x,y,c,wheelbase,width,front_wheel_theta};
    if(!m_detector_ceres.detect(seg_clouds, detect_result))
        return WJ_REGION_CERES_SOLVE_ERROR;
    else{
        x = detect_result.cx;
        y = detect_result.cy;
        c = detect_result.theta;
        wheelbase = detect_result.wheel_base;
        width = detect_result.width;
        front_wheel_theta = detect_result.front_theta;
    }

    return Error_manager(Error_code::SUCCESS);
}


/**
 * 输出中心点、角度、轮距与宽度的四轮点云检测函数
 * */
Error_manager Region_detector::detect(pcl::PointCloud<pcl::PointXYZ>::Ptr &cloud_in, double &x, double &y, double &c, double &wheelbase, double &width, bool print)
{
    Error_manager result = Error_manager(Error_code::SUCCESS);
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered(pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>));
    // 1.预处理
    result = preprocess(cloud_in, cloud_filtered);
    if (result != SUCCESS)
        return result;

    // 2.聚类计算角点
    std::vector<cv::Point2f> corner_points;
    std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> seg_clouds;
    result = clustering(cloud_filtered, corner_points, seg_clouds, print);
    if (result != SUCCESS)
        return result;
    // 修改原始点云为预处理后点云，并加入角点供查验
    cloud_in->clear();
    cloud_in->operator+=(*cloud_filtered);
    for (size_t i = 0; i < corner_points.size(); i++)
    {
        cloud_in->points.push_back(pcl::PointXYZ(corner_points[i].x, corner_points[i].y, 0.0));
    }

    // convert all points after preprocessed into 2d
    std::vector<cv::Point2f> all_points;
    // car center
    for (int j = 0; j < cloud_filtered->size(); ++j)
    {
        all_points.push_back(cv::Point2f(cloud_filtered->points[j].x, cloud_filtered->points[j].y));
    }
    // find bounding rectangle of all wheel points
    cv::RotatedRect wheel_box = cv::minAreaRect(all_points);
    x = wheel_box.center.x;
    y = wheel_box.center.y;
    // add center point to the input cloud for further check
    cloud_in->points.push_back(pcl::PointXYZ(x, y, 0.0));



    // 长边向量
    cv::Point2f vec;
    cv::Point2f vertice[4];
    wheel_box.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));
    c = angle_x;

    // get line formula, normal is (cos(theta), sin(theta)), towards the long side
    // the line formula is: nx * x + ny * y + (-(nx*cx+ny*cy)) = 0
    Eigen::Vector2f normal, center_point;
    normal << vec.x / sqrt(vec.x * vec.x + vec.y * vec.y), vec.y / sqrt(vec.x * vec.x + vec.y * vec.y);
    center_point << x, y;
    float line_param_c = -1 * center_point.transpose() * normal;
    // get rotation matrix, get the angle towards normal vector, rather than x axis
    // R = [ cos -sin]
    //     [ sin  cos]
    float rotate_angle = M_PI_2 - acos(vec.x / sqrt(vec.x * vec.x + vec.y * vec.y));
    Eigen::Matrix2f rotation_matrix;
    rotation_matrix << cos(rotate_angle), -sin(rotate_angle), sin(rotate_angle), cos(rotate_angle);

    // find min x and max x, separate y values according to the line, calculate difference between mean of y values as wheelbase
    float min_x = 20, max_x = 0;
    float y_values0 = 0, y_values1 = 0;
    int count0 = 0, count1 = 0;
    for (size_t i = 0; i < seg_clouds.size(); i++)
    {
        if (seg_clouds[i]->size() <= 0)
            continue;
        for (size_t j = 0; j < seg_clouds[i]->size(); j++)
        {
            // origin point and the point rotated around the origin
            Eigen::Vector2f vec, vec_rot;
            vec << seg_clouds[i]->points[j].x, seg_clouds[i]->points[j].y;
            vec_rot = rotation_matrix * (vec - center_point) + center_point;
            // find min max x
            if (vec_rot[0] < min_x)
                min_x = vec_rot[0];
            if (vec_rot[0] > max_x)
                max_x = vec_rot[0];
            // separate point as two clusters(front and back), calc y values respectively
            if (normal.transpose() * vec + line_param_c > 0)
            {
                y_values0 += vec_rot[1];
                count0 += 1;
            }
            else
            {
                y_values1 += vec_rot[1];
                count1 += 1;
            }
        }
    }
    // check if front and back both has points
    if (count0 > 0 && count1 > 0)
    {
        y_values0 /= count0;
        y_values1 /= count1;
        wheelbase = fabs(y_values1 - y_values0);
        width = fabs(min_x - max_x);
//        LOG(INFO) << "--------detector find width, wheelbase: " << width << ", " << wheelbase << ", y means: [" << y_values0 << ", " << y_values1 << "] -----";
        cloud_in->points.push_back(pcl::PointXYZ(min_x, y, 0.0));
        cloud_in->points.push_back(pcl::PointXYZ(max_x, y, 0.0));
        cloud_in->points.push_back(pcl::PointXYZ(x, y_values0, 0.0));
        cloud_in->points.push_back(pcl::PointXYZ(x, y_values1, 0.0));
    }
    else
    {
        // calculate wheelbase according to corner points
        // wheelbase = std::max(wheel_box.size.width, wheel_box.size.height);
        // LOG(INFO) << "--------detector find border: [" << wheel_box.size.width << ", " << wheel_box.size.height << "] -----";
        cv::RotatedRect wheel_center_box = cv::minAreaRect(corner_points);
        wheelbase = std::max(wheel_center_box.size.width, wheel_center_box.size.height);
        width = std::min(wheel_box.size.width, wheel_box.size.height);
    }

    return Error_manager(Error_code::SUCCESS);
}