Vzense-Tof-3D/detect/detect_manager.cpp

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

#include "detect_manager.h"

Error_manager DetectManager::Init() {

#ifdef ENABLE_TENSORRT_DETECT
    detector = new TensorrtWheelDetector(ETC_PATH PROJECT_NAME "/model.engine", ETC_PATH PROJECT_NAME "/class.txt");
#else
    detector = new TensorrtWheelDetector(ETC_PATH PROJECT_NAME "/model.onnx", ETC_PATH PROJECT_NAME "/class.txt");
#endif

    m_thread = new std::thread(&DetectManager::run, this);
    m_condit.notify_all(true);

    return Error_manager();
}

void DetectManager::run() {
    LOG(INFO) << "detect thread running in " << std::this_thread::get_id();

    DetectResult m_detect_result_record;
    std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> vct_wheel_cloud;
    for (int i = 0; i < DeviceAzimuth::DEVICE_AZIMUTH_MAX; i++) {
        vct_wheel_cloud.emplace_back(new pcl::PointCloud<pcl::PointXYZ>);
    }

    auto t_start_time = std::chrono::steady_clock::now();
    std::chrono::duration<double> cost = std::chrono::steady_clock::now() - t_start_time;

    while (m_condit.is_alive()) {
        m_condit.wait();
        // 参数初始化
        t_start_time = std::chrono::steady_clock::now();

        if (m_condit.is_alive()) {
            DetectResult detect_result;
            cv::Mat merge_mat = cv::Mat::zeros(480 * 2, 640 * 2, CV_8UC1);
            DeviceTof3D::DeviceTof3DSaveInfo t_device_mat[DeviceAzimuth::DEVICE_AZIMUTH_MAX];
            for (int device_index = 0; device_index < DeviceAzimuth::DEVICE_AZIMUTH_MAX; device_index++) {
                t_device_mat[device_index] = DeviceTof3D::iter()->getDeviceSaveInfo((DeviceAzimuth)device_index);
                t_device_mat[device_index].irMat.copyTo(merge_mat(cv::Rect((device_index & 0x1) * 640,
                                                                           ((device_index & 0x2) >> 1) * 480, 640, 480)));
            }

            cost = std::chrono::steady_clock::now() - t_start_time;
            DLOG(INFO) << "cost time is " << cost.count() * 1000 << " ms";

            // 模型识别
            std::vector<Object> objs;
            cv::cvtColor(merge_mat, merge_mat, cv::COLOR_GRAY2RGB);
            detector->detect(merge_mat, objs, merge_mat);
            cv::imshow("merge_mat", merge_mat);
            cv::waitKey(1);
            if (objs.empty()) {
                detect_statu = MeasureStatu::Measure_Empty_Clouds;
                m_detect_result_record.reset();
                continue;
            }

            cost = std::chrono::steady_clock::now() - t_start_time;
            DLOG(INFO) << "cost time is " << cost.count() * 1000 << " ms";

            // 标记识别点云
            for (auto &obj: objs) {
                auto seg_points = detector->getPointsFromObj(obj);
                int device_index = (int(seg_points[0].x / 640) * 0x01) | (int(seg_points[0].y / 480) << 1);
                detect_result.wheel[device_index].confidence = obj.prob;

                if (obj.prob > 0.9) {
                    detect_result.wheel[device_index].detectOk = true;
                    vct_wheel_cloud[device_index]->clear();
                    float d = MAX(obj.rect.height, obj.rect.width) / 2;
                    pcl::PointXYZ point;
                    int x_alpha = (device_index & 0x1);
                    int y_alpha = ((device_index & 0x2) >> 1);
                    for (auto &pt: seg_points) {    // use 7.5~9ms
                        pt.x -= x_alpha * t_device_mat[device_index].irMat.cols;
                        pt.y -= y_alpha * t_device_mat[device_index].irMat.rows;
                        t_device_mat[device_index].pointMat.at<cv::Vec4f>(pt)[3] = obj.prob;
                        point.x = t_device_mat[device_index].pointMat.at<cv::Vec4f>(pt)[0];
                        point.y = t_device_mat[device_index].pointMat.at<cv::Vec4f>(pt)[1];
                        point.z = t_device_mat[device_index].pointMat.at<cv::Vec4f>(pt)[2];
                        vct_wheel_cloud[device_index]->push_back(point);
                    }

                    TransitData::get_instance_pointer()->setMask(seg_points, device_index);
                }
            }

            cost = std::chrono::steady_clock::now() - t_start_time;
            DLOG(INFO) << "cost time is " << cost.count() * 1000 << " ms";

            // 点云处理
            if (!objs.empty()) {
                WheelCloudOptimize(vct_wheel_cloud, detect_result);
                cost = std::chrono::steady_clock::now() - t_start_time;
                DLOG(INFO) << "cost time is " << cost.count() * 1000 << " ms";

                bool ceres_detect_ret = false;
                detect_wheel_ceres wheel_detect;
                // 采用上次测量结果的数据
                if (!m_detect_result_time_lock_data.timeout(0.5)) {
                    detect_result = m_detect_result_time_lock_data();
                    detect_result.car.wheel_width = MIN(detect_result.car.wheel_width + 0.4, 2.5);
                } else {
                    detect_result.car.wheel_width = 3;
                    detect_result.car.wheel_base = 2.7;
                }

                // 根据车轮数采用不同的优化方式
                if (m_detect_result_record.car.wheel_width < 1.0 || m_detect_result_record.car.wheel_base < 2.0 ||
                    objs.size() > 2) {
                    ceres_detect_ret = wheel_detect.detect_2Step(vct_wheel_cloud[DeviceAzimuth::DEVICE_AZIMUTH_LF], vct_wheel_cloud[DeviceAzimuth::DEVICE_AZIMUTH_RF],
                                    vct_wheel_cloud[DeviceAzimuth::DEVICE_AZIMUTH_LR], vct_wheel_cloud[DeviceAzimuth::DEVICE_AZIMUTH_RR],
                                    detect_result.car.wheels_center_x, detect_result.car.wheels_center_y,
                                    detect_result.car.theta, detect_result.car.wheel_base,
                                    detect_result.car.wheel_width,detect_result.car.front_wheels_theta, detect_result.car.loss);
                    if ((m_detect_result_record.car.wheel_width < 1.0 && m_detect_result_record.car.wheel_base < 1.0 && objs.size() == 2) || objs.size() < 2) {
                        detect_result.car.wheel_width = m_detect_result_record.car.wheel_width;
                        detect_result.car.wheel_base = m_detect_result_record.car.wheel_base;
                    } else if (objs.size() == 2) {
                        std::vector<int> have_cloud;
                        for (int device_index = 0; device_index < DeviceAzimuth::DEVICE_AZIMUTH_MAX; device_index++) {
                            if (!vct_wheel_cloud[device_index]->empty()) {
                                have_cloud.push_back(device_index);
                            }
                        }

                        if (have_cloud[0] % 2 == have_cloud[1] %2 ) {
                            detect_result.car.wheel_width = m_detect_result_record.car.wheel_width;
                        } else {
                            detect_result.car.wheel_base = m_detect_result_record.car.wheel_base;
                        }
                    }
                } else {
                    ceres_detect_ret = wheel_detect.detect_dynP(vct_wheel_cloud[DeviceAzimuth::DEVICE_AZIMUTH_LF], vct_wheel_cloud[DeviceAzimuth::DEVICE_AZIMUTH_RF],
                                         vct_wheel_cloud[DeviceAzimuth::DEVICE_AZIMUTH_LR], vct_wheel_cloud[DeviceAzimuth::DEVICE_AZIMUTH_RR],
                                         detect_result.car.wheels_center_x, detect_result.car.wheels_center_y,
                                         detect_result.car.theta, m_detect_result_record.car.wheel_base,
                                         m_detect_result_record.car.wheel_width,detect_result.car.front_wheels_theta, detect_result.car.loss);
                    detect_result.car.wheel_base = m_detect_result_record.car.wheel_base;
                    detect_result.car.wheel_width = m_detect_result_record.car.wheel_width;
                }

                // 如果车辆运动，不断刷新运动时间，如果由运动转为静止则刷新运动状态到静止
                float move_distance = detect_result.car.wheels_center_y - m_detect_result_record.car.wheels_center_y;
                if (fabs(move_distance) > 0.1) {
                    move_statu = true;
                } else if (move_statu()) {
                    move_statu = false;
                }

                if (ceres_detect_ret) {
                    // 记录下测量结果
                    m_detect_result_record = detect_result;
                    detect_statu = Measure_OK;
                    m_detect_result_time_lock_data = m_detect_result_record;
                } else {
                    detect_statu = Measure_Failture;
                }

            }
            cost = std::chrono::steady_clock::now() - t_start_time;
            DLOG(INFO) << "cost time is " << cost.count() * 1000 << " ms";

            // 算法结果处理
//            static float min_wheel_width = 1e8, max_wheel_width = -1e8, min_wheel_base = 1e8, max_wheel_base = -1e8;
//            if (m_detect_result_record.car.wheel_width > 1.0) {
//                min_wheel_width = std::min(min_wheel_width, m_detect_result_record.car.wheel_width);
//                max_wheel_width = std::max(max_wheel_width, m_detect_result_record.car.wheel_width);
//            }
//            if (m_detect_result_record.car.wheel_base > 1.0) {
//                min_wheel_base = std::min(min_wheel_base, m_detect_result_record.car.wheel_base);
//                max_wheel_base = std::max(max_wheel_base, m_detect_result_record.car.wheel_base);
//            }

//            LOG(INFO) << "\n" << m_detect_result_record.info();
            usleep(1000 * 100);
        }
    }
    LOG(INFO) << "detect thread " << std::this_thread::get_id() << " closed.";
}


Error_manager DetectManager::WheelCloudOptimize(std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> &vct_wheel_cloud, DetectResult &result) {
    //下采样
    pcl::VoxelGrid<pcl::PointXYZ> vox;                     //创建滤波对象
    vox.setLeafSize(0.03f, 0.03f, 0.03f);     //设置滤波时创建的体素体积为1cm的立方体
    pcl::PointCloud<pcl::PointXYZ>::Ptr filtered_cloud(new pcl::PointCloud<pcl::PointXYZ>);
    for (int device_index = 0; device_index < DeviceAzimuth::DEVICE_AZIMUTH_MAX; device_index++) {
        if (vct_wheel_cloud[device_index]->empty()) { continue;}

        vox.setInputCloud(vct_wheel_cloud[device_index]);                   //设置需要过滤的点云给滤波对象
        vox.filter(*vct_wheel_cloud[device_index]);                         //执行滤波处理，存储输出
    }

    return {};
}

MeasureStatu DetectManager::getMeasureResult(DetectManager::DetectResult &result) {
    if (detect_statu() != MeasureStatu::Measure_OK) {
        return detect_statu();
    }
    if (m_detect_result_time_lock_data.timeout()) {
        return MeasureStatu::Measure_Failture;
    }
    result = m_detect_result_time_lock_data();
    return MeasureStatu::Measure_OK;
}

Error_manager DetectManager::carMoveStatus() {
    if (move_statu.timeout(3) && !move_statu()) {
        return {FAILED, NORMAL};
    }
    return {};
}