#include "ground_region.h"

std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr>
Ground_region::segmentation(pcl::PointCloud<pcl::PointXYZ>::Ptr &sor_cloud, const float &tolerance) {
    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(tolerance);
    ece.setMinClusterSize(20);
    ece.setMaxClusterSize(sor_cloud->size());
    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;
}

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) {
            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) {
            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) {
            return false;
        }
        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))) {
            return false;
        }

        double cosa = (l1 * l1 + l2 * l2 - max_l * max_l) / (2.0 * l1 * l2);
        if (fabs(cosa) >= 0.15) {
            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);
            return true;
        } else {
            return false;
        }
    }
    return false;
}

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

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

    Region_status_checker::get_instance_references().Region_status_checker_uninit();
    // 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) {
    Region_status_checker::get_instance_references().Region_status_checker_init();
    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_detect_failture_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_surface_hull = 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;

    //创建多边形区域指针
    pcl::PointCloud<pcl::PointXYZ>::Ptr boundingbox_ptr(new pcl::PointCloud<pcl::PointXYZ>);
    printf("%d\n", m_region.cut_box().cut_points_size());
    for (auto &point: m_region.cut_box().cut_points()) {
        boundingbox_ptr->push_back(pcl::PointXYZ(point.x(), point.y(), point.z()));
    }

    pcl::ConvexHull<pcl::PointXYZ> hull;                    //建立一个凸包对象
    hull.setInputCloud(boundingbox_ptr);                //设置凸包的维度
    hull.reconstruct(*m_surface_hull, m_polygons); 				//计算凸包结果

    return SUCCESS;
}

bool Ground_region::classify_ceres_detect(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, double thresh_z,
                                          detect_wheel_ceres3d::Detect_result &result, std::string &error) {
    if (m_detector == nullptr || cloud->empty()) {
        return false;
    }

    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered(new pcl::PointCloud<pcl::PointXYZ>);
    copyPointCloud(*cloud, *cloud_filtered);

    // 滤波
    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->empty()) {
        return false;
    }

    // 欧式聚类
    std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> seg_clouds;
    seg_clouds = segmentation(cloud_filtered, 0.3);
//    if (!(seg_clouds.size() == 4 || seg_clouds.size() == 3)) {
//        return false;
//    }
    if (seg_clouds.size() != 4) {
        error.append("seg_clouds not is 4, it's " + std::to_string(seg_clouds.size()));
        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;
        // WriteTxtCloud(ETC_PATH"MeasureNode/data/" + std::to_string(m_region.region_id()) + "/seg_clouds" + std::to_string(i) + ".txt", seg_clouds[i]);
        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);

    // ceres库寻优
    if (ret) {
        std::string error_str;
        if (m_detector->detect(seg_clouds, result, error_str)) {
            return true;
        } else {
            error.append(error_str);
            return false;
        }
    } else {
        error.append("not is rect.");
    }
    return ret;
}

#define WriteCloud 0
#define DebugTime 0
Error_manager
Ground_region::detect(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, detect_wheel_ceres3d::Detect_result &last_result) {
#if WriteCloud
    if (m_region.region_id() == 3117) {
        cloud->clear();
        ReadTxtCloud(ETC_PATH"MeasureNode/data/" + std::to_string(m_region.region_id()) + "/total.txt", cloud);
        LOG(INFO) << cloud->size();

//        pcl::PointCloud<pcl::PointXYZ>::Ptr ssscloud_cut(new pcl::PointCloud<pcl::PointXYZ>);
//        float d_theta = 0;
//        d_theta = -(90 + -100) * M_PI / 180.0f;
//        for (auto &point: cloud->points) {
//            pcl::PointXYZ out_point;
//            float dx = point.x - m_region.turnplate_cx();
//            float dy = point.y - m_region.turnplate_cy();
//            out_point.x = cos(d_theta) * dx - sin(d_theta) * dy + m_region.turnplate_cx() + m_region.plc_forward_offset().plc_offsetx();
//            out_point.y = sin(d_theta) * dx + cos(d_theta) * dy + m_region.turnplate_cy() + m_region.plc_forward_offset().plc_offsety();
//            out_point.z = point.z;
//            ssscloud_cut->points.emplace_back(out_point);
//        }
//        WriteTxtCloud(ETC_PATH"/etc/data/" + std::to_string(m_region.region_id()) + "/ssscloud_cut.txt", ssscloud_cut);
//        exit(0);
    }else {
        return Error_manager(VELODYNE_REGION_EMPTY_CLOUD, NEGLIGIBLE_ERROR, "no input point");
    }
#endif
#if DebugTime
    std::chrono::steady_clock::time_point t = std::chrono::steady_clock::now();
    std::chrono::duration<double> coast_t = std::chrono::steady_clock::now() - t;
#endif
    // 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");
    }

#if WriteCloud
    WriteTxtCloud(ETC_PATH"MeasureNode/data/" + std::to_string(m_region.region_id()) + "/cloud.txt", cloud);
//    exit(EXIT_SUCCESS);
#endif
    // 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>);

    pcl::CropHull<pcl::PointXYZ> bb_filter;	 		//创建CropHull对象
    bb_filter.setDim(2);		                    //设置维度
    bb_filter.setInputCloud(cloud);	                //设置输入点云
    bb_filter.setHullIndices(m_polygons);	//输入封闭多边形的顶点
    bb_filter.setHullCloud(m_surface_hull);	//输入封闭多边形的形状
    bb_filter.filter(*cloud_cut);

    pcl::PassThrough<pcl::PointXYZ> cut_pass;
    cut_pass.setInputCloud(cloud_cut);
    cut_pass.setFilterFieldName("z");
    cut_pass.setFilterLimits(m_region.minz(), m_region.maxz());
    cut_pass.setFilterLimitsNegative(false);
    cut_pass.filter(*cloud_cut);

    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");
    }
#if WriteCloud
//    cloud_cut->clear();
//    ReadTxtCloud(ETC_PATH"MeasureNode/data/" + std::to_string(m_region.region_id()) + "/cloud_cut_cc.txt", cloud_cut);
    WriteTxtCloud(ETC_PATH"MeasureNode/data/" + std::to_string(m_region.region_id()) + "/cloud_cut.txt", cloud_cut);
#endif

    //下采样
    pcl::VoxelGrid<pcl::PointXYZ> vox;                     //创建滤波对象
    vox.setInputCloud(cloud_cut);                   //设置需要过滤的点云给滤波对象
    vox.setLeafSize(0.02f, 0.02f, 0.02f);     //设置滤波时创建的体素体积为1cm的立方体
    vox.filter(*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() <= 100) {
        // 更新过滤点
        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");
    }
#if WriteCloud
    WriteTxtCloud(ETC_PATH"MeasureNode/data/" + std::to_string(m_region.region_id()) + "/cloud_filtered.txt", cloud_filtered);
#endif

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

#if DebugTime
    coast_t = std::chrono::steady_clock::now() - t;
    LOG(INFO) << "get cloud_filtered coast time: " << coast_t.count() * 1000 << "ms";
#endif
    // 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;
    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.");
    }
    if (fabs(car_pose_theta * 180.0 / M_PI) > 8) {
        return Error_manager(VELODYNE_REGION_ANGLE_PRECHECK_FAILED, NEGLIGIBLE_ERROR,
                             "find general car pose value failed.");
    }
    car_pose_theta = M_PI_2 + car_pose_theta;
//    WriteTxtCloud(ETC_PATH"MeasureNode/data/" + std::to_string(m_region.region_id()) + "/cloud_detect_z.txt", cloud_detect_z);

    // 4.*********模糊车宽获取*********
    double rough_width, rough_length, extract_width, extract_length;
    pcl::PointXYZ t_rough_min_p, t_rough_max_p, t_body_min_p, t_body_max_p;
    pcl::PointCloud<pcl::PointXYZ>::Ptr car_body_cloud(new pcl::PointCloud<pcl::PointXYZ>);
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_for_width_upwheel(new pcl::PointCloud<pcl::PointXYZ>);
    for (auto &point: cloud_detect_z->points) {
            cloud_for_width_upwheel->push_back(pcl::PointXYZ(point.x, point.y, 0));
    }
#if WriteCloud
    WriteTxtCloud(ETC_PATH"MeasureNode/data/" + std::to_string(m_region.region_id()) + "/cloud_for_width_upwheel.txt", cloud_for_width_upwheel);
#endif

    {
        pcl::getMinMax3D(*cloud_for_width_upwheel, t_rough_min_p, t_rough_max_p);
        rough_length = t_rough_max_p.y - t_rough_min_p.y;

        cut_pass.setInputCloud(cloud_for_width_upwheel);
        cut_pass.setFilterFieldName("y");
        cut_pass.setFilterLimits(t_rough_min_p.y + rough_length * 0.25, t_rough_max_p.y - rough_length * 0.25);
        cut_pass.setFilterLimitsNegative(false);
        cut_pass.filter(*car_body_cloud);

        // 车宽重计算，并赋值到当前车宽
        pcl::getMinMax3D(*car_body_cloud, t_body_min_p, t_body_max_p);
        rough_width = t_body_max_p.x - t_body_min_p.x;
    }

    // 根据车身剔除左右两侧数据
    cv::Point2f car_body_rect_vertex[4];
    cv::Point2f lf_p, lr_p, rf_p, rr_p;
    cv::RotatedRect car_body_rect;
    Point3D_tool::getMinRect(car_body_rect, car_body_cloud);
    car_body_rect.points(car_body_rect_vertex);
    if (car_body_rect.angle < -70) {
        lf_p = car_body_rect_vertex[2];
        lr_p = car_body_rect_vertex[1];
        rf_p = car_body_rect_vertex[3];
        rr_p = car_body_rect_vertex[0];
    } else if (car_body_rect.angle > -20) {
        lf_p = car_body_rect_vertex[1];
        lr_p = car_body_rect_vertex[0];
        rf_p = car_body_rect_vertex[2];
        rr_p = car_body_rect_vertex[3];
    }

    rough_width = distance(lf_p, rf_p) + 0.03;  // 0.03车宽补偿，部分车辆会底部窄，顶部宽，导致车宽比实际小
    DLOG(INFO) << rough_width;

    pcl::PointCloud<pcl::PointXYZ>::Ptr car_clean_cloud(new pcl::PointCloud<pcl::PointXYZ>);
    for (auto &point: cloud_filtered->points) {
        double x_min, x_max;
        x_max = (point.y - rf_p.y) * (rr_p.x - rf_p.x) / (rr_p.y - rf_p.y) + rf_p.x + 0.05;
        x_min = (point.y - lf_p.y) * (lr_p.x - lf_p.x) / (lr_p.y - lf_p.y) + lf_p.x - 0.05;
        if (point.x < x_min - 0.02 && point.x > x_min - 0.10) {
            continue;
        }
        if (point.x < x_max + 0.10 && point.x > x_max + 0.02) {
            continue;
        }
        car_clean_cloud->emplace_back(point);
    }
#if WriteCloud
    WriteTxtCloud(ETC_PATH"MeasureNode/data/" + std::to_string(m_region.region_id()) + "/car_clean_cloud.txt", car_clean_cloud);
#endif

#if DebugTime
    coast_t = std::chrono::steady_clock::now() - t;
    LOG(INFO) << "get car_clean_cloud coast time: " << coast_t.count() * 1000 << "ms";
#endif

    // 5.*********算法检测*********
    std::vector<detect_wheel_ceres3d::Detect_result> results;
    detect_wheel_ceres3d::Detect_result result;
    double chassis_z = m_region.minz() + 0.2;
    int index_z_detect = 0;
    double z_detect[] = {0.20, 0.17, 0.15, 0.14, 0.13, 0.12, 0.11, 0.10, 0.09, 0.08, 0.07, 0.06, 0.05};
    // LOG(INFO) << sizeof(z_detect);
    std::vector<std::string> detect_error_v;
    while (index_z_detect < sizeof(z_detect)/sizeof(double)) {
        result.cx = car_pose_x;                 // 初值中x使用第一步优化的值
        result.theta = -(car_pose_theta);       // 传入整车角度，用于准确判断轮宽，此处需要的是点云的角度，通常逆时针90度到x轴
        result.width = rough_width;

        cut_pass.setInputCloud(car_clean_cloud);
        cut_pass.setFilterFieldName("z");
        cut_pass.setFilterLimits(m_region.minz(), m_region.minz() + z_detect[index_z_detect]);
        cut_pass.setFilterLimitsNegative(false);
        cut_pass.filter(*car_clean_cloud);

#if DebugTime
        coast_t = std::chrono::steady_clock::now() - t;
        LOG(INFO) << "before classify_ceres_detect coast time: " << coast_t.count() * 1000 << "ms";
#endif
        std::string detect_error;
        detect_error.append("index: " + std::to_string(index_z_detect) + " "+ result.to_string());
        bool ret = classify_ceres_detect(car_clean_cloud, m_region.minz() + z_detect[index_z_detect], result, detect_error);
        detect_error_v.emplace_back(detect_error);

#if DebugTime
        coast_t = std::chrono::steady_clock::now() - t;
        LOG(INFO) << "after classify_ceres_detect coast time: " << coast_t.count() * 1000 << "ms";
#endif

#if WriteCloud
        pcl::PointCloud<pcl::PointXYZ>::Ptr m_transformed_cloud(new pcl::PointCloud<pcl::PointXYZ>);
        m_transformed_cloud->operator+=(m_detector->m_left_front_transformed_cloud);
        WriteTxtCloud(ETC_PATH"MeasureNode/data/" + std::to_string(m_region.region_id()) + "/m_left_front_transformed_cloud_" + std::to_string(index_z_detect) + ".txt", m_transformed_cloud);
        m_transformed_cloud->clear();
        m_transformed_cloud->operator+=(m_detector->m_left_rear_transformed_cloud);
        WriteTxtCloud(ETC_PATH"MeasureNodec/data/" + std::to_string(m_region.region_id()) + "/m_left_rear_transformed_cloud_" + std::to_string(index_z_detect) + ".txt", m_transformed_cloud);
        m_transformed_cloud->clear();
        m_transformed_cloud->operator+=(m_detector->m_right_front_transformed_cloud);
        WriteTxtCloud(ETC_PATH"MeasureNode/data/" + std::to_string(m_region.region_id()) + "/m_right_front_transformed_cloud_" + std::to_string(index_z_detect) + ".txt", m_transformed_cloud);
        m_transformed_cloud->clear();
        m_transformed_cloud->operator+=(m_detector->m_right_rear_transformed_cloud);
        WriteTxtCloud(ETC_PATH"MeasureNode/data/" + std::to_string(m_region.region_id()) + "/m_right_rear_transformed_cloud_" + std::to_string(index_z_detect) + ".txt", m_transformed_cloud);
        pcl::PointCloud<pcl::PointXYZ>::Ptr width_extract_cloud = pcl::PointCloud<pcl::PointXYZ>::Ptr(
                new pcl::PointCloud<pcl::PointXYZ>);
        width_extract_cloud->operator+=(m_detector->m_left_front_cloud);
        width_extract_cloud->operator+=(m_detector->m_left_rear_cloud);
        width_extract_cloud->operator+=(m_detector->m_right_front_cloud);
        width_extract_cloud->operator+=(m_detector->m_right_rear_cloud);
        WriteTxtCloud(ETC_PATH"MeasureNode/data/" + std::to_string(m_region.region_id()) + "/width_extract_cloud" + std::to_string(index_z_detect) + ".txt", width_extract_cloud);
        DLOG(WARNING) << index_z_detect << " " << z_detect[index_z_detect] << " " << result.to_string();
#endif

        if (ret) {
            if (!results.empty() && CompareCeresResult(results.back(), result)) {
                    last_result = result;
                    results.push_back(result);
#if 0
                    pcl::PointCloud<pcl::PointXYZ>::Ptr wheels_clouds = pcl::PointCloud<pcl::PointXYZ>::Ptr(
                        new pcl::PointCloud<pcl::PointXYZ>);
                    wheels_clouds->operator+=(m_detector->m_left_front_cloud);
                    wheels_clouds->operator+=(m_detector->m_left_rear_cloud);
                    wheels_clouds->operator+=(m_detector->m_right_front_cloud);
                    wheels_clouds->operator+=(m_detector->m_right_rear_cloud);
                    WriteTxtCloud(ETC_PATH"MeasureNode/data/" + std::to_string(m_region.region_id()) + "/wheels_clouds_" + std::to_string(current_index++) + ".txt", wheels_clouds);
#endif
                    break;
            }
            results.push_back(result);
        }
        index_z_detect++;
    }

    if (results.size() == 1) {
        last_result = results.back();
    }

#if DebugTime
    coast_t = std::chrono::steady_clock::now() - t;
    LOG(INFO) << "get detect result coast time: " << coast_t.count() * 1000 << "ms";
#endif
    // 6.*********算法结果校验*********
    pcl::PointCloud<pcl::PointXYZ>::Ptr t_width_extract_cloud = pcl::PointCloud<pcl::PointXYZ>::Ptr(
            new pcl::PointCloud<pcl::PointXYZ>);
    t_width_extract_cloud->operator+=(m_detector->m_left_front_cloud);
    t_width_extract_cloud->operator+=(m_detector->m_left_rear_cloud);
    t_width_extract_cloud->operator+=(m_detector->m_right_front_cloud);
    t_width_extract_cloud->operator+=(m_detector->m_right_rear_cloud);
#if WriteCloud
    WriteTxtCloud(ETC_PATH"MeasureNode/data/" + std::to_string(m_region.region_id()) + "/t_width_extract_cloud.txt", t_width_extract_cloud);
#endif

    WheelsPredictor::PreResult pre_result;
    if (results.empty()) {
        //m_predictor.predict(cloud_detect_z, car_pose_theta * M_PI / 180, pre_result);
        //LOG(INFO) << pre_result.log();
        for (auto &error: detect_error_v) {
            LOG(ERROR) << error;
        }
        return Error_manager(VELODYNE_REGION_CERES_SOLVE_ERROR, NEGLIGIBLE_ERROR, "3d wheel detect failed: ");
    } else {
        //m_predictor.predict(cloud_detect_z, car_pose_theta * M_PI / 180, pre_result);
        //LOG(INFO) << pre_result.log();
        //m_predictor.update(cloud_detect_z, last_result.cx, last_result.cy, (last_result.theta - 90) * M_PI / 180.0, last_result.width, last_result.wheel_base);
    }

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

//    // 车宽精度优化
//    {
//        pcl::PointCloud<pcl::PointXYZ>::Ptr t_width_rough_cloud = pcl::PointCloud<pcl::PointXYZ>::Ptr(
//                new pcl::PointCloud<pcl::PointXYZ>);
//        t_width_rough_cloud->operator+=(*mp_cloud_filtered);
//
        // 获取车轮精确长宽
        {
            //离群点过滤
            sor.setInputCloud(t_width_extract_cloud);
            sor.setMeanK(5);                           //K近邻搜索点个数
            sor.setStddevMulThresh(1.0);        //标准差倍数
            sor.setNegative(false);                         //保留未滤波点（内点）
            sor.filter(*t_width_extract_cloud);          //保存滤波结果到cloud_filter

            Eigen::Affine3d t_affine = Eigen::Affine3d::Identity();
            t_affine.prerotate(Eigen::AngleAxisd((90 - last_result.theta) * M_PI / 180.0, Eigen::Vector3d::UnitZ()));
            pcl::transformPointCloud(*t_width_extract_cloud, *t_width_extract_cloud, t_affine.matrix());

            // 车宽重计算，并赋值到当前车宽
            pcl::PointXYZ t_min_p, t_max_p;
            pcl::getMinMax3D(*t_width_extract_cloud, t_min_p, t_max_p);

            extract_width = t_max_p.x - t_min_p.x;
            extract_length = t_max_p.y - t_min_p.y;
            LOG(INFO) << "extract_width: " << extract_width << " extract_length: " << extract_length;
        }

        last_result.wheel_width = extract_width;
        last_result.width = extract_width;
        last_result.length = rough_length;

//        if (fabs(rough_width - extract_width) > 0.15) {
//            std::string disp_str = std::string("rough data and extract difference is too large: ") +
//                    "(" + std::to_string(rough_width) + ", " + std::to_string(rough_length) + "), " +
//                    "(" + std::to_string(extract_width) + ", " + std::to_string(extract_length) + ")";
//            LOG(WARNING) << disp_str;
//            return Error_manager(VELODYNE_REGION_CERES_SOLVE_ERROR, MINOR_ERROR, disp_str.c_str());
//        }
//    }

#if DebugTime
    coast_t = std::chrono::steady_clock::now() - t;
    LOG(INFO) << "detect all coast time: " << coast_t.count() * 1000 << "ms";
#endif
    return SUCCESS;
}

int Ground_region::outOfRangeDetection(Common_data::Car_wheel_information &measure_result, const velodyne::outOfRangeInfo &out_info, detect_wheel_ceres3d::Detect_result &result) {
    // 车位位于y负方向, 判断后轮超界情况, 4.80是搬运器大Y的运动极限, 2.7是同侧两个小y夹持杆中心距离
    // 判断车辆轴距超界情况
    if (measure_result.car_wheel_base < out_info.car_min_wheelbase() ||    // 轴距过小
        measure_result.car_wheel_base > out_info.car_max_wheelbase()) {    // 轴距过长
        last_range_statu |= Range_status::Range_car_wheelbase;
    } else if (measure_result.car_wheel_base > out_info.car_min_wheelbase() + 0.01 &&    // 轴距过小
               measure_result.car_wheel_base < out_info.car_max_wheelbase() - 0.01) {
        last_range_statu &= (~Range_status::Range_car_wheelbase);
    }
    if (last_range_statu & Range_status::Range_car_wheelbase) {
        return Range_status::Range_car_wheelbase;
    }

    // 判断车辆宽度超界情况
    if (measure_result.car_wheel_width < out_info.car_min_width() ||   // 车宽过窄
        measure_result.car_wheel_width > out_info.car_max_width()) {   // 车宽过宽
        last_range_statu |= Range_status::Range_car_width;       // 车辆超宽
    } else if (measure_result.car_wheel_width > out_info.car_min_width() + 0.01 &&   // 车宽过窄
               measure_result.car_wheel_width < out_info.car_max_width() - 0.01) {
        last_range_statu &= (~Range_status::Range_car_width);
    }
    if (last_range_statu & Range_status::Range_car_width) {
        return Range_status::Range_car_width;
    }

    // 判断车辆旋转角超界情况
    double t_dtheta = measure_result.car_angle;
    if (measure_result.car_angle < out_info.turnplate_angle_limit_min_clockwise()) {
        last_range_statu |= Range_status::Range_angle_clock;
        last_range_statu &= (~Range_status::Range_angle_anti_clock);
    } else if (measure_result.car_angle > out_info.turnplate_angle_limit_max_clockwise()) {
        last_range_statu |= Range_status::Range_angle_anti_clock;
        last_range_statu &= (~Range_status::Range_angle_clock);
    } else if ( measure_result.car_angle > out_info.turnplate_angle_limit_min_clockwise() &&
                measure_result.car_angle < out_info.turnplate_angle_limit_max_clockwise()) {
        last_range_statu &= (~Range_status::Range_angle_clock);
        last_range_statu &= (~Range_status::Range_angle_anti_clock);
    }
    if (last_range_statu & Range_status::Range_angle_clock) {
        return Range_status::Range_angle_clock;
    }
    if (last_range_statu & Range_status::Range_angle_anti_clock) {
        return Range_status::Range_angle_anti_clock;
    }

    // 方向盘调正
    if (fabs(measure_result.car_front_theta) > 10) {
        last_range_statu |= Range_status::Range_steering_wheel_nozero;
    } else if (fabs(measure_result.car_front_theta) < 7) {
        last_range_statu &= (~Range_status::Range_steering_wheel_nozero);
    }
    if (last_range_statu & Range_status::Range_angle_anti_clock) {
        return Range_status::Range_steering_wheel_nozero;
    }

    // 左右超界
    if (measure_result.uniform_car_x < out_info.border_minx()) {
        last_range_statu |= Range_status::Range_left;
        last_range_statu &= (~Range_status::Range_right);
    } else if (measure_result.uniform_car_x > out_info.border_maxx()) {
        last_range_statu |= Range_status::Range_right;
        last_range_statu &= (~Range_status::Range_left);
    } else if (measure_result.uniform_car_x > out_info.border_minx() + 0.01 &&
               measure_result.uniform_car_x < out_info.border_maxx() - 0.01) {
        last_range_statu &= (~Range_status::Range_left);
        last_range_statu &= (~Range_status::Range_right);
    }
    if (last_range_statu & Range_status::Range_left) {
        return Range_status::Range_left;
    }
    if (last_range_statu & Range_status::Range_right) {
        return Range_status::Range_right;
    }

    // 前超界（前轮胎超出转盘）
    double lf_wheel_x = result.cx - measure_result.car_wheel_width * 0.5;
    double rf_wheel_x = result.cx + measure_result.car_wheel_width * 0.5;
    double f_wheel_y = result.cy + measure_result.car_wheel_base * 0.5;
    double angle = (90 - result.theta) * M_PI / 180.0;
    double lf_uniform_wheel_x = ( lf_wheel_x - result.cx) * cos(angle) - (f_wheel_y - result.cy) * sin(angle) + result.cx;
    double lf_uniform_wheel_y = (lf_wheel_x - result.cx) * sin(angle) + (f_wheel_y - result.cy) * cos(angle) + result.cy;
    double rf_uniform_wheel_x = ( rf_wheel_x - result.cx) * cos(angle) - (f_wheel_y - result.cy) * sin(angle) + result.cx;
    double rf_uniform_wheel_y = (rf_wheel_x - result.cx) * sin(angle) + (f_wheel_y - result.cy) * cos(angle) + result.cy;
    double lf_tur_dis = (lf_uniform_wheel_x - m_region.turnplate_cx()) * (lf_uniform_wheel_x - m_region.turnplate_cx()) + (lf_uniform_wheel_y - m_region.turnplate_cy()) * (lf_uniform_wheel_y - m_region.turnplate_cy());
    double rf_tur_dis = (rf_uniform_wheel_x - m_region.turnplate_cx()) * (rf_uniform_wheel_x - m_region.turnplate_cx()) + (rf_uniform_wheel_y - m_region.turnplate_cy()) * (rf_uniform_wheel_y - m_region.turnplate_cy());
    auto max_dis = MAX(lf_tur_dis, rf_tur_dis);
    if (sqrt(max_dis) > (4.2 * 0.5 - 0.1)) {  // 4.2 转盘直径
        last_range_statu |= Range_status::Range_front;
    } else if (sqrt(max_dis) < (4.2 * 0.5 - 0.2)) {
        last_range_statu &= (~Range_status::Range_front);
    }
    if (last_range_statu & Range_status::Range_front) {
        return Range_status::Range_front;
    }

    // 后超界
    double t_center_y = fabs(measure_result.uniform_car_y);
    DLOG(INFO) << t_center_y - 4.80 + 0.5 * (measure_result.car_wheel_base - 2.7) << " " << out_info.plc_border_maxy();
    DLOG(INFO) << t_center_y - 4.80 - 0.5 * (measure_result.car_wheel_base - 2.7) << " " << out_info.plc_border_miny();
    if (t_center_y - 4.80 + 0.5 * (measure_result.car_wheel_base - 2.7) > out_info.plc_border_maxy() ||     // 前轮无法抱到
        t_center_y - 4.80 - 0.5 * (measure_result.car_wheel_base - 2.7) > out_info.plc_border_miny()) {     // 后轮无法抱到
        last_range_statu |= Range_status::Range_back;       // 后超界
    } else if (t_center_y - 4.80 + 0.5 * (measure_result.car_wheel_base - 2.7) < out_info.plc_border_maxy() - 0.010 &&     // 前轮无法抱到
               t_center_y - 4.80 - 0.5 * (measure_result.car_wheel_base - 2.7) < out_info.plc_border_miny() - 0.010) {
        last_range_statu &= (~Range_status::Range_back);
    }
    if (last_range_statu & Range_status::Range_back) {
        // 前进距离
        measure_result.forward_dis = MAX((t_center_y - 4.80 + 0.5 * (measure_result.car_wheel_base - 2.7) - (out_info.plc_border_maxy() - 0.010)),
                                         (t_center_y - 4.80 - 0.5 * (measure_result.car_wheel_base - 2.7) - (out_info.plc_border_miny() - 0.010)));
        return Range_status::Range_back;
    }

    return Range_status::Range_correct;
}

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 == nullptr) {
        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_forward_information;
        if (m_car_wheel_forward_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(std::vector<Common_data::Car_wheel_information> &car_wheel_information,
                                          std::chrono::system_clock::time_point command_time) {
    //获取指令时间之后的信息, 如果没有就会报错, 不会等待
    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);
        car_wheel_information.clear();
        car_wheel_information.push_back(m_car_wheel_forward_information);
        car_wheel_information.push_back(m_car_wheel_reverse_information);
        car_wheel_information.push_back(m_car_wheel_terminal_information);
        if (m_car_wheel_forward_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 ");
    }

    return Error_manager();
}
Error_manager Ground_region::get_last_forward_wheel_information(Common_data::Car_wheel_information *p_car_wheel_information,
                                                        std::chrono::system_clock::time_point command_time) {
    if (p_car_wheel_information == nullptr) {
        return Error_manager(Error_code::POINTER_IS_NULL, Error_level::MINOR_ERROR,
                             "  POINTER IS NULL ");
    }

    //获取指令时间之后的信息, 如果没有就会报错, 不会等待
    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_forward_information;
        if (m_car_wheel_forward_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_reverse_wheel_information(Common_data::Car_wheel_information *p_car_wheel_information,
                                                        std::chrono::system_clock::time_point command_time) {
    if (p_car_wheel_information == nullptr) {
        return Error_manager(Error_code::POINTER_IS_NULL, Error_level::MINOR_ERROR,
                             "  POINTER IS NULL ");
    }

    //获取指令时间之后的信息, 如果没有就会报错, 不会等待
    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_reverse_information;
        if (m_car_wheel_forward_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_terminal_wheel_information(Common_data::Car_wheel_information *p_car_wheel_information,
                                                        std::chrono::system_clock::time_point command_time) {
    if (p_car_wheel_information == nullptr) {
        return Error_manager(Error_code::POINTER_IS_NULL, Error_level::MINOR_ERROR,
                             "  POINTER IS NULL ");
    }

    //获取指令时间之后的信息, 如果没有就会报错, 不会等待
    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_terminal_information;
        if (m_car_wheel_forward_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) {
    // 限定锁区域，解决因条件变量在析构时无法获得内部mutex导致此互斥锁卡住，造成死锁无法结束程序的问题。
    {
        std::lock_guard<std::mutex> lck(m_cloud_collection_mutex);
        mp_cloud_collection = cloud;
        DLOG(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() {
    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;
            Error_manager ec = Error_code::FAILED;
            detect_wheel_ceres3d::Detect_result t_result{};

            ec = detect(mp_cloud_collection, t_result);
            if ((ec == VELODYNE_REGION_CERES_SOLVE_ERROR || ec == VELODYNE_REGION_ANGLE_PRECHECK_FAILED) && m_region.region_id() != 13219) {
                LOG(ERROR) << m_region.region_id() << " detect failture result: " << t_result.to_string();
//                m_cloud_collection_mutex.lock();
                m_detect_failture_cloud_collection_mutex.lock();
                m_detect_failture_cloud_collection_time = std::chrono::steady_clock::now();
                pcl::copyPointCloud(*mp_cloud_collection, *mp_detect_failture_cloud_collection);
                m_detect_failture_cloud_collection_mutex.unlock();
//                m_cloud_collection_mutex.unlock();
                LOG(ERROR) << ec.to_string();
            }

            if (ec == VELODYNE_REGION_EMPTY_CLOUD) {
                m_predictor.reset();
            }

            // 车辆移动检测
            Common_data::Car_wheel_information_stamped t_wheel_info_stamped_for_car_move;
            // 1、测量成功，先进行终端、plc的超界判断，再将数据转换给正反向

            Common_data::Car_wheel_information t_car_wheel_forward_information;         // 车轮的定位结果,
            Common_data::Car_wheel_information t_car_wheel_reverse_information;         // 车轮的定位结果,
            Common_data::Car_wheel_information t_car_wheel_terminal_information;        // 车轮的定位结果,

            // 计算运动状态
            if (ec == SUCCESS) {
//                exit(EXIT_FAILURE);
                LOG(WARNING) << m_region.region_id() << " detect result: " << t_result.to_string();
                // 1、传递测量信息
                t_car_wheel_forward_information.correctness = true;
                t_car_wheel_forward_information.car_center_x = t_result.cx;
                t_car_wheel_forward_information.car_center_y = t_result.cy;
                t_car_wheel_forward_information.car_angle = t_result.theta;
                t_car_wheel_forward_information.car_wheel_base = t_result.wheel_base;
                t_car_wheel_forward_information.car_wheel_width = t_result.wheel_width;
                t_car_wheel_forward_information.car_width = t_result.width;
                t_car_wheel_forward_information.car_length = t_result.length;
                t_car_wheel_forward_information.car_front_theta = t_result.front_theta;
                t_car_wheel_forward_information.range_status = 0;
                t_car_wheel_forward_information.forward_dis = 0;

                m_car_cx_box.save(t_car_wheel_forward_information.uniform_car_x);
                m_car_theta_box.save(t_car_wheel_forward_information.car_angle);
                m_car_width_box.save(t_car_wheel_forward_information.car_width);
                m_car_wheel_base_box.save(t_car_wheel_forward_information.car_wheel_base);
                // LOG(INFO) << m_car_cx_box.size();

                t_wheel_info_stamped_for_car_move.wheel_data = t_car_wheel_forward_information;
                t_wheel_info_stamped_for_car_move.measure_time = m_detect_update_time;
                Error_manager car_status_res = Region_status_checker::get_instance_references().get_region_parking_status(
                        m_region.region_id(), t_wheel_info_stamped_for_car_move, *mp_cloud_filtered);
                if (car_status_res == SUCCESS) {
                    t_car_wheel_forward_information.range_status &= ~((int) Range_status::Range_car_moving);
                } else {
                    t_car_wheel_forward_information.range_status |= Range_status::Range_car_moving;
                }

                // 3、更新ICP数据
                Region_status_checker::get_instance_references().add_measure_data(m_region.region_id(),
                                                                                  t_wheel_info_stamped_for_car_move,
                                                                                  *mp_cloud_filtered);

            } else {
                t_car_wheel_forward_information.correctness = false;

                Error_manager car_status_res = Region_status_checker::get_instance_references().get_region_parking_status(
                        m_region.region_id(), t_wheel_info_stamped_for_car_move, *mp_cloud_filtered);

                if (car_status_res == SUCCESS) {
                    t_car_wheel_forward_information.range_status &= ~((int) Range_status::Range_car_moving);
                } else {
                    t_car_wheel_forward_information.range_status |= Range_status::Range_car_moving;
                }
            }

            // 连续静止3秒, 发出静止状态, 并将坐标、车身角度更新为稳定轴距
            if (!(t_car_wheel_forward_information.range_status & Range_status::Range_car_moving)) {
                std::chrono::duration<double> pass_time = std::chrono::steady_clock::now() - m_last_move_time;
                if (pass_time.count() > 3) {
                    // 轴距
                    getMSE(m_car_wheel_base_box, t_car_wheel_forward_information.car_wheel_base);
                    // 车宽
                    getMSE(m_car_width_box, t_car_wheel_forward_information.car_width);
                    // x坐标
                    getMSE(m_car_cx_box, t_car_wheel_forward_information.uniform_car_x);
                    // 车身角度
                    getMSE(m_car_theta_box, t_car_wheel_forward_information.car_angle);

                    t_car_wheel_forward_information.motion_statu = 1;
                }
            } else {
                m_last_move_time = std::chrono::steady_clock::now();
                t_car_wheel_forward_information.motion_statu = 0;
            }

            t_car_wheel_reverse_information = t_car_wheel_forward_information;
            if (ec == SUCCESS) {
                // 偏移坐标转换
                t_car_wheel_forward_information.theta_uniform(m_region.plc_forward_offset().plc_offset_degree(), m_region.turnplate_cx(), m_region.turnplate_cy());
                t_car_wheel_reverse_information.theta_uniform(m_region.plc_reverse_offset().plc_offset_degree(), m_region.turnplate_cx(), m_region.turnplate_cy());

                t_car_wheel_forward_information.car_angle += m_region.plc_forward_offset().plc_offset_degree();
                t_car_wheel_reverse_information.car_angle += m_region.plc_reverse_offset().plc_offset_degree();
                t_car_wheel_forward_information.car_center_x += m_region.plc_forward_offset().plc_offsetx();
                t_car_wheel_forward_information.car_center_y += m_region.plc_forward_offset().plc_offsety();
                t_car_wheel_forward_information.uniform_car_x += m_region.plc_forward_offset().plc_offsetx();
                t_car_wheel_forward_information.uniform_car_y += m_region.plc_forward_offset().plc_offsety();
                t_car_wheel_forward_information.car_wheel_base += m_region.plc_forward_offset().plc_offset_wheel_base();

                t_car_wheel_reverse_information.car_center_x += m_region.plc_reverse_offset().plc_offsetx();
                t_car_wheel_reverse_information.car_center_y += m_region.plc_reverse_offset().plc_offsety();
                t_car_wheel_reverse_information.uniform_car_x += m_region.plc_reverse_offset().plc_offsetx();
                t_car_wheel_reverse_information.uniform_car_y += m_region.plc_reverse_offset().plc_offsety();
                t_car_wheel_reverse_information.car_wheel_base += m_region.plc_reverse_offset().plc_offset_wheel_base();

                t_car_wheel_terminal_information = t_car_wheel_forward_information;
                // 取距离x轴中心最远的x坐标
                if (fabs(t_car_wheel_forward_information.uniform_car_x - (m_region.terminal_range_info().border_maxx() + m_region.terminal_range_info().border_minx()) * 0.5)
                < fabs(t_car_wheel_reverse_information.uniform_car_x - (m_region.terminal_range_info().border_maxx() + m_region.terminal_range_info().border_minx()) * 0.5)) {
                    t_car_wheel_terminal_information.uniform_car_x = t_car_wheel_reverse_information.uniform_car_x;
                }
                // 取距离y轴原点最远的y坐标
                if (fabs(t_car_wheel_forward_information.uniform_car_y) < fabs(t_car_wheel_reverse_information.uniform_car_y)) {
                    t_car_wheel_terminal_information.uniform_car_y = t_car_wheel_reverse_information.uniform_car_y;
                }
                // 取距离90度偏差最大的角度
                if (fabs(t_car_wheel_forward_information.car_angle) < fabs(t_car_wheel_forward_information.car_angle)) {
                    t_car_wheel_terminal_information.car_angle = t_car_wheel_reverse_information.car_angle;
                }
                // 计算超界状态
                t_car_wheel_forward_information.range_status |= outOfRangeDetection(t_car_wheel_forward_information, m_region.car_range_info(), t_result);
                t_car_wheel_reverse_information.range_status |= outOfRangeDetection(t_car_wheel_reverse_information, m_region.car_range_info(), t_result);
                t_car_wheel_terminal_information.range_status |= outOfRangeDetection(t_car_wheel_terminal_information, m_region.terminal_range_info(), t_result);
            } else {
                if (fabs(90 + m_region.plc_forward_offset().plc_offset_degree()) > fabs(90 + m_region.plc_reverse_offset().plc_offset_degree())) {
                    t_car_wheel_terminal_information = t_car_wheel_forward_information;
                } else {
                    t_car_wheel_terminal_information = t_car_wheel_reverse_information;
                }
            }

            {
                std::lock_guard<std::mutex> lck(m_car_result_mutex);
                m_detect_update_time = std::chrono::system_clock::now();
                m_car_wheel_forward_information.clear();
                m_car_wheel_reverse_information.clear();
                m_car_wheel_terminal_information.clear();

                m_car_wheel_forward_information = t_car_wheel_forward_information;
                m_car_wheel_reverse_information = t_car_wheel_reverse_information;
                m_car_wheel_terminal_information = t_car_wheel_terminal_information;
            }
        }

        m_region_status = E_READY;
    }
    LOG(INFO) << " Ground_region::thread_measure_func() end " << this;
}

void Ground_region::get_region_cloud(pcl::PointCloud<pcl::PointXYZ>::Ptr &out_cloud, const Region_cloud_type &type) {
    out_cloud->clear();
    if (m_detector == nullptr)
        return;
    std::chrono::duration<double> t_cost_t = std::chrono::steady_clock::now() - m_detect_failture_cloud_collection_time;
    pcl::PointXYZ out_point;
    float d_theta = 0;
    switch (type) {
        case Region_cloud_type::total:
            m_cloud_collection_mutex.lock();
            out_cloud->operator+=(*mp_cloud_collection);
            m_cloud_collection_mutex.unlock();
            // LOG(WARNING) << "region cloud size: " << mp_cloud_collection->size();
            break;
        case Region_cloud_type::left_front:
            out_cloud->operator+=(m_detector->m_left_front_cloud);
            break;
        case Region_cloud_type::left_back:
            out_cloud->operator+=(m_detector->m_left_rear_cloud);
            break;
        case Region_cloud_type::right_front:
            out_cloud->operator+=(m_detector->m_right_front_cloud);
            break;
        case Region_cloud_type::right_back:
            out_cloud->operator+=(m_detector->m_right_rear_cloud);
            break;
        case Region_cloud_type::filtered:
            m_filtered_cloud_mutex.lock();
            out_cloud->operator+=(*mp_cloud_filtered);
            m_filtered_cloud_mutex.unlock();
            break;
        case Region_cloud_type::failure:
            m_detect_failture_cloud_collection_mutex.lock();
            if (t_cost_t.count() < 2) {
                out_cloud->operator+=(*mp_detect_failture_cloud_collection);
            }
            m_detect_failture_cloud_collection_mutex.unlock();
            break;
        case Region_cloud_type::trans:
            m_cloud_collection_mutex.lock();
            d_theta = -(90 + m_region.plc_forward_offset().plc_offset_degree()) * M_PI / 180.0f;
            for (auto &point: mp_cloud_collection->points) {
                float dx = point.x - m_region.turnplate_cx();
                float dy = point.y - m_region.turnplate_cy();
                out_point.x = cos(d_theta) * dx - sin(d_theta) * dy + m_region.turnplate_cx() + m_region.plc_forward_offset().plc_offsetx();
                out_point.y = sin(d_theta) * dx + cos(d_theta) * dy + m_region.turnplate_cy() + m_region.plc_forward_offset().plc_offsety();
                out_point.z = point.z;
                out_cloud->points.emplace_back(out_point);
            }
            m_cloud_collection_mutex.unlock();
            break;
    }
}

int Ground_region::get_terminal_id() {
    if (m_region_status == E_UNKNOWN)
        return -1;
    else
        return m_region.region_id();
}

bool Ground_region::check_lidar_inside(const int &lidar_id) {
    for (size_t i = 0; i < m_region.lidar_exts_size(); i++) {
        if (m_region.lidar_exts(i).lidar_id() == lidar_id)
            return true;
    }
    return false;
}

void Ground_region::getMSE(MemoryBox<double> &box, float &value) {;
    std::vector<float> values;
    auto infos = box.get();
    for (auto &info: infos) {
        if (isnan(info.second)) {
            continue;
        }
        values.emplace_back(info.second);
    }
    if (values.size() > 4) {
        double mse = MSE(values, 2);
        if (isnan(mse)) {
            value = mean(values);
        } else {
            value = mse;
        }
    }
}

bool Ground_region::CompareCeresResult(detect_wheel_ceres3d::Detect_result &first, detect_wheel_ceres3d::Detect_result &second) {
    bool radio_cx = fabs(first.cx - second.cx) < 0.01;
    bool radio_cy = fabs(first.cy - second.cy) < 0.03;
    bool radio_th = fabs(first.theta - second.theta) < 0.4;
    bool radio_wb = fabs(first.wheel_base - second.wheel_base) < 0.025;
    bool radio_wwd = true;//fabs(first.wheel_width - second.wheel_width) <0.0;
    bool radio_fth = fabs(first.front_theta - second.front_theta) < 4;
    return (radio_cx && radio_cy && radio_th && radio_wb && radio_wwd && radio_fth);
}
