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++) {
        TofTransformationManager::iter()->set((DeviceAzimuth)device_index, tof_devices[device_index].trans());
        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;
    rotation_matrix4[azimuth] = rotaition;
    return {};
}

#include "tool/time.hpp"

bool boxIsValid(const JetStream::SegBox& box, int min ,bool reverse=false){
    if(box.confidence()<0.8){
        return false;
    }
    int left_side = box.x();
    if(reverse){
        left_side=640-(box.x()+box.width());
    }

    return left_side > min;
}

int IsInImageValidRegion(const JetStream::LabelYolo &labels) {
    if(labels.boxes_size()!=4){
        return -1; //没有识别框
    }

    for (int device_index = 0; device_index < 4; device_index++) {
        // LOG(INFO) << device_index << ": "<< labels.boxes(device_index).confidence() << " " << labels.boxes(device_index).x();
    }

    JetStream::SegBox box2=labels.boxes(1);
    JetStream::SegBox box4=labels.boxes(3);
    bool frontExsit = labels.boxes(0).confidence() > 0.8 || labels.boxes(1).confidence() > 0.8;
    bool backExist = labels.boxes(2).confidence() > 0.8 || labels.boxes(3).confidence() > 0.8;
    if(!frontExsit){
        if(backExist){  //后有前没有
            return 1;  //往前
        }else{  //前后都没有
            return -1;
        }
    }else{
        if(backExist){
           bool front_OK = boxIsValid(labels.boxes(1), 20) || boxIsValid(labels.boxes(0), 20,true);
            bool back_OK = boxIsValid(labels.boxes(2), 20,true) || boxIsValid(labels.boxes(3), 20);
            if(front_OK && back_OK){
                return 0;
            }else{
                return 1;
            }
        }else{  // 前有后没有
            bool front_OK=boxIsValid(labels.boxes(1),320) || boxIsValid(labels.boxes(0),320,true);
            if(!front_OK){
                return 1;
            }else{
                return -1;
            }
        }
    }
}

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();
        // 参数初始化
        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 seg_num = 0;
            for (int device_index = 0; device_index < DeviceAzimuth::DEVICE_AZIMUTH_MAX; device_index++) {
                // LOG(INFO) << device_index << ": " << seg_results.boxes(device_index).confidence();
                if (seg_results.boxes(device_index).confidence() > 0.7) {
                    seg_num++;
                }
            }
            if (seg_num == 0) {
                detect_statu = MeasureStatu::Measure_Empty_Clouds;
                m_detect_result_record.reset();
                continue;
            }
            // 取高置信度识别结果
            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)) {
                    have_cloud.push_back(device_index);
                    std::vector<cv::Point> seg_points;
                    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++) {
                            seg_points.emplace_back(col, row);
                            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);
                        }
                    }
                    TransitData::get_instance_pointer()->setMask(seg_points, device_index);
                    CoordinateTransformation3D device_trans;
                    // 更新旋转坐标
                    if (TofTransformationManager::iter()->get((DeviceAzimuth)device_index, device_trans)) {
                        updateTrans((DeviceAzimuth)device_index, device_trans);
                    }
                    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]);
                }
            }
            int flag = IsInImageValidRegion(seg_results);
            if (flag == 1) {
                LOG(INFO) << "向前";
                detect_statu = MeasureStatu::Measure_Border;
                continue;
            } else if (flag == -1){
                LOG(WARNING) << "缺少轮子";
                continue;
            }

            // 点云处理
            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 {};
}