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

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

#include "detect_manager.h"

Error_manager DetectManager::Init(const google::protobuf::RepeatedPtrField<tof3dVzenseEtc> &tof_devices) {
    for (int device_index = 0; device_index < DeviceAzimuth::DEVICE_AZIMUTH_MAX; device_index++) {
        updateTrans((DeviceAzimuth)device_index, tof_devices[device_index].trans());
    }

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

    return {};
}

Error_manager DetectManager::Release() {
    stop();
    return {};
}

Error_manager DetectManager::updateTrans(const DeviceAzimuth &azimuth, const CoordinateTransformation3D &trans) {
    Eigen::Matrix4f rotaition = Eigen::Matrix4f::Identity();
    Eigen::Matrix3f rotation_matrix3 = Eigen::Matrix3f::Identity();
    rotation_matrix3 =
            Eigen::AngleAxisf(trans.yaw() * M_PI / 180.f, Eigen::Vector3f::UnitZ()) *
            Eigen::AngleAxisf(trans.pitch() * M_PI / 180.f, Eigen::Vector3f::UnitY()) *
            Eigen::AngleAxisf(trans.roll() * M_PI / 180.f, Eigen::Vector3f::UnitX());
    Eigen::Vector3f move;
    move << trans.x(), trans.y(), trans.z();
    rotaition.block<3, 3>(0, 0) = rotation_matrix3;
    rotaition.col(3) << move;
    LOG(INFO) << "device " << azimuth << " update rotation info: \n" << rotaition;
    rotation_matrix4[azimuth] = rotaition;
    return {};
}

#include "tool/time.hpp"

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;
    RecordImages record_images;

    while (m_condit.is_alive()) {
        m_condit.wait();
        // 参数初始化
        cost = std::chrono::steady_clock::now() - t_start_time;
        std::this_thread::sleep_for(std::chrono::milliseconds(50 - std::min<int>(49, cost.count() * 1000)));
        t_start_time = std::chrono::steady_clock::now();

        if (m_condit.is_alive()) {
            std::vector<int> have_cloud;
            DetectResult detect_result;
            JetStream::LabelYolo seg_results;
            for (int i = 0; i < DeviceAzimuth::DEVICE_AZIMUTH_MAX; i++) {
                vct_wheel_cloud[i]->clear();
            }
            if (!LabelYoloDataBuffer::iter()->lock_exe(&LabelYoloDataBuffer::get, seg_results, 10)) {
                continue;
            }
            int lable = TimeTool::timeDropoutLL(TimeTool::TemporalPrecision::TemporalPrecisionMs, TimeTool::TemporalPrecision::TemporalPrecisionHour);
            LOG(INFO) << "after " << (lable - seg_results.label()) << "ms";
            // 判断识别结果数量以及是否存图
            int seg_num = 0;
            for (int device_index = 0; device_index < DeviceAzimuth::DEVICE_AZIMUTH_MAX; device_index++) {
                if (seg_results.boxes(device_index).confidence() > 0.7) {
                    seg_num++;
                }
            }
            if (seg_num == 1 && record_images.name_1.empty()) {
                if (MatDataBuffer::iter()->lock_exe(&MatDataBuffer::getIr, seg_results.label(), record_images.img_1, 10)) {
                    record_images.name_1 = TimeTool::timeDropoutStr();
                } else {
                    record_images.name_1.clear();
                }

            }
            if (seg_num == 2 && record_images.name_2.empty()) {
                if (MatDataBuffer::iter()->lock_exe(&MatDataBuffer::getIr, seg_results.label(), record_images.img_2, 10)) {
                    record_images.name_2 = TimeTool::timeDropoutStr();
                } else {
                    record_images.name_2.clear();
                }

            }
            if (seg_num == 3 && record_images.name_3.empty()) {
                if (MatDataBuffer::iter()->lock_exe(&MatDataBuffer::getIr, seg_results.label(), record_images.img_3, 10)) {
                    record_images.name_3 = TimeTool::timeDropoutStr();
                } else {
                    record_images.name_3.clear();
                }

            }
            if (seg_num == 4 && record_images.name_4.empty()) {
                if (MatDataBuffer::iter()->lock_exe(&MatDataBuffer::getIr, seg_results.label(), record_images.img_4, 10)) {
                    record_images.name_4 = TimeTool::timeDropoutStr();
                } else {
                    record_images.name_4.clear();
                }

            }
            // 取高置信度识别结果
            for (int device_index = 0; device_index < DeviceAzimuth::DEVICE_AZIMUTH_MAX; device_index++) {
                if (seg_results.boxes(device_index).confidence() < 0.9) {
                    DLOG(INFO) << device_index << " empty";
                    continue;
                }
                cv::Mat pointMat;
                //LOG(INFO) << "b get data" << " empty";
                if (MatDataBuffer::iter()->lock_exe(&MatDataBuffer::get, device_index, seg_results.label(), pointMat, 10)) {
                    cv::imshow(std::to_string(device_index), pointMat);
                    cv::waitKey(1);
                    have_cloud.push_back(device_index);
                    for (int row = 0; row < 480; row++) {
                        for (int col = seg_results.boxes(device_index).lines(row).begin(); col < seg_results.boxes(device_index).lines(row).end(); col++) {
                            vct_wheel_cloud[device_index]->emplace_back(pointMat.at<cv::Vec3f>(row, col)[0] * 0.001,
                                                                        pointMat.at<cv::Vec3f>(row, col)[1] * 0.001,
                                                                        pointMat.at<cv::Vec3f>(row, col)[2] * 0.001);
                        }
                    }
                    pcl::transformPointCloud(*vct_wheel_cloud[device_index], *vct_wheel_cloud[device_index], rotation_matrix4[device_index]());
                    // Point3D_tool::WriteTxtCloud(ETC_PATH PROJECT_NAME "/data/cloud_" + std::to_string(device_index) + ".txt", vct_wheel_cloud[device_index]);
                }
            }

            // 点云处理
            if (!have_cloud.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;
                auto wheel_detect = detect_wheel_ceres::iter();
                // 采用上次测量结果的数据
                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 ||
                        have_cloud.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 && have_cloud.size() == 2) || have_cloud.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 (have_cloud.size() == 2) {
                        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";
        } else {
            cv::destroyAllWindows();
        }
    }
    LOG(INFO) << "detect thread " << std::this_thread::get_id() << " closed.";
}

void DetectManager::stop() {
    //杀死线程,
    m_condit.kill_all();

    //在线程join结束之后, 就可以可以回收线程内存资源
    m_thread->join();
    delete m_thread;
    m_thread = nullptr;
}

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

Error_manager DetectManager::Stop() {
    m_condit.kill_all();
    m_thread->join();
    delete m_thread;
    m_thread = nullptr;
    return {};
}

Error_manager DetectManager::Continue() {
    m_condit.reset();
    m_thread = new std::thread(&DetectManager::run, this);
    m_condit.notify_all(true);
    return {};
}