puai_wj_2021/velodyne_lidar/ground_region.cpp

//
// Created by zx on 2021/5/20.
//

#include "ground_region.h"

#include <pcl/common/transforms.h>
#include <pcl/filters/statistical_outlier_removal.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/segmentation/extract_clusters.h>
#include <fcntl.h>

// 测量结果滤波,不影响现有结构
#include "../tool/measure_filter.h"

//欧式聚类*******************************************************
std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> Ground_region::segmentation(pcl::PointCloud<pcl::PointXYZ>::Ptr sor_cloud)
{
    std::vector<pcl::PointIndices> ece_inlier;
    pcl::search::KdTree<pcl::PointXYZ>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZ>);
    pcl::EuclideanClusterExtraction<pcl::PointXYZ> ece;
    ece.setInputCloud(sor_cloud);
    ece.setClusterTolerance(0.07);
    ece.setMinClusterSize(20);
    ece.setMaxClusterSize(20000);
    ece.setSearchMethod(tree);
    ece.extract(ece_inlier);

    std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> segmentation_clouds;
    for (int i = 0; i < ece_inlier.size(); i++)
    {
        pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_copy(new pcl::PointCloud<pcl::PointXYZ>);
        std::vector<int> ece_inlier_ext = ece_inlier[i].indices;
        copyPointCloud(*sor_cloud, ece_inlier_ext, *cloud_copy); //按照索引提取点云数据
        segmentation_clouds.push_back(cloud_copy);
    }
    return segmentation_clouds;
}

/**
 * @description: distance between two points
 * @param {Point2f} p1
 * @param {Point2f} p2
 * @return the distance
 */
double Ground_region::distance(cv::Point2f p1, cv::Point2f p2)
{
    return sqrt(pow(p1.x - p2.x, 2.0) + pow(p1.y - p2.y, 2.0));
}

/**
 * @description: point rectangle detect
 * @param  points detect if points obey the rectangle rule
 * @return wether forms a rectangle
 */
bool Ground_region::isRect(std::vector<cv::Point2f> &points)
{
    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];
        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_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];
            }
        }

        //直角边与坐标轴的夹角 <20°
        float thresh = 20.0 * M_PI / 180.0;
        cv::Point2f vct(pt.x - pc.x, pt.y - pc.y);
        float angle = atan2(vct.y, vct.x);
        if (!(fabs(angle) < thresh || (M_PI_2 - fabs(angle) < thresh)))
        {
            //std::cout<<" 4 wheel axis angle : "<<angle<<std::endl;
            return false;
        }

        double cosa = (l1 * l1 + l2 * l2 - max_l * max_l) / (2.0 * l1 * l2);
        if (fabs(cosa) >= 0.15)
        {
            /*char description[255]={0};
            sprintf(description,"angle cos value(%.2f) >0.13 ",cosa);
            std::cout<<description<<std::endl;*/
            return false;
        }

        float width = std::min(l1, l2);
        float length = std::max(l1, l2);
        if (width < 1.400 || width > 1.900 || length > 3.300 || length < 2.200)
        {
            /*char description[255]={0};
            sprintf(description,"width<1400 || width >1900 || length >3300 ||length < 2200 l:%.1f,w:%.1f",length,width);
            std::cout<<description<<std::endl;*/
            return false;
        }

        double d = distance(pc, points[3]);
        cv::Point2f center1 = (ps + pt) * 0.5;
        cv::Point2f center2 = (pc + points[3]) * 0.5;
        if (fabs(d - max_l) > max_l * 0.1 || distance(center1, center2) > 0.150)
        {
            /*std::cout << "d:" << d << " maxl:" << max_l << "  center1:" << center1 << "  center2:" << center2<<std::endl;
            char description[255]={0};
            sprintf(description,"Verify failed-4  fabs(d - max_l) > max_l * 0.1 || distance(center1, center2) > 0.150 ");
            std::cout<<description<<std::endl;*/
            return false;
        }
        //std::cout << "d:" << d << " maxl:" << max_l << "  center1:" << center1 << "  center2:" << center2<<std::endl;

        return true;
    }
    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];
            }
        }

        //直角边与坐标轴的夹角 <20°
        float thresh = 20.0 * M_PI / 180.0;
        cv::Point2f vct(pt.x - pc.x, pt.y - pc.y);
        float angle = atan2(vct.y, vct.x);
        if (!(fabs(angle) < thresh || (M_PI_2 - fabs(angle) < thresh)))
        {
            //std::cout<<" 4 wheel axis angle : "<<angle<<std::endl;
            return false;
        }

        double cosa = (l1 * l1 + l2 * l2 - max_l * max_l) / (2.0 * l1 * l2);
        if (fabs(cosa) >= 0.15)
        {
            /*char description[255]={0};
            sprintf(description,"3 wheels angle cos value(%.2f) >0.13 ",cosa);
            std::cout<<description<<std::endl;*/
            return false;
        }

        double l = std::max(l1, l2);
        double w = std::min(l1, l2);
        if (l > 2.100 && l < 3.300 && w > 1.400 && w < 2.100)
        {
            //生成第四个点
            cv::Point2f vec1 = ps - pc;
            cv::Point2f vec2 = pt - pc;
            cv::Point2f point4 = (vec1 + vec2) + pc;
            points.push_back(point4);

            /*char description[255]={0};
            sprintf(description,"3 wheels rectangle cos angle=%.2f,L=%.1f, w=%.1f",cosa,l,w);
            std::cout<<description<<std::endl;*/
            return true;
        }
        else
        {

            /*char description[255]={0};
            sprintf(description,"3 wheels rectangle verify Failed  cos angle=%.2f,L=%.1f, w=%.1f",cosa,l,w);
            std::cout<<description<<std::endl;*/
            return false;
        }
    }
    //std::cout<<" default false"<<std::endl;
    return false;
}

/**
 * @description: 3d wheel detect core func
 * @param cloud  input cloud for measure
 * @param thresh_z  z value to cut wheel
 * @param result  detect result
 * @return wether successfully detected
 */
bool Ground_region::classify_ceres_detect(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, double thresh_z,
                                          detect_wheel_ceres3d::Detect_result &result)
{
    if (m_detector == nullptr)
        return false;
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered(new pcl::PointCloud<pcl::PointXYZ>);
    for (int i = 0; i < cloud->size(); ++i)
    {
        pcl::PointXYZ pt = cloud->points[i];
        if (pt.z < thresh_z)
        {
            cloud_filtered->push_back(pt);
        }
    }
    //下采样
    pcl::VoxelGrid<pcl::PointXYZ> vox;   //创建滤波对象
    vox.setInputCloud(cloud_filtered);   //设置需要过滤的点云给滤波对象
    vox.setLeafSize(0.02f, 0.02f, 0.2f); //设置滤波时创建的体素体积为1cm的立方体
    vox.filter(*cloud_filtered);         //执行滤波处理，存储输出
    if (cloud_filtered->size() == 0)
    {
        return false;
    }

    if (cloud_filtered->size() == 0)
        return false;
    pcl::StatisticalOutlierRemoval<pcl::PointXYZ> sor; //创建滤波器对象
    sor.setInputCloud(cloud_filtered);                 //设置待滤波的点云
    sor.setMeanK(5);                                   //设置在进行统计时考虑的临近点个数
    sor.setStddevMulThresh(3.0);                       //设置判断是否为离群点的阀值，用来倍乘标准差，也就是上面的std_mul
    sor.filter(*cloud_filtered);                       //滤波结果存储到cloud_filtered

    if (cloud_filtered->size() == 0)
    {
        return false;
    }

    std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> seg_clouds;
    seg_clouds = segmentation(cloud_filtered);

    if (!(seg_clouds.size() == 4 || seg_clouds.size() == 3))
    {
        return false;
    }
    std::vector<cv::Point2f> centers;
    for (int i = 0; i < seg_clouds.size(); ++i)
    {
        Eigen::Vector4f centroid;
        pcl::compute3DCentroid(*seg_clouds[i], centroid);
        centers.push_back(cv::Point2f(centroid[0], centroid[1]));
    }
    bool ret = isRect(centers);
    if (ret)
    {

        std::string error_str;
        if (m_detector->detect(seg_clouds, result, error_str))
        {
            return true;
        }
        else
        {
            LOG(WARNING) << error_str;
            return false;
        }
    }
    return ret;
}

// constructor
Ground_region::Ground_region()
{
    m_region_status = E_UNKNOWN;
    m_detector = nullptr;
    m_measure_thread = nullptr;
}

// deconstructor
Ground_region::~Ground_region()
{
    if(m_measure_thread){
        m_measure_condition.kill_all();
        // Close Capturte Thread
        if (m_measure_thread->joinable())
        {
            m_measure_thread->join();
            delete m_measure_thread;
            m_measure_thread = nullptr;
        }
    }
}

Error_manager Ground_region::init(velodyne::Region region, pcl::PointCloud<pcl::PointXYZ>::Ptr left_model, pcl::PointCloud<pcl::PointXYZ>::Ptr right_model)
{
    m_region = region;
    m_detector = new detect_wheel_ceres3d(left_model,right_model);
    mp_cloud_collection = pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>);
    mp_cloud_filtered = pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>);
    m_measure_thread = new std::thread(&Ground_region::thread_measure_func, this);
    m_measure_condition.reset();
    m_region_status = E_READY;
    return SUCCESS;
}

// 计算均方误差
bool computer_var(std::vector<double> data, double &var)
{
    if (data.size() == 0)
        return false;
    Eigen::VectorXd dis_vec(data.size());
    for (int i = 0; i < data.size(); ++i)
    {
        dis_vec[i] = data[i];
    }
    double mean = dis_vec.mean();
    Eigen::VectorXd mean_vec(data.size());
    Eigen::VectorXd mat = dis_vec - (mean_vec.setOnes() * mean);
    Eigen::MatrixXd result = (mat.transpose()) * mat;
    var = sqrt(result(0) / double(data.size()));
    return true;
}

Error_manager Ground_region::detect(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, detect_wheel_ceres3d::Detect_result &last_result)
{
    if (cloud->size() == 0)
        return Error_manager(VELODYNE_REGION_EMPTY_CLOUD, NORMAL, "no point");

    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered(new pcl::PointCloud<pcl::PointXYZ>);
    for (int i = 0; i < cloud->size(); ++i)
    {
        pcl::PointXYZ pt = cloud->points[i];
        if (pt.x > m_region.minx() && pt.x < m_region.maxx() && pt.y > m_region.miny() && pt.y < m_region.maxy() && pt.z > m_region.minz() && pt.z < m_region.maxz())
        {
            cloud_filtered->push_back(pt);
        }
    }
    if (cloud_filtered->size() == 0)
        return Error_manager(VELODYNE_REGION_EMPTY_CLOUD, NORMAL, "filtered no point");

    m_filtered_cloud_mutex.lock();
    mp_cloud_filtered->clear();
    mp_cloud_filtered->operator+=(*cloud_filtered);
    m_filtered_cloud_mutex.unlock();

    float start_z = m_region.minz();
    float max_z = 0.2;
    float center_z = (start_z + max_z) / 2.0;
    float last_center_z = start_z;
    float last_succ_z = -1.0;
    int count = 0;
    //二分法 找识别成功的 最高的z
    std::vector<detect_wheel_ceres3d::Detect_result> results;
    do
    {
        detect_wheel_ceres3d::Detect_result result;
        bool ret = classify_ceres_detect(cloud_filtered, center_z, result);
        // std::cout << "z: " << center_z <<", "<<start_z<<"," << max_z <<(ret?"clustered":"clustering failed")<< std::endl;
        if (ret)
        {
            results.push_back(result);
            last_succ_z = center_z;
            start_z = center_z;
            last_center_z = center_z;
        }
        else
        {
            max_z = center_z;
            last_center_z = center_z;
        }
        center_z = (start_z + max_z) / 2.0;
        count++;

    } while (fabs(center_z - last_center_z) > 0.01);

    //
    if (results.size() == 0)
    {
        return Error_manager(FAILED, NORMAL, "no car detected");
    }
    ///  to be
    float min_mean_loss = 1.0;
    for (int i = 0; i < results.size(); ++i)
    {
        detect_wheel_ceres3d::Detect_result result = results[i];
        std::vector<double> loss;
        loss.push_back(result.loss.lf_loss);
        loss.push_back(result.loss.rf_loss);
        loss.push_back(result.loss.lb_loss);
        loss.push_back(result.loss.rb_loss);
        double mean = (result.loss.lf_loss + result.loss.rf_loss + result.loss.lb_loss + result.loss.rb_loss) / 4.0;
        double var = -1.;
        computer_var(loss, var);
        if (mean < min_mean_loss)
        {
            last_result = result;
            min_mean_loss = mean;
        }
    }
    // printf("z : %.3f  angle : %.3f front : %.3f wheel_base:%.3f,width:%.3f, mean:%.5f\n",
    //        center_z, last_result.theta, last_result.front_theta, last_result.wheel_base, last_result.width,
    //        min_mean_loss);
    //m_detector->save_debug_data("/home/zx/zzw/catkin_ws/src/feature_extra/debug");
    return SUCCESS;
}

//外部调用获取当前车轮定位信息, 获取指令时间之后的车轮定位信息, 如果没有就会报错, 不会等待
Error_manager Ground_region::get_current_wheel_information(Common_data::Car_wheel_information *p_car_wheel_information, std::chrono::system_clock::time_point command_time)
{
    if ( p_car_wheel_information == NULL )
	{
	    return Error_manager(Error_code::POINTER_IS_NULL, Error_level::MINOR_ERROR,
	    					"  POINTER IS NULL ");
	}
	//获取指令时间之后的信息, 如果没有就会报错, 不会等待
	if( m_detect_update_time > command_time )
	{
		*p_car_wheel_information = m_car_wheel_information;
		if(m_car_wheel_information.correctness)
		    return Error_code::SUCCESS;
		else
		    return Error_manager(Error_code::VELODYNE_REGION_CERES_SOLVE_ERROR, Error_level::MINOR_ERROR, " Ground_region detect error");
	}
	else
	{
		return Error_manager(Error_code::VELODYNE_REGION_EMPTY_NO_WHEEL_INFORMATION, Error_level::MINOR_ERROR,
							 " Ground_region::get_current_wheel_information error ");
	}
}

//外部调用获取最新的车轮定位信息, 获取指令时间往前一个周期内的车轮定位信息, 如果没有就会报错, 不会等待
Error_manager Ground_region::get_last_wheel_information(Common_data::Car_wheel_information *p_car_wheel_information, std::chrono::system_clock::time_point command_time)
{
    if ( p_car_wheel_information == NULL )
	{
	    return Error_manager(Error_code::POINTER_IS_NULL, Error_level::MINOR_ERROR,
	    					"  POINTER IS NULL ");
	}
	//获取指令时间之后的信息, 如果没有就会报错, 不会等待
//    LOG(WARNING) << std::chrono::duration_cast<std::chrono::milliseconds>(command_time-m_detect_update_time).count()/1000.0 <<", "
//                <<std::chrono::duration_cast<std::chrono::milliseconds>(command_time-m_cloud_collection_time).count()/1000.0;
	if( m_detect_update_time > command_time - std::chrono::milliseconds(GROUND_REGION_DETECT_CYCLE_MS))
	{
		*p_car_wheel_information = m_car_wheel_information;
        if(m_car_wheel_information.correctness)
            return Error_code::SUCCESS;
        else
            return Error_manager(Error_code::VELODYNE_REGION_CERES_SOLVE_ERROR, Error_level::MINOR_ERROR, " Ground_region detect error");
	}
	else
	{
		return Error_manager(Error_code::VELODYNE_REGION_EMPTY_NO_WHEEL_INFORMATION, Error_level::MINOR_ERROR,
							 " Ground_region::get_current_wheel_information error ");
	}
}

Error_manager Ground_region::update_cloud(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud)
{
    // // 点云z转90度，调试用
    //Eigen::AngleAxisd rot_z = Eigen::AngleAxisd(M_PI_2, Eigen::Vector3d::UnitZ());
    //for (size_t i = 0; i < cloud->size(); i++)
    //{
    //    Eigen::Vector3d t_point(cloud->points[i].x, cloud->points[i].y, cloud->points[i].z);
    //    t_point = rot_z.toRotationMatrix() * t_point;
    //    cloud->points[i].x = t_point.x();
    //    cloud->points[i].y = t_point.y();
    //    cloud->points[i].z = t_point.z();
    //}

    std::lock_guard<std::mutex> lck(m_cloud_collection_mutex);
    mp_cloud_collection = cloud;
    // LOG(WARNING) << "update region cloud size: " << mp_cloud_collection->size() << ",,, input size: " << cloud->size();
    m_cloud_collection_time = std::chrono::system_clock::now();
    m_measure_condition.notify_one(false, true);
    return SUCCESS;
}

void Ground_region::thread_measure_func()
{
    while (m_measure_condition.is_alive())
    {
        m_measure_condition.wait();
        if (m_measure_condition.is_alive())
        {
            m_region_status = E_BUSY;
            detect_wheel_ceres3d::Detect_result t_result;

            Error_manager ec = detect(mp_cloud_collection, t_result);
            m_detect_update_time = std::chrono::system_clock::now();

            m_car_wheel_information.center_x = t_result.cx;
            m_car_wheel_information.center_y = t_result.cy;
            m_car_wheel_information.car_angle = t_result.theta;
            m_car_wheel_information.wheel_base = t_result.wheel_base;
            m_car_wheel_information.wheel_width = t_result.width;
            m_car_wheel_information.front_theta = t_result.front_theta;
            if (ec == SUCCESS)
            {
                m_car_wheel_information.correctness = true;
                Common_data::Car_wheel_information_stamped t_wheel_info_stamped;
				t_wheel_info_stamped.wheel_data = m_car_wheel_information;
				t_wheel_info_stamped.measure_time = m_detect_update_time;
				Measure_filter::get_instance_references().update_data(m_region.region_id(), t_wheel_info_stamped);
            }
            else
            {
                m_car_wheel_information.correctness = false;
                // LOG(ERROR) << ec.to_string();
            }
        }
        m_region_status = E_READY;
    }
}