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.01f, 0.01f, 0.01f); //设置滤波时创建的体素体积为1cm的立方体
    vox.filter(*cloud_filtered);         //执行滤波处理，存储输出
    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;
    cv::Point2f temp_centers(0,0);
    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]));
        temp_centers += cv::Point2f(centroid[0], centroid[1]);
    }
    temp_centers /= 4.0f;
    bool ret = isRect(centers);
    if (ret)
    {
        std::string error_str;
        // LOG(WARNING) << "region id: "<< m_region.region_id();
        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()
{
    // LOG(WARNING) << "start deconstruct 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;
        }
    }
    // LOG(WARNING) << "thread released";
    // 将创建的检测器析构
    if(m_detector)
    {
        delete m_detector;
        m_detector = nullptr;
    }
    // LOG(WARNING) << "detector released";
}

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>);
    mp_cloud_detect_z = 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)
{
    // 1.*********点云合法性检验*********
    if (cloud->size() == 0){
        // 更新过滤点
        m_filtered_cloud_mutex.lock();
        mp_cloud_filtered->clear();
        m_filtered_cloud_mutex.unlock();
        return Error_manager(VELODYNE_REGION_EMPTY_CLOUD, NEGLIGIBLE_ERROR, "no input point");
    }

    // 2.*********点云预处理*********
    std::chrono::steady_clock::time_point t1 = std::chrono::steady_clock::now();
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_cut(new pcl::PointCloud<pcl::PointXYZ>);
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_clean(new pcl::PointCloud<pcl::PointXYZ>);
    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_cut->push_back(pt);
        }
    }
    if(cloud_cut->size() <= 0)
    {
        // 更新过滤点
        m_filtered_cloud_mutex.lock();
        mp_cloud_filtered->clear();
        m_filtered_cloud_mutex.unlock();
        return Error_manager(VELODYNE_REGION_EMPTY_CLOUD, NEGLIGIBLE_ERROR, "cut no point");
    }

    //下采样
    pcl::VoxelGrid<pcl::PointXYZ> vox;   //创建滤波对象
    vox.setInputCloud(cloud_cut);   //设置需要过滤的点云给滤波对象
    vox.setLeafSize(0.02f, 0.02f, 0.02f); //设置滤波时创建的体素体积为1cm的立方体
    vox.filter(*cloud_clean);         //执行滤波处理，存储输出
    // cloud_filtered = cloud_clean;
    if (cloud_clean->size() == 0){
        // 更新过滤点
        m_filtered_cloud_mutex.lock();
        mp_cloud_filtered->clear();
        m_filtered_cloud_mutex.unlock();
        return Error_manager(VELODYNE_REGION_EMPTY_CLOUD, NEGLIGIBLE_ERROR, "ApproximateVoxelGrid no point");
    }

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

    if (cloud_filtered->size() == 0){
        // 更新过滤点
        m_filtered_cloud_mutex.lock();
        mp_cloud_filtered->clear();
        m_filtered_cloud_mutex.unlock();
        return Error_manager(VELODYNE_REGION_EMPTY_CLOUD, NEGLIGIBLE_ERROR, "StatisticalOutlierRemoval no point");
    }

    // 更新过滤点
    m_filtered_cloud_mutex.lock();
    mp_cloud_filtered->clear();
    mp_cloud_filtered->operator+=(*cloud_filtered);
    m_filtered_cloud_mutex.unlock();

    // 3.*********位姿优化，获取中心xy与角度*********
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_detect_z(new pcl::PointCloud<pcl::PointXYZ>);
    double car_pose_x, car_pose_y, car_pose_theta, car_pose_width, z_value=0.2;
    // if(!Car_pose_detector::get_instance_references().detect_pose(cloud_filtered, cloud_detect_z, car_pose_x, car_pose_y, car_pose_theta, car_pose_width, z_value, false))
    if(!Car_pose_detector::get_instance_references().detect_pose_mat(cloud_filtered, cloud_detect_z, car_pose_x, car_pose_y, car_pose_theta, false))
    {
        return Error_manager(VELODYNE_REGION_CERES_SOLVE_ERROR, NEGLIGIBLE_ERROR, "find general car pose value failed.");
    }
    // LOG_IF(INFO, m_region.region_id() == 0) << "car pose :::: cx: " << car_pose_x+m_region.plc_offsetx() << ", cy: " << car_pose_y+m_region.plc_offsety() << ", theta: " << car_pose_theta*180.0/M_PI << std::endl;

    std::chrono::steady_clock::time_point t2 = std::chrono::steady_clock::now();
    std::chrono::duration<double> time_used_bowl = std::chrono::duration_cast<std::chrono::duration<double>>(t2 - t1);

    // 4.*********xoz优化获取底盘高度*********
    // 重新取包含地面点的点云,用于底盘优化
    double z_solver_x = car_pose_x;
    double z_solver_y = car_pose_y;
    double z_solver_theta = car_pose_theta;
    double z_solver_width = 1.0;
    chassis_ceres_solver t_solver;
    // ！！！重新筛选地面点并滤波，融入第一步位姿使用的滤波后点云，传入第二步获取底盘高度
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_z_solver(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())
        {
            cloud_z_solver->push_back(pt);
        }
    }
    //离群点过滤
    sor.setInputCloud(cloud_z_solver);
    sor.setMeanK(5);            //K近邻搜索点个数
    sor.setStddevMulThresh(3.0); //标准差倍数
    sor.setNegative(false);      //保留未滤波点（内点）
    sor.filter(*cloud_z_solver); //保存滤波结果到cloud_filter
    // 补上车身与车轮点
    cloud_z_solver->operator+=(*mp_cloud_filtered);
    // 去中心，角度调正
    Car_pose_detector::get_instance_references().inv_trans_cloud(cloud_z_solver, z_solver_x, z_solver_y, z_solver_theta);

    if (cloud_z_solver->size() == 0)
        return Error_manager(VELODYNE_REGION_EMPTY_CLOUD, NORMAL, "z solver no point");

    // 给予底盘z中心与高度初值
    double mid_z = 0.05, height = 0.08;
    z_solver_x = 0.0;
    // Error_manager ec = t_solver.solve(cloud_z_solver, z_solver_x, mid_z, z_solver_width, height);
    Error_manager ec = t_solver.solve_mat(cloud_z_solver, z_solver_x, mid_z, z_solver_width, height, false);
    // 切除大于height高度以外点，并显示width直线
    // 根据z值切原始点云
    pcl::PassThrough<pcl::PointXYZ> pass;
    pass.setInputCloud(cloud_z_solver);
    pass.setFilterFieldName("z");
    pass.setFilterLimits(m_region.minz(), mid_z + height / 2.0);
    pass.setFilterLimitsNegative(false);
    pass.filter(*cloud_z_solver);
    // // 车宽方向画线
    // for (double i = -3.0; i < 3.0; i+=0.02)
    // {
    //     cloud_z_solver->push_back(pcl::PointXYZ(-width/2.0, i, 0));
    //     cloud_z_solver->push_back(pcl::PointXYZ(width/2.0, i, 0));
    // }
    // std::cout << "\n------------------------------------ chassis z1: " << mid_z + height / 2.0 << ", mid z: "<< mid_z << std::endl;

    if(ec != SUCCESS)
        return Error_manager(VELODYNE_REGION_CERES_SOLVE_ERROR, NORMAL, "failed to find chassis z value.");
    std::chrono::steady_clock::time_point t3 = std::chrono::steady_clock::now();
    std::chrono::duration<double> time_used_block = std::chrono::duration_cast<std::chrono::duration<double>>(t3 - t2);

    // 更新z中间点
    m_detect_z_cloud_mutex.lock();
    mp_cloud_detect_z->clear();
    mp_cloud_detect_z->operator+=(*cloud_z_solver);
    m_detect_z_cloud_mutex.unlock();

    // 二分法存在错误弃用！！！直接使用底盘z值
    std::vector<detect_wheel_ceres3d::Detect_result> results;
    detect_wheel_ceres3d::Detect_result result;
    double chassis_z = mid_z + height / 2.0; // + 0.02;
    if(chassis_z > m_region.maxz() || chassis_z < m_region.minz())
    {
        return Error_manager(VELODYNE_REGION_CERES_SOLVE_ERROR, NEGLIGIBLE_ERROR, (std::string("optimized chassis z value out of range: ")+std::to_string(chassis_z)).c_str());
    }
    bool ret = false;

    while (chassis_z > (mid_z - height / 2.0))
    {
        // LOG_IF(WARNING, m_region.region_id() == 0) << "chassis z: " << chassis_z << ", midz: " << mid_z - height / 2.0;
        // 初值中x使用第一步优化的值
        result.cx = car_pose_x;
        ret = classify_ceres_detect(cloud_filtered, chassis_z, result);
        // changed by yct, 根据底盘z识别，失败则向下移动直至成功或超过底盘内空中点
        if(ret)
        {
            results.push_back(result);
            break;
        }else{
            chassis_z -= 0.01;
        }
    }

    // } while (fabs(center_z - last_center_z) > 0.01);
    std::chrono::steady_clock::time_point t4 = std::chrono::steady_clock::now();
    std::chrono::duration<double> time_used_div = std::chrono::duration_cast<std::chrono::duration<double>>(t4 - t3);
    // std::cout << "\n------------------------------------------------------------ " << std::to_string(time_used_bowl.count()) << " " << std::to_string(time_used_block.count()) << " " << std::to_string(time_used_div.count())<<" || "<< std::to_string(time_used_bowl.count()+time_used_block.count()+time_used_div.count())<< std::endl;

    if (results.size() == 0)
    {
        // std::cout << "\n-------- no result: " << std::endl;
        //LOG_IF(INFO, m_region.region_id() == 4) << "failed with midz, currz: " << mid_z << ", " << chassis_z;
        return Error_manager(VELODYNE_REGION_CERES_SOLVE_ERROR, NEGLIGIBLE_ERROR, "3d wheel detect failed.");
    }
    else
    {
        // // 20201010, may lead to problem in chutian, uncomment in debug only
        // // changed by yct, save 3d wheel detect result. 
        // static int save_debug = 0;
        // if (m_region.region_id() == 0 && save_debug++ == 5)
        //     m_detector->save_debug_data("/home/zx/yct/chutian_measure_2021/build");
        // LOG_IF(INFO, m_region.region_id() == 4) << "detected with suitable z value: " << chassis_z;
    }
    ///  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);
    // std::cout << "\n-------- final z: " << chassis_z << std::endl;
    // std::cout << "cx: " << last_result.cx << ", cy: " << last_result.cy << ", theta: " << last_result.theta
    //           << ", front: " << last_result.front_theta << ", wheelbase: " << last_result.wheel_base << ", width: " << last_result.width << std::endl << std::endl;

    // last_result.cx -= x;
    // last_result.cy -= y;
    // last_result.theta -= theta;

    // 角度校验，理论上car pose检测使用车身点云，应与车轮点云识别得到的车身 x偏移(0.025)与角度(<1°)一致
    double car_pose_theta_deg = 90 - car_pose_theta * 180.0 / M_PI;
    if (fabs(car_pose_x - last_result.cx) > 0.025 || fabs(car_pose_theta_deg - last_result.theta) > 1)
    {
        char valid_info[255];
        sprintf(valid_info, "validation failed for x and theta, carpose: (%.3f, %.3f), result: (%.3f, %.3f)", car_pose_x, car_pose_theta_deg, last_result.cx, last_result.theta);
        return Error_manager(VELODYNE_REGION_CERES_SOLVE_ERROR, NEGLIGIBLE_ERROR, valid_info);
    }

    // LOG_IF(INFO, m_region.region_id() == 4) << last_result.to_string();

    return SUCCESS;
}

int Ground_region::outOfRangeDetection(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, double plate_cx, double plate_cy, double theta)
{
    if(cloud->size() <= 0)
        return Range_status::Range_correct;
    int res = Range_status::Range_correct;
    // 计算转盘旋转后点云坐标
    double theta_rad = theta * M_PI / 180.0;
    pcl::PointCloud<pcl::PointXYZ> t_cloud;
    for (size_t i = 0; i < cloud->size(); i++)
    {
        Eigen::Vector2d t_point(cloud->points[i].x - plate_cx, cloud->points[i].y - plate_cy);
        pcl::PointXYZ t_pcl_point;
        t_pcl_point.x = (t_point.x() * cos(theta_rad) - t_point.y() * sin(theta_rad)) + plate_cx;
        t_pcl_point.y = (t_point.x() * sin(theta_rad) + t_point.y() * cos(theta_rad)) + plate_cy;
        t_pcl_point.z = cloud->points[i].z;
        t_cloud.push_back(t_pcl_point);
    }
    pcl::PointXYZ min_p, max_p;
    pcl::getMinMax3D(t_cloud, min_p, max_p);
    // 判断左右超界情况
    if(min_p.x < m_region.border_minx())
        res |= Range_status::Range_left;
    if(max_p.x > m_region.border_maxx())
        res |= Range_status::Range_right;

    // LOG_IF(WARNING, m_region.region_id() == 4)<< "border minmax x: "<< min_p.x<<", "<<max_p.x<<", res: "<<res;
    return res;
}

int Ground_region::outOfRangeDetection(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, Common_data::Car_wheel_information measure_result, double plate_cx, double plate_cy, double theta)
{
    int res = Range_status::Range_correct;
    if(cloud->size() <= 0)
        return res;

    // 计算转盘旋转后车辆中心点坐标
    Eigen::Vector2d car_center(measure_result.car_center_x, measure_result.car_center_y);
    Eigen::Vector2d car_uniform_center;
    double theta_rad = theta * M_PI / 180.0;
    car_uniform_center.x() = car_center.x() * cos(theta_rad) - car_center.y() * sin(theta_rad);
    car_uniform_center.y() = car_center.x() * sin(theta_rad) + car_center.y() * cos(theta_rad);
    // 车位位于y负方向，判断后轮超界情况
    double rear_wheel_y = car_uniform_center.y() + m_region.plc_offsety() - 0.5 * measure_result.car_wheel_base;
    if(rear_wheel_y < m_region.plc_border_miny())
    {
        res |= Range_status::Range_back;
    }
    // 判断车辆宽度超界情况
    if (measure_result.car_wheel_width < m_region.car_min_width() || measure_result.car_wheel_width > m_region.car_max_width())
    {
        res |= Range_status::Range_car_width;
    }
    // 判断车辆轴距超界情况
    if (measure_result.car_wheel_base < m_region.car_min_wheelbase() || measure_result.car_wheel_base > m_region.car_max_wheelbase())
    {
        res |= Range_status::Range_car_wheelbase;
    }
    // 判断车辆旋转角超界情况
    double dtheta = 90-measure_result.car_angle;
    if (dtheta > m_region.turnplate_angle_limit_anti_clockwise())
    {
        res |= Range_status::Range_angle_clock;
    }
    if (dtheta < -m_region.turnplate_angle_limit_clockwise())
    {
        res |= Range_status::Range_angle_anti_clock;
    }
    // // 判断车辆前轮角回正情况
    // if (fabs(measure_result.car_front_theta) > 8.0)
    // {
    //     res |= Range_status::Range_steering_wheel_nozero;
    // }
    res |= outOfRangeDetection(cloud, plate_cx, plate_cy, theta);
    return res;
}

//外部调用获取当前车轮定位信息, 获取指令时间之后的车轮定位信息, 如果没有就会报错, 不会等待
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 )
	{
        std::lock_guard<std::mutex> lck(m_car_result_mutex);
		*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))
	{
        std::lock_guard<std::mutex> lck(m_car_result_mutex);
        *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();
    //}
    // 限定锁区域，解决因条件变量在析构时无法获得内部mutex导致此互斥锁卡住，造成死锁无法结束程序的问题。
    {
        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);

    // std::chrono::steady_clock::time_point t1 = std::chrono::steady_clock::now();
    // std::chrono::duration<double> time_used_update = std::chrono::duration_cast<std::chrono::duration<double>>(t1 - t0);
    // std::cout << "update cloud time: " << time_used_update.count() << std::endl;
    return SUCCESS;
}

void Ground_region::thread_measure_func()
{
    LOG(INFO) << " Ground_region::thread_measure_func() start " << this;
    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);
      
            std::lock_guard<std::mutex> lck(m_car_result_mutex);
            // 增加滤波轴距
            Common_data::Car_wheel_information_stamped t_wheel_info_stamped;
            if (ec == SUCCESS)
            {
                m_car_wheel_information.correctness = true;
                m_car_wheel_information.car_center_x = t_result.cx;
                m_car_wheel_information.car_center_y = t_result.cy;
                m_car_wheel_information.car_angle = t_result.theta;
                m_car_wheel_information.car_wheel_base = t_result.wheel_base;
                m_car_wheel_information.car_wheel_width = t_result.width;
                m_car_wheel_information.car_front_theta = t_result.front_theta;
                m_car_wheel_information.theta_uniform(m_region.turnplate_cx(), m_region.turnplate_cy());
                // 超界校验
                {
                    std::lock_guard<std::mutex> lck(m_filtered_cloud_mutex);
                    int res = outOfRangeDetection(mp_cloud_filtered, m_car_wheel_information, m_region.turnplate_cx(), m_region.turnplate_cy(), 90.0 - t_result.theta);
                    m_car_wheel_information.range_status = res;
                }
                // 添加plc偏移
                m_car_wheel_information.car_center_x += m_region.plc_offsetx();
                m_car_wheel_information.car_center_y += m_region.plc_offsety();
                m_car_wheel_information.uniform_car_x += m_region.plc_offsetx();
                m_car_wheel_information.uniform_car_y += m_region.plc_offsety();
                m_car_wheel_information.car_angle += m_region.plc_offset_degree();

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

                ec = Measure_filter::get_instance_references().get_filtered_wheel_information(m_region.region_id(), t_wheel_info_stamped.wheel_data);
                if (ec == SUCCESS)
                {
                    m_car_wheel_information.car_wheel_base = t_wheel_info_stamped.wheel_data.car_wheel_base;
                    m_car_wheel_information.car_front_theta = t_wheel_info_stamped.wheel_data.car_front_theta;
                    // 临时添加，滤波后前轮超界
                    if(fabs(m_car_wheel_information.car_front_theta)>8.0)
                    {
                        m_car_wheel_information.range_status |= Range_status::Range_steering_wheel_nozero;
                    }
                }
                // else{
                //     std::cout<<ec.to_string()<<std::endl;
                // }
                 //LOG_IF(INFO, m_region.region_id() == 4) << m_car_wheel_information.to_string();
            }
            else
            {
                m_car_wheel_information.correctness = false;
                // LOG_IF(ERROR, m_region.region_id() == 0) << ec.to_string();
            }
            m_detect_update_time = std::chrono::system_clock::now();
        }
        m_region_status = E_READY;
    }
    LOG(INFO) << " Ground_region::thread_measure_func() end " << this;
}