//
// Created by zx on 2023/10/24.
//

#include "alg.h"

std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr>
GroundRegion::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 GroundRegion::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;
}

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

GroundRegion::~GroundRegion() {
    // 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 GroundRegion::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_pose_record.cy = -1e8;
    m_cut_line = m_region.minz() + 0.07;
    // 创建多边形区域指针，用于裁剪点云
    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); 			//计算凸包结果

    // 启动测量线程
    m_measure_thread = new std::thread(&GroundRegion::thread_measure_func, this);
    m_measure_condition.reset();
    m_region_status = E_READY;
    m_measure_condition.notify_all(true);

    return SUCCESS;
}

bool GroundRegion::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"MeasureNodeAlgTest/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;
}

Error_manager GroundRegion::ClippingAndFilterCloud(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered) {
    // 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::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");
    }

//    WriteTxtCloud(ETC_PATH"MeasureNodeAlgTest/data/" + std::to_string(m_region.region_id()) + "/cloud_cut.txt", cloud_cut);


    //下采样
    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");
    }
//    WriteTxtCloud(ETC_PATH"MeasureNodeAlgTest/data/" + std::to_string(m_region.region_id()) + "/cloud_filtered.txt", cloud_filtered);

    return {};
}

Error_manager GroundRegion::CalculateInitialPose(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered, detect_wheel_ceres3d::Detect_result &pose, pcl::PointCloud<pcl::PointXYZ>::Ptr car_clean_cloud) {
    if (cloud_filtered->empty()) {
        return {VELODYNE_REGION_EMPTY_CLOUD, NEGLIGIBLE_ERROR, "StatisticalOutlierRemoval no point"};
    }

    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_detect_z(new pcl::PointCloud<pcl::PointXYZ>);
    if (!Car_pose_detector::get_instance_references().detect_pose_mat(cloud_filtered, cloud_detect_z, pose.cx,
                                                                      pose.cy, pose.theta, false)) {
        return Error_manager(VELODYNE_REGION_CERES_SOLVE_ERROR, NEGLIGIBLE_ERROR,
                             "find general car pose value failed.");
    }
    if (fabs(pose.theta * 180.0 / M_PI) > 8) {
        return Error_manager(VELODYNE_REGION_ANGLE_PRECHECK_FAILED, NEGLIGIBLE_ERROR,
                             "find general car pose value failed.");
    }
    pose.theta += M_PI_2;
    pose.theta = -pose.theta;
//    WriteTxtCloud(ETC_PATH"MeasureNodeAlgTest/data/" + std::to_string(m_region.region_id()) + "/cloud_detect_z.txt", cloud_detect_z);

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

//    WriteTxtCloud(ETC_PATH"MeasureNodeAlgTest/data/" + std::to_string(m_region.region_id()) + "/cloud_for_width_upwheel.txt", cloud_for_width_upwheel);

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

        pcl::PassThrough<pcl::PointXYZ> cut_pass;
        cut_pass.setInputCloud(cloud_for_width_upwheel);
        cut_pass.setFilterFieldName("y");
        cut_pass.setFilterLimits(t_rough_min_p.y + pose.length * 0.25, t_rough_max_p.y - pose.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);
        pose.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];
    }

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

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

//    WriteTxtCloud(ETC_PATH"MeasureNodeAlgTest/data/" + std::to_string(m_region.region_id()) + "/car_clean_cloud.txt", car_clean_cloud);

    return {};
}

Error_manager
GroundRegion::detect(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, detect_wheel_ceres3d::Detect_result &last_result) {
    Error_manager ret;
    std::chrono::steady_clock::time_point t = std::chrono::steady_clock::now();
    std::chrono::duration<double> coast_t = std::chrono::steady_clock::now() - t;

    // 1.********* 点云合法性检验 *********
    if (cloud->size() == 0) {
        // 更新过滤点
        m_filtered_cloud_mutex.lock();
        mp_cloud_filtered->clear();
        m_filtered_cloud_mutex.unlock();
        m_cut_line = m_region.minz() + 0.07;
        return Error_manager(VELODYNE_REGION_EMPTY_CLOUD, NEGLIGIBLE_ERROR, "no input point");
    }

#if OPTION_ENABLE_DATA_FROM_LOCAL
//    WriteTxtCloud(ETC_PATH"MeasureNodeAlgTest/data/" + std::to_string(m_region.region_id()) + "/cloud.txt", cloud);
#endif

    // 2.********* 点云裁剪过滤 *********
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered(new pcl::PointCloud<pcl::PointXYZ>);
    ret = ClippingAndFilterCloud(cloud, cloud_filtered);
    if (ret != SUCCESS) {
        m_cut_line = m_region.minz() + 0.07;
        return ret;
    }

//    coast_t = std::chrono::steady_clock::now() - t;
//    LOG(INFO) << "get cloud_filtered coast time: " << coast_t.count() * 1000 << "ms";

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

    // 2.********* 点云初始位姿获取 *********
    pcl::PointCloud<pcl::PointXYZ>::Ptr car_clean_cloud(new pcl::PointCloud<pcl::PointXYZ>);
    detect_wheel_ceres3d::Detect_result roughly_pose;
    ret = CalculateInitialPose(cloud_filtered, roughly_pose, car_clean_cloud);
    if (ret != SUCCESS) {
//        LOG(WARNING) << ret.to_string();
        return ret;
    }
//    LOG(INFO) << "m_pose_record.cy " << m_pose_record.cy << ", roughly_pose.cy " << roughly_pose.cy;
//    LOG(INFO) << fabs(m_pose_record.cy - roughly_pose.cy) << " " << m_cut_line;
    if (m_pose_record.success) {
        // cut line 调整
        m_cut_line < m_region.minz() + 0.04 ? m_cut_line += 0.05 : false;
        m_pose_record.loss.total_avg_loss > 0.03 ? m_cut_line -= 0.01 : false;
        m_pose_record.loss.total_avg_loss > 0.02 ? m_cut_line -= 0.01 : false;
        m_pose_record.loss.total_avg_loss < 0.005 ? m_cut_line += 0.01 : false;
        m_pose_record.loss.total_avg_loss < 0.003 ? m_cut_line += 0.01 : false;
    } else {
        m_cut_line += 0.07;
    }
    m_cut_line = MIN(m_cut_line, m_region.minz() + 0.2);
    m_cut_line = MAX(m_cut_line, m_region.minz());

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

    // 3.********* 算法检测 *********
    std::vector<detect_wheel_ceres3d::Detect_result> results;
    int index_z_detect = 0;
    std::vector<std::string> detect_error_v;
    while (m_cut_line > m_region.minz() + 0.03) {
        detect_wheel_ceres3d::Detect_result result = roughly_pose;

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

//        coast_t = std::chrono::steady_clock::now() - t;
//        LOG(INFO) << "before classify_ceres_detect coast time: " << coast_t.count() * 1000 << "ms";
        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_cut_line, result, detect_error);
        LOG(INFO) << m_region.region_id() << " " << index_z_detect << " " << m_cut_line << " " << result.to_string()
                  << " loss:" << result.loss.total_avg_loss;
        detect_error_v.emplace_back(detect_error);

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

        if (ret) {
            if (!results.empty()/* && CompareCeresResult(results.back(), result)*/) {
                last_result = results.back().loss.total_avg_loss < result.loss.total_avg_loss ? results.back() : result;
                results.push_back(result);
                m_cut_line -= 0.01; // 为了抵消只成功一次导致cut line的削减，这里也削减一次
                break;
            }
            results.push_back(result);
        }
        index_z_detect++;
        m_cut_line -= 0.01;
    }
    m_cut_line += 0.01; // 抵消cut line 未检测过的削减

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

//    coast_t = std::chrono::steady_clock::now() - t;
//    LOG(INFO) << "get detect result coast time: " << coast_t.count() * 1000 << "ms";

    // 4.********* 算法结果校验 *********
    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);

    WheelsPredictor::PreResult pre_result;
    if (results.empty()) {
        for (auto &error: detect_error_v) {
            // LOG(ERROR) << error;
        }
        return Error_manager(VELODYNE_REGION_CERES_SOLVE_ERROR, NEGLIGIBLE_ERROR, "3d wheel detect failed: ");
    }

    // 获取车轮精确长宽
    {
        //离群点过滤
        pcl::StatisticalOutlierRemoval<pcl::PointXYZ> sor;
        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);

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

    last_result.wheel_width = last_result.width;

//    coast_t = std::chrono::steady_clock::now() - t;
//    LOG(INFO) << "detect all coast time: " << coast_t.count() * 1000 << "ms";

    return SUCCESS;
}

int GroundRegion::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夹持杆中心距离
    int last_range_statu = Range_status::Range_correct;

    // 判断车辆轴距超界情况
    if (measure_result.car_wheel_base < out_info.car_min_wheelbase() - 0.010 ||    // 轴距过小
        measure_result.car_wheel_base > out_info.car_max_wheelbase() + 0.010) {    // 轴距过长
        return last_range_statu |= Range_status::Range_car_wheelbase;       // 车辆前后轴距不合法
    }

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

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

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

    // 左右超界
    if (measure_result.uniform_car_x < out_info.border_minx()) {
        return last_range_statu |= Range_status::Range_left;
    } else if (measure_result.uniform_car_x > out_info.border_maxx()) {
        return last_range_statu |= 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 转盘直径LOG(INFO) << m_region.region_id() << " 前超界！！！！！！！！ " << sqrt(max_dis);
        return last_range_statu |= 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() - 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;       // 后超界
    }
    // 前进距离
    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 last_range_statu;
}

Error_manager GroundRegion::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 GroundRegion::thread_measure_func() {
    LOG(INFO) << " GroundRegion::thread_measure_func() start " << this;
#if OPTION_ENABLE_DATA_FROM_LOCAL
    pcl::PointCloud<pcl::PointXYZ>::Ptr read_cloud(new pcl::PointCloud<pcl::PointXYZ>);
    ReadTxtCloud(ETC_PATH"MeasureNodeAlgTest/data/" + std::to_string(m_region.region_id()) + "/total.txt", read_cloud);
#endif
    while (m_measure_condition.is_alive()) {
        m_measure_condition.wait();
//        if (m_region.region_id() != 3118) {
//            std::this_thread::sleep_for(std::chrono::milliseconds(1000));
//            continue;
//        }
        if (m_measure_condition.is_alive()) {
            m_region_status = E_BUSY;
            Error_manager ec = Error_code::FAILED;
            detect_wheel_ceres3d::Detect_result t_result{};

            mp_cloud_collection->clear();
#if OPTION_ENABLE_DATA_FROM_LOCAL
            pcl::copyPointCloud(*read_cloud, *mp_cloud_collection);
#else
            for (auto &lidar_ext: m_region.lidar_exts()) {
                CloudDataMqttAsync::instance()->getCloudData("rslidar/" + std::to_string(lidar_ext.lidar_id()), mp_cloud_collection);
            }
#endif
//            LOG(INFO) << "thread_measure_func is running: " << mp_cloud_collection->size();
//            continue;
            ec = detect(mp_cloud_collection, t_result);
            m_pose_record = t_result;
            if (ec == SUCCESS) {
                LOG(WARNING) << m_region.region_id() << " detect result: " << t_result.to_string();
            } else {
                if (ec != VELODYNE_REGION_EMPTY_CLOUD) {
                    LOG(ERROR) << m_region.region_id() << " detect result: " << t_result.to_string();
                }
            }
        }

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

void GroundRegion::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 GroundRegion::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);
}