Vzense-Tof-3D/vzense/back/device_tof3d_b.cpp

#include "device_tof3d_b.h"

Error_manager DeviceTof3D::Init(google::protobuf::RepeatedPtrField<tof3dVzenseEtc> &tof_devices) {
    LOG(INFO) << "---------- Init DeviceTof3D ----------";
    // 使能tof3d相机SDK
    m_vz_status = VZ_Initialize();
    if (m_vz_status != VzReturnStatus::VzRetOK && m_vz_status != VzReturnStatus::VzRetReInitialized) {
        return {TOF3D_VZENSE_DEVICE_INIT_FAILED, MAJOR_ERROR, "VzInitialize failed status: %d", m_vz_status};
    }

    // 注册相机列表

    // 检索相机

    // 配置相机列表配置

    // 使能相机列表设备

    return {TOF3D_VZENSE_DEVICE_INIT_SUCCESS, NORMAL, "VzInitialize success."};
}

Error_manager DeviceTof3D::Init(const VzEtcMap &tp_tof3d, bool search) {
    m_vz_status = VZ_Initialize();
    if (m_vz_status != VzReturnStatus::VzRetOK && m_vz_status != VzReturnStatus::VzRetReInitialized) {
        return {TOF3D_VZENSE_DEVICE_INIT_FAILED, MAJOR_ERROR, "VzInitialize failed status: %d", m_vz_status};
    }

    auto t_ip_list = INetInfo::getIpv4List();
    for (auto &ip: t_ip_list) {
        if (ip != "127.0.0.1") {
            // 启动grpc服务
            LOG(INFO) << "grpc communication with " << ip;
            m_grpc_server.Listenning(ip, 9876);
            break;
        }
    }
#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
    loadEtc(tp_tof3d);

    if (search) {
        SearchDevice(5);
    }

    ConnectAllEtcDevices();

    VZ_SetHotPlugStatusCallback(DeviceTof3D::HotPlugStateCallback, &mp_device_info);

    //启动 线程。
    for (auto &info: mp_device_info) {
        if (!info.second->etc.enable_device()) {
            continue;
        }
        setTof3dParams(info.second->etc.ip());
        getTof3dParams(info.second->etc.ip());
        mp_thread_info.insert(std::pair<std::string, ThreadInfo>(
                info.first, ThreadInfo(new std::thread(&DeviceTof3D::receiveFrameThread, this, info.first),
                                       new Thread_condition()
                )
        ));
    }

    for (auto &t: mp_thread_info) {
        t.second.condit->notify_all(true);
    }
    detect_thread.t = new std::thread(&DeviceTof3D::detectThread, this);
    detect_thread.condit = new Thread_condition();
    detect_thread.condit->notify_all(true);

    return {TOF3D_VZENSE_DEVICE_INIT_SUCCESS, NORMAL, "VzInitialize success."};
}

Error_manager DeviceTof3D::SearchDevice(const double &time) {
    uint32_t deviceCount = 0;

    auto t = std::chrono::steady_clock::now();
    std::chrono::duration<double> cost = std::chrono::steady_clock::now() - t;
    while (cost.count() < time) {
        m_vz_status = VZ_GetDeviceCount(&deviceCount);
        cost = std::chrono::steady_clock::now() - t;
        if (m_vz_status != VzReturnStatus::VzRetOK) {
            LOG(INFO) << "VzGetDeviceCount failed! make sure the DCAM is connected";
            std::this_thread::sleep_for(std::chrono::seconds(1));
            continue;
        }
        std::this_thread::sleep_for(std::chrono::seconds(1));
        LOG(INFO) << "Found device count: " << deviceCount;
        if (deviceCount == 4) {
            break;
        }
    }

    VzDeviceInfo *pDeviceListInfo = new VzDeviceInfo[deviceCount];

    m_vz_status = VZ_GetDeviceInfoList(deviceCount, pDeviceListInfo);

    if (m_vz_status != VzReturnStatus::VzRetOK) {
        LOG(INFO) << "GetDeviceListInfo failed status:" << m_vz_status;
        return {}; //TODO
    } else {
        for (int i = 0; i < deviceCount; i++) {
            auto iter = mp_device_info.find(pDeviceListInfo[i].ip);
            if (iter == mp_device_info.end()) {
                mp_device_info.insert(
                        std::pair<std::string, tof3dVzenseInfo *>(pDeviceListInfo[i].ip, new tof3dVzenseInfo()));
                iter = mp_device_info.find(pDeviceListInfo[i].ip);
            }
            iter->second->info = pDeviceListInfo[i];
            LOG(INFO) << "Found device: " << pDeviceListInfo[i].ip;
        }
    }
    return {};
}

Error_manager DeviceTof3D::ConnectDevice(const std::string &ip, bool open_stream) {
    auto iter = mp_device_info.find(ip);
    if (iter == mp_device_info.end()) {
        LOG(WARNING) << "Can\'t find " << ip << " in mp_device_info";
        return {}; //TODO
    }

    m_vz_status = VZ_OpenDeviceByIP(iter->second->info.ip, &iter->second->handle);
    if (m_vz_status != VzReturnStatus::VzRetOK) {
        iter->second->is_connect = false;
        LOG(WARNING) << "OpenDevice " << ip << " failed status:" << m_vz_status;
        return {}; //TODO
    } else {
        iter->second->is_connect = true;
        LOG(INFO) << "OpenDevice " << ip << " success status:" << m_vz_status;
    }

    if (open_stream) {
        m_vz_status = VZ_StartStream(iter->second->handle);
        if (m_vz_status != VzReturnStatus::VzRetOK) {
            iter->second->is_start_stream = false;
            LOG(WARNING) << "VZ_StartStream " << ip << " failed status:" << m_vz_status;
            return {};
        } else {
            iter->second->is_start_stream = true;
            LOG(INFO) << "VZ_StartStream " << ip << " success status:" << m_vz_status;
        }
    }

    return {};
}

Error_manager DeviceTof3D::ConnectAllEtcDevices(bool open_stream) {
    for (auto &device: mp_device_info) {
        if (device.second->etc.enable_device()) {
            ConnectDevice(device.first, open_stream);
        }
    }
    return {};
}

Error_manager DeviceTof3D::ConnectAllDevices(bool open_stream) {
    for (auto &device: mp_device_info) {
        ConnectDevice(device.first, open_stream);
    }
    return {};
}

Error_manager DeviceTof3D::DisConnectDevice(const std::string &ip) {
    auto iter = mp_device_info.find(ip);
    if (iter == mp_device_info.end()) {
        return {}; //TODO
    }

    m_vz_status = VZ_StopStream(iter->second->handle);
    if (m_vz_status != VzReturnStatus::VzRetOK) {
        iter->second->is_start_stream = true;
        LOG(WARNING) << "VZ_StopStream failed status:" << m_vz_status;
    } else {
        iter->second->is_start_stream = false;
        LOG(INFO) << "VZ_StopStream success status:" << m_vz_status;
    }

    m_vz_status = VZ_CloseDevice(&iter->second->handle);
    if (m_vz_status != VzReturnStatus::VzRetOK) {
        iter->second->is_connect = true;
        LOG(WARNING) << "VZ_CloseDevice failed status:" << m_vz_status;
    } else {
        iter->second->is_connect = false;
        LOG(INFO) << "VZ_CloseDevice success status:" << m_vz_status;
    }

    return {};
}

Error_manager DeviceTof3D::DisConnectAllEtcDevices() {
    for (auto &device: mp_device_info) {
        if (device.second->etc.enable_device()) {
            DisConnectDevice(device.first);
        }
    }
    return {};
}

Error_manager DeviceTof3D::DisConnectAllDevices() {
    for (auto &device: mp_device_info) {
        DisConnectDevice(device.first);
    }
    return {};
}

Error_manager DeviceTof3D::getDepthFrame(const std::string &ip, VzFrame &depthFrame) {
    auto iter = mp_device_info.find(ip);
    if (iter == mp_device_info.end()) {
        return {FAILED, NORMAL, "Device %s not in list, can\'t get depth frame.", ip.c_str()}; //TODO
    }

    if (iter->second->handle == nullptr || !iter->second->is_connect) {
        if (ConnectDevice(ip, true) != SUCCESS) {
            return {FAILED, NORMAL, "Open device %s failed, stop get depth frame.", ip.c_str()};
        }
    }

    VzFrameReady frameReady = {0};
    m_vz_status = VZ_GetFrameReady(iter->second->handle, 200, &frameReady);

    if (m_vz_status != VzRetOK) {
        LOG(WARNING) << ip << "  VZ_GetFrameReady failed status:" << m_vz_status;
        return {FAILED, MINOR_ERROR, "%s VZ_GetFrameReady failed status %d!", ip.c_str(), m_vz_status}; //TODO
    }

    if (1 == frameReady.depth) {
        m_vz_status = VZ_GetFrame(iter->second->handle, VzDepthFrame, &depthFrame);

        if (m_vz_status == VzRetOK && depthFrame.pFrameData != nullptr) {
//            LOG(INFO) << ip << " VZ_GetFrame VzDepthFrame success:" << m_vz_status;
        } else {
            LOG(INFO) << ip << " VZ_GetFrame VzDepthFrame failed:" << m_vz_status;
        }
    }

    return {};
}

Error_manager DeviceTof3D::getIrFrame(const std::string &ip, VzFrame &irFrame) {
    auto iter = mp_device_info.find(ip);
    if (iter == mp_device_info.end()) {
        return {FAILED, NORMAL, "Device %s not in list, can\'t get ir frame.", ip.c_str()}; //TODO
    }

    if (iter->second->handle == nullptr || !iter->second->is_connect) {
        if (ConnectDevice(ip, true) != SUCCESS) {
            return {FAILED, NORMAL, "Open device %s failed, stop get ir frame.", ip.c_str()};
        }
    }

    VzFrameReady frameReady = {0};
    m_vz_status = VZ_GetFrameReady(iter->second->handle, 200, &frameReady);

    if (m_vz_status != VzRetOK) {
        LOG(WARNING) << ip << "  VZ_GetFrameReady failed status:" << m_vz_status;
        return {}; //TODO
    }

    if (1 == frameReady.ir) {
        m_vz_status = VZ_GetFrame(iter->second->handle, VzIRFrame, &irFrame);

        if (m_vz_status == VzRetOK && irFrame.pFrameData != nullptr) {
            LOG(INFO) << ip << "VZ_GetFrame VzIrFrame success:" << m_vz_status;
        } else {
            LOG(INFO) << ip << "VZ_GetFrame VzIrFrame failed:" << m_vz_status;
        }
    }

    return {};
}

Error_manager DeviceTof3D::getDepthAndIrPicture(const std::string &ip, VzFrame &depthFrame, VzFrame &irFrame) {
    auto iter = mp_device_info.find(ip);
    if (iter == mp_device_info.end()) {
        return {FAILED, NORMAL, "Device %s not in list, can\'t get depth and ir frame.", ip.c_str()}; //TODO
    }

    if (iter->second->handle == nullptr || !iter->second->is_connect) {
        if (ConnectDevice(ip, true) != SUCCESS) {
            return {FAILED, NORMAL, "Open device %s failed, stop get depth and ir frame.", ip.c_str()};
        }
    }

    Error_manager ret;
    VzFrameReady frameReady = {0};
    m_vz_status = VZ_GetFrameReady(iter->second->handle, 200, &frameReady);

    if (m_vz_status != VzRetOK) {
        LOG(WARNING) << ip << "  VZ_GetFrameReady failed status:" << m_vz_status;
        ret.compare_and_merge_up({FAILED, MINOR_ERROR, "Device %s VZ_GetFrameReady failed status: %d.", ip.c_str(),
                                  m_vz_status});    //TODO
        return ret;
    }

    //Get depth frame, depth frame only output in following data mode
    if (1 == frameReady.depth) {
        m_vz_status = VZ_GetFrame(iter->second->handle, VzDepthFrame, &depthFrame);

        if (m_vz_status == VzRetOK && depthFrame.pFrameData != nullptr) {
            // LOG(INFO) << ip << " frameIndex :" << depthFrame.frameIndex;
        } else {
            ret.compare_and_merge_up(
                    {FAILED, MINOR_ERROR, "Device %s VZ_GetFrame VzIrFrame failed status: %d.", ip.c_str(),
                     m_vz_status});    //TODO
            LOG(INFO) << ip << "VZ_GetFrame VzIrFrame status:" << m_vz_status;
        }
    } else {
        ret.compare_and_merge_up(
                {FAILED, MINOR_ERROR, "Device %s VZ_GetFrameReady depth not ready: %d.", ip.c_str(), frameReady.depth});
    }

    if (1 == frameReady.ir) {
        m_vz_status = VZ_GetFrame(iter->second->handle, VzIRFrame, &irFrame);
        if (m_vz_status == VzRetOK && irFrame.pFrameData != nullptr) {
            // LOG(INFO) << ip << " frameIndex :" << irFrame.frameIndex;
        } else {
            ret.compare_and_merge_up(
                    {FAILED, MINOR_ERROR, "Device %s VZ_GetFrame VzIrFrame failed status: %d.", ip.c_str(),
                     m_vz_status});    //TODO
            LOG(INFO) << ip << "VZ_GetFrame VzIrFrame status:" << m_vz_status;
        }
    } else {
        ret.compare_and_merge_up(
                {FAILED, NORMAL, "Device %s VZ_GetFrameReady ir not ready: %d.", ip.c_str(), frameReady.depth});
    }

    return ret;
}

Error_manager DeviceTof3D::getDepthPointCloud(const std::string &ip, pcl::PointCloud<pcl::PointXYZ>::Ptr &cloud) {
    auto iter = mp_device_info.find(ip);
    if (iter == mp_device_info.end()) {
        return {FAILED, NORMAL, "Device %s not in list, can\'t get point cloud.", ip.c_str()}; //TODO
    }

    if (iter->second->handle == nullptr || !iter->second->is_connect) {
        if (ConnectDevice(ip, true) != SUCCESS) {
            return {FAILED, NORMAL, "Open device %s failed, stop get point cloud.", ip.c_str()};
        }
    }

    VzFrameReady frameReady = {0};
    m_vz_status = VZ_GetFrameReady(iter->second->handle, 200, &frameReady);

    if (m_vz_status != VzRetOK) {
        LOG(WARNING) << ip << "  VZ_GetFrameReady failed status:" << m_vz_status;
        return {}; //TODO
    }

    //Get depth frame, depth frame only output in following data mode
    if (1 == frameReady.depth) {
        VzFrame depthFrame = {0};
        m_vz_status = VZ_GetFrame(iter->second->handle, VzDepthFrame, &depthFrame);

        if (m_vz_status == VzRetOK && depthFrame.pFrameData != nullptr) {
            LOG(INFO) << ip << " frameIndex :" << depthFrame.frameIndex;

            VzFrame &srcFrame = depthFrame;
            const int len = srcFrame.width * srcFrame.height;
            VzVector3f *worldV = new VzVector3f[len];

            m_vz_status = VZ_ConvertDepthFrameToPointCloudVector(iter->second->handle, &srcFrame,
                                                                 worldV);

            if (m_vz_status == VzRetOK) {
                cloud->clear();
                for (int i = 0; i < len; i++) {
                    if (worldV[i].x == 0 && worldV[i].y == 0 && worldV[i].z == 0) {
                        continue;
                    }
                    cloud->points.emplace_back(worldV[i].x, worldV[i].y, worldV[i].z);
                }
                delete[] worldV;
                worldV = nullptr;
                LOG(INFO) << "Save point cloud successful to cloud: " << cloud->size();
            }
        } else {
            LOG(INFO) << ip << "VZ_GetFrame VzIrFrame status:" << m_vz_status;
        }
    }

    return {};
}

Error_manager DeviceTof3D::DepthFrame2PointCloud(const std::string &ip, VzFrame &depthFrame,
                                                 pcl::PointCloud<pcl::PointXYZ>::Ptr &cloud) {
//    if (depthFrame.frameType != VzDepthFrame) {
//        return {};
//    }
//
//    auto handle_iter = mp_devices_handle.find(ip);
//    if (handle_iter == mp_devices_handle.end()) {
//        return {}; //TODO
//    }
//
//    cloud->clear();
//    if (handle_iter->second) {
//
//        VzFrame &srcFrame = depthFrame;
//        const int len = srcFrame.width * srcFrame.height;
//        VzVector3f *worldV = new VzVector3f[len];
//
//        VZ_ConvertDepthFrameToPointCloudVector(handle_iter->second, &depthFrame, worldV);
//
//        for (int i = 0; i < len; i++) {
//            cloud->points.emplace_back(worldV[i].x, worldV[i].y, worldV[i].z);
//        }
//        delete[] worldV;
//        worldV = nullptr;
//        LOG(INFO) << "Save point cloud successful to cloud: " << cloud->size();
//        WriteTxtCloud(ETC_PATH"Tof3d/data/cloud.txt", cloud);
//    }

    return {};
}

Error_manager DeviceTof3D::Frame2Mat(VzFrame &frame, cv::Mat &mat) {
    switch (frame.frameType) {
        case VzDepthFrame:
            return DepthFrame2Mat(frame, mat);
        case VzIRFrame:
            return IrFrame2Mat(frame, mat);
        default:
            break;
    }
    return {};
}

Error_manager DeviceTof3D::DepthFrame2Mat(VzFrame &frame, cv::Mat &mat) {
    if (frame.frameType != VzDepthFrame) {
        return {};
    }

    if (mat.type() != CV_16UC1) {
        LOG(INFO) << "Mat type " << mat.type() << " not is " << CV_16UC1;
        return {};
    }

    if (mat.rows != 480 || mat.cols != 640) {
        LOG(INFO) << "Mat size (" << mat.rows << ", " << mat.cols << ") not is (480, 640)";
        return {};
    }

    memcpy(mat.data, frame.pFrameData, frame.dataLen);
    mat.convertTo(mat, CV_8U, 255.0 / 7495);
    applyColorMap(mat, mat, cv::COLORMAP_RAINBOW);

    return {};
}

Error_manager DeviceTof3D::IrFrame2Mat(VzFrame &frame, cv::Mat &mat) {
    if (frame.frameType != VzIRFrame) {
        return {};
    }

    if (mat.type() != CV_8UC1) {
        LOG(INFO) << "Mat type " << mat.type() << " not is " << CV_8UC1;
        return {};
    }

    if (mat.rows != 480 || mat.cols != 640) {
        LOG(INFO) << "Mat size (" << mat.rows << ", " << mat.cols << ") not is (480, 640)";
        return {};
    }

    memcpy(mat.data, frame.pFrameData, frame.dataLen);

    return {};
}

#include <random>
#include "file/pathcreator.h"

void DeviceTof3D::receiveFrameThread(const std::string &ip) {
    LOG(INFO) << ip << " in thread " << std::this_thread::get_id();
    auto iter = mp_device_info.find(ip);
    auto t_iter = mp_thread_info.find(ip);
    std::default_random_engine e;
    e.seed(time(0));
    const std::string path = ETC_PATH PROJECT_NAME "/image/";

    Eigen::Matrix3f rotation_matrix3;
    rotation_matrix3 =
            Eigen::AngleAxisf(iter->second->etc.trans().yaw() * M_PI / 180.f, Eigen::Vector3f::UnitZ()) *
            Eigen::AngleAxisf(iter->second->etc.trans().pitch() * M_PI / 180.f, Eigen::Vector3f::UnitY()) *
            Eigen::AngleAxisf(iter->second->etc.trans().roll() * M_PI / 180.f, Eigen::Vector3f::UnitX());
    Eigen::Matrix4f pcl_transform;
    pcl_transform.setIdentity();
    pcl_transform.block<3,3>(0,0) = rotation_matrix3;
    Eigen::Vector3f move;
    move << iter->second->etc.trans().x(), iter->second->etc.trans().y(), iter->second->etc.trans().z();
    pcl_transform.topRightCorner<3, 1>() = move;
    pcl::PointCloud<pcl::PointXYZ>::Ptr frame_cloud(new pcl::PointCloud<pcl::PointXYZ>);
    LOG(INFO) << "\n" << pcl_transform;

    while (t_iter->second.condit->is_alive()) {
        t_iter->second.condit->wait();
        if (t_iter->second.condit->is_alive()) {
            Error_manager ret;
            if (iter->second->handle) {
                DeviceMat depthInfo;
                VzFrame depthFrame = {0};
                VzFrame irFrame = {0};
                frame_cloud->clear();

                // 图像获取
                if (getDepthAndIrPicture(ip, depthFrame, irFrame) == SUCCESS &&
                    Frame2Mat(depthFrame, depthInfo.depthMat) == SUCCESS &&
                    Frame2Mat(irFrame, depthInfo.irMat) == SUCCESS) {
                    // 点云转换
                    VzSensorIntrinsicParameters cameraParam = {};
                    VZ_GetSensorIntrinsicParameters(iter->second->handle, VzToFSensor, &cameraParam);

                    const uint16_t *pDepthFrameData = (uint16_t *) depthFrame.pFrameData;
                    for (int cols = 0; cols < depthInfo.pointMat.cols - 0; cols++) {
                        for (int rows = 0; rows < depthInfo.pointMat.rows - 0; rows++) {
                            VzDepthVector3 depthPoint = {cols, rows, pDepthFrameData[rows * depthInfo.pointMat.cols + cols]};
                            VzVector3f worldV = {};
                            VZ_ConvertDepthToPointCloud(iter->second->handle, &depthPoint, &worldV, 1, &cameraParam);
                            if (sqrt(worldV.x * worldV.x + worldV.y * worldV.y + worldV.z * worldV.z) * 0.001f > 5) {
                                continue;
                            }
                            frame_cloud->emplace_back(worldV.x * 0.001f, worldV.y * 0.001f, worldV.z * 0.001f);
                            Eigen::Vector3f trans;
                            trans << worldV.x * 0.001f, worldV.y * 0.001f, worldV.z * 0.001f;
                            trans =  rotation_matrix3 * trans + move;
                            if (trans(2) < 0.03 || fabs(trans(0)) > 1.5 || fabs(trans(1)) > 2.7 || trans(2) > 1) {
                                continue;
                            }
                            depthInfo.pointMat.at<cv::Vec4f>(rows, cols)[0] = trans(0);
                            depthInfo.pointMat.at<cv::Vec4f>(rows, cols)[1] = trans(1);
                            depthInfo.pointMat.at<cv::Vec4f>(rows, cols)[2] = trans(2);
                        }
                    }
                    // 将图片和点云存储起来
                    m_device_mat[iter->second->etc.azimuth()].reset(depthInfo, 0.4);
                } else {
                    ret.compare_and_merge_up({FAILED, MINOR_ERROR, "Device %s VZ_GetFrame failed.", ip.c_str()});
                }
            } else {
                LOG_EVERY_N(WARNING, 1000) << ip << " device handle is null.";
            }

            if (!ret.get_error_description().empty()) {
                LOG(INFO) << ret.get_error_description();
            }
            std::this_thread::sleep_for(std::chrono::milliseconds(1));
        }
    }

    DisConnectDevice(ip);
    LOG(INFO) << ip << " thread end " << std::this_thread::get_id();
}

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

    pcl::PointCloud<pcl::PointXYZ>::Ptr t_wheel_cloud(new pcl::PointCloud<pcl::PointXYZ>);
    std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> vct_wheel_cloud;
    for (int i = 0; i < DeviceAzimuth::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;

    int index = 0;
    const std::string path = ETC_PATH PROJECT_NAME "/image/";
    if (PathCreator::IsFolder(path)) {
        std::vector<std::string> imagePathList;
        cv::glob(path + "/*.jpg", imagePathList);
        index = imagePathList.size();
    }

    DetectResult detect_result;
    while (detect_thread.condit->is_alive()) {
        detect_thread.condit->wait();
        // 参数初始化
        t_wheel_cloud->clear();
        detect_result.reset();
        t_start_time = std::chrono::steady_clock::now();

        if (detect_thread.condit->is_alive()) {
            ::JetStream::ResFrame frame;
            // 获取最新数据
            cv::Mat merge_mat = cv::Mat::zeros(480*2, 640*2, CV_8UC1);
            DeviceMat t_device_mat[DeviceAzimuth::MAX];
            for (int device_index = 0; device_index < DeviceAzimuth::MAX; device_index++) {
                if (!m_device_mat[device_index].timeout()) {
                    detect_result.wheel[device_index].haveCloud = true;
                    t_device_mat[device_index] = m_device_mat[device_index].Get();
                    // 拼接图像
                    cv::Mat merge_mat_lf = merge_mat(cv::Rect(0, 0, 640, 480));
                    t_device_mat[device_index].irMat.copyTo(
                            merge_mat(cv::Rect(
                                    (device_index & 0x1) * t_device_mat[device_index].irMat.cols,
                                    ((device_index & 0x2) >> 1) * t_device_mat[device_index].irMat.rows,
                                    t_device_mat[device_index].irMat.cols,
                                    t_device_mat[device_index].irMat.rows)));

                    grpcVzenseMat(frame, (DeviceAzimuth)device_index, t_device_mat[device_index].irMat);
                } else {
                    DLOG(WARNING) << device_index << " not get data.";
                }
            }

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

            // 模型识别
            std::vector<Object> objs;
            cv::Mat mer_mat_rgb;
            cv::cvtColor(merge_mat, mer_mat_rgb, cv::COLOR_GRAY2RGB);
            detector->detect(mer_mat_rgb, objs, mer_mat_rgb);
            cost = std::chrono::steady_clock::now() - t_start_time;
            LOG(INFO) << "cost time is " << cost.count() * 1000 << " ms";

            // 标记识别点云
            bool save_image = false;
            for (auto &obj: objs) {
                if (obj.prob > 0.9) {
                    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].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);
                    }

                    // 自动存图功能
                    // cv::Point rect_center;
                    // rect_center.x = obj.rect.x + obj.rect.width * 0.5 - (device_index & 0x1) * t_device_mat[device_index].irMat.cols;
                    // rect_center.y = obj.rect.y + obj.rect.height * 0.5 - ((device_index & 0x2) >> 1) * t_device_mat[device_index].irMat.rows;
                    // detect_result.wheel[device_index].center.x = t_device_mat[device_index].pointMat.at<cv::Vec4f>(rect_center)[0];
                    // detect_result.wheel[device_index].center.y = t_device_mat[device_index].pointMat.at<cv::Vec4f>(rect_center)[1];
                    // if (fabs(m_detect_result_record.wheel[device_index].center.y - detect_result.wheel[device_index].center.y) > 0.2) {
                    //     save_image = true;
                    // }
                }
            }
            // if (save_image) {
            //     std::string file_name = ETC_PATH PROJECT_NAME "/image/merge_" + std::to_string(index++)+ ".jpg";
            //     cv::imwrite(file_name, merge_mat);
            // }

            // 提取点云数据
            for (int device_index = 0; device_index < DeviceAzimuth::MAX; device_index++) {
                if(!detect_result.wheel[device_index].haveCloud) { continue;}
                grpcVzensePointMat(frame, (DeviceAzimuth)device_index, t_device_mat[device_index].pointMat);    // use 10ms
            }
            cv::imshow("mergeImage", mer_mat_rgb);
            cv::waitKey(1);


            cost = std::chrono::steady_clock::now() - t_start_time;
            LOG(INFO) << "cost time is " << cost.count() * 1000 << " ms";
            // 当没有车辆点云的时候，说明规定区域内没有车辆，将重置测量记录
            if (objs.empty()) {
                m_detect_result_record.reset();
            }
            // 点云处理
            if (!objs.empty()) {
//                CarPoseOptimize(viewer, view_port, t_car_cloud, detect_result);
//                cost = std::chrono::steady_clock::now() - t_start_time;
//                LOG(INFO) << "cost time is " << cost.count() * 1000 << " ms";
                WheelCloudOptimize(vct_wheel_cloud, detect_result);
                cost = std::chrono::steady_clock::now() - t_start_time;
                LOG(INFO) << "cost time is " << cost.count() * 1000 << " ms";

                detect_wheel_ceres detector;
                if (m_detect_result_record.car.wheel_width < 1.0 || m_detect_result_record.car.wheel_base < 2.0 ||
                    objs.size() > 2) {
                    detect_result.car.wheel_width = 3;
                    detect_result.car.wheel_base = 2.7;
                    detector.detect(vct_wheel_cloud[DeviceAzimuth::LF], vct_wheel_cloud[DeviceAzimuth::RF],
                                    vct_wheel_cloud[DeviceAzimuth::LR], vct_wheel_cloud[DeviceAzimuth::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);
                    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 = 0;
                        detect_result.car.wheel_base = 0;
                    } else if (objs.size() == 2) {
                        std::vector<int> have_cloud;
                        for (int device_index = 0; device_index < DeviceAzimuth::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 = 0;
                        } else {
                            detect_result.car.wheel_base = 0;
                        }
                    }
                } else {
                    detector.detect_dynP(vct_wheel_cloud[DeviceAzimuth::LF], vct_wheel_cloud[DeviceAzimuth::RF],
                                         vct_wheel_cloud[DeviceAzimuth::LR], vct_wheel_cloud[DeviceAzimuth::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.wheel_base = m_detect_result_record.car.wheel_base;
                    detect_result.car.wheel_width = m_detect_result_record.car.wheel_width;
                }
                m_detect_result_record = detect_result;
            }
            cost = std::chrono::steady_clock::now() - t_start_time;
            LOG(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);
            }
            char debug_info[512];
            sprintf(debug_info, "\nwidth: (%0.4f, %0.4f, %0.4f), base: (%0.4f, %0.4f, %0.4f)\n",
                    min_wheel_width, m_detect_result_record.car.wheel_width, max_wheel_width,
                    min_wheel_base, m_detect_result_record.car.wheel_base, max_wheel_base);

            // grpc数据传出
            frame.mutable_measure_info()->set_x(m_detect_result_record.car.wheels_center_x);
            frame.mutable_measure_info()->set_y(m_detect_result_record.car.wheels_center_y);
            frame.mutable_measure_info()->set_width(m_detect_result_record.car.wheel_width);
            frame.mutable_measure_info()->set_wheelbase(m_detect_result_record.car.wheel_base);
            frame.mutable_measure_info()->set_theta(m_detect_result_record.car.theta);
            frame.mutable_measure_info()->set_ftheta(m_detect_result_record.car.front_wheels_theta);
            frame.mutable_measure_info()->set_error(m_detect_result_record.info() + debug_info);
            LOG(INFO) << frame.measure_info().error();
            m_grpc_server.ResetData(frame);
            cost = std::chrono::steady_clock::now() - t_start_time;
            m_detect_result_record = detect_result;
            DLOG(INFO) << "All cost time is " << cost.count() * 1000 << " ms";
            usleep(1000 * 100);
        }
    }
    LOG(INFO) << "detect thread " << std::this_thread::get_id() << " closed.";
}

void DeviceTof3D::HotPlugStateCallback(const VzDeviceInfo *pInfo, int status, void *contex) {
    LOG(WARNING) << "uri " << status << "  " << pInfo->uri << "    " << (status == 0 ? "add" : "remove");
    LOG(WARNING) << "alia " << status << "  " << pInfo->alias << "    " << (status == 0 ? "add" : "remove");

    if (contex == nullptr) {
        return;
    }

    tof3dVzenseInfoMap *mp = (tof3dVzenseInfoMap *) contex;
    auto iter = mp->find(pInfo->ip);
    LOG(INFO) << iter->first;
    if (status == 0) {
        LOG(WARNING) << "VZ_OpenDevice " << VZ_OpenDeviceByUri(pInfo->uri, &iter->second->handle);
        LOG(WARNING) << "VZ_StartStream " << VZ_StartStream(iter->second->handle);
        iter->second->is_connect = true;
        iter->second->is_start_stream = true;
    } else {
        LOG(WARNING) << "VZ_StopStream " << VZ_StopStream(iter->second->handle);
        LOG(WARNING) << "VZ_CloseDevice " << VZ_CloseDevice(&iter->second->handle);
        iter->second->is_connect = false;
        iter->second->is_start_stream = false;
    }
}

bool DeviceTof3D::drawBox(cv::Mat &mat, cv::Rect &box, cv::Scalar &color, std::string className, float confidence) {
    // Detection box
    cv::rectangle(mat, box, color, 2);
    // Detection box text
    std::string classString = className + ' ' + std::to_string(confidence).substr(0, 4);
    cv::Size textSize = cv::getTextSize(classString, cv::FONT_HERSHEY_DUPLEX, 1, 2, 0);
    cv::Rect textBox(box.x, box.y - 40, textSize.width + 10, textSize.height + 20);
    cv::rectangle(mat, textBox, color, cv::FILLED);
    cv::putText(mat, classString, cv::Point(box.x + 5, box.y - 10), cv::FONT_HERSHEY_DUPLEX, 1, cv::Scalar(0, 0, 0), 2,
                0);
    return true;
}

Error_manager DeviceTof3D::updateTof3dEtc() {
    for (auto &info: mp_device_info) {
        if (info.second->etc.enable_device()) {

        }
    }
    return Error_manager();
}

Error_manager DeviceTof3D::loadEtc(const DeviceTof3D::VzEtcMap &etc) {
    for (auto &iter: etc) {
        tof3dVzenseInfo *info = new tof3dVzenseInfo();
        info->etc = iter.second;
        mp_device_info.insert(std::pair<std::string, tof3dVzenseInfo *>(iter.first, info));
        LOG(INFO) << "Get device " << iter.first << " etc: " << iter.second.DebugString();
    }
    return Error_manager();
}

void DeviceTof3D::stopWorking() {
    for (auto &info: mp_device_info) {
        auto iter = mp_thread_info.find(info.second->etc.ip());
        if (iter != mp_thread_info.end()) {
            iter->second.condit->kill_all();
            iter->second.t->join();
            delete iter->second.t;
            iter->second.t = nullptr;
        }
    }
    mp_device_info.clear();
    mp_thread_info.clear();
}

Error_manager DeviceTof3D::reInit(const VzEtcMap &etc) {
    LOG(INFO) << "================= Reinit Device Tof3D.";
    stopWorking();

    Error_manager ret = Init(etc, true);
    return ret;
}

Error_manager DeviceTof3D::setTof3dParams(const std::string &ip) {
    auto iter = mp_device_info.find(ip);
    if (iter == mp_device_info.end()) {
        LOG(WARNING) << "Can\'t find " << ip << " in mp_device_info";
        return {}; //TODO
    }

    m_vz_status = VZ_SetWorkMode(iter->second->handle, (VzWorkMode) iter->second->etc.bip().work_mode());
    if (m_vz_status != VzReturnStatus::VzRetOK) {
        LOG(WARNING) << ip << " VZ_SetWorkMode failed status:" << m_vz_status;
        return {};  //TODO
    }

    m_vz_status = VZ_SetIRGMMGain(iter->second->handle, iter->second->etc.bip().irgmmgain());
    if (m_vz_status != VzReturnStatus::VzRetOK) {
        LOG(WARNING) << ip << " VZ_SetIRGMMGain failed status:" << m_vz_status;
        return {};  //TODO
    }

    m_vz_status = VZ_SetFrameRate(iter->second->handle, iter->second->etc.bip().frame_rate());
    if (m_vz_status != VzReturnStatus::VzRetOK) {
        LOG(WARNING) << ip << " VZ_SetFrameRate failed status:" << m_vz_status;
        return {};  //TODO
    }

    VzExposureTimeParams exposure_param;
    exposure_param.mode = (VzExposureControlMode) iter->second->etc.bip().enable_manual_exposure_time();
    exposure_param.exposureTime = iter->second->etc.bip().exposure_time();
    m_vz_status = VZ_SetExposureTime(iter->second->handle, VzToFSensor, exposure_param);
    if (m_vz_status != VzReturnStatus::VzRetOK) {
        LOG(WARNING) << ip << " VZ_SetExposureTime failed status:" << m_vz_status;
        return {};  //TODO
    }

    m_vz_status = VZ_SetFillHoleFilterEnabled(iter->second->handle, iter->second->etc.bip().enable_filter_fill_hole());
    if (m_vz_status != VzReturnStatus::VzRetOK) {
        LOG(WARNING) << ip << " VZ_SetFillHoleFilterEnabled failed status:" << m_vz_status;
        return {};  //TODO
    }

    m_vz_status = VZ_SetSpatialFilterEnabled(iter->second->handle, iter->second->etc.bip().enable_filter_spatial());
    if (m_vz_status != VzReturnStatus::VzRetOK) {
        LOG(WARNING) << ip << " VZ_SetSpatialFilterEnabled failed status:" << m_vz_status;
        return {};  //TODO
    }

    VzFlyingPixelFilterParams fly;
    fly.enable = iter->second->etc.bip().enable_flying_pixel_filter();
    fly.threshold = iter->second->etc.bip().flying_pixel_filter_value();
    m_vz_status = VZ_SetFlyingPixelFilterParams(iter->second->handle, fly);
    if (m_vz_status != VzReturnStatus::VzRetOK) {
        LOG(WARNING) << ip << " VZ_SetFlyingPixelFilterParams failed status:" << m_vz_status;
        return {};  //TODO
    }

    VzConfidenceFilterParams confidence;
    confidence.enable = iter->second->etc.bip().enable_confidence_filter();
    confidence.threshold = iter->second->etc.bip().confidence_filter_value();
    m_vz_status = VZ_SetConfidenceFilterParams(iter->second->handle, confidence);
    if (m_vz_status != VzReturnStatus::VzRetOK) {
        LOG(WARNING) << ip << " VZ_SetConfidenceFilterParams failed status:" << m_vz_status;
        return {};  //TODO
    }

    VzTimeFilterParams time_filter;
    time_filter.enable = iter->second->etc.bip().enable_time_filter();
    time_filter.threshold = iter->second->etc.bip().time_filter_value();
    m_vz_status = VZ_SetTimeFilterParams(iter->second->handle, time_filter);
    if (m_vz_status != VzReturnStatus::VzRetOK) {
        LOG(WARNING) << ip << " VZ_SetTimeFilterParams failed status:" << m_vz_status;
        return {};  //TODO
    }

    return Error_manager();
}

Error_manager DeviceTof3D::getTof3dParams(const std::string &ip) {
    auto iter = mp_device_info.find(ip);
    if (iter == mp_device_info.end()) {
        LOG(WARNING) << "Can\'t find " << ip << " in mp_device_info";
        return {}; //TODO
    }

    // 获取参数
    VzWorkMode mode;
    m_vz_status = VZ_GetWorkMode(iter->second->handle, &mode);
    if (m_vz_status != VzReturnStatus::VzRetOK) {
        LOG(WARNING) << ip << " VZ_GetWorkMode failed status:" << m_vz_status;
        return {};  //TODO
    }
    iter->second->etc.mutable_bip()->set_work_mode(mode);

    uint8_t irgmmgain;
    m_vz_status = VZ_GetIRGMMGain(iter->second->handle, &irgmmgain);
    if (m_vz_status != VzReturnStatus::VzRetOK) {
        LOG(WARNING) << ip << " VZ_GetIRGMMGain failed status:" << m_vz_status;
        return {};  //TODO
    }
    iter->second->etc.mutable_bip()->set_irgmmgain(irgmmgain);

    int frame_rate;
    m_vz_status = VZ_GetFrameRate(iter->second->handle, &frame_rate);
    if (m_vz_status != VzReturnStatus::VzRetOK) {
        LOG(WARNING) << ip << " VZ_GetFrameRate failed status:" << m_vz_status;
        return {};  //TODO
    }
    iter->second->etc.mutable_bip()->set_frame_rate(frame_rate);

    VzExposureTimeParams exposure_param = {VzExposureControlMode_Manual, 0};
    m_vz_status = VZ_GetProperty(iter->second->handle, "Py_ToFExposureTimeMax", &exposure_param,
                                 sizeof(exposure_param));
    if (m_vz_status != VzReturnStatus::VzRetOK) {
        LOG(WARNING) << ip << " VZ_GetExposureTime failed status:" << m_vz_status;
        return {};  //TODO
    }
    iter->second->etc.mutable_bip()->set_enable_manual_exposure_time(exposure_param.mode);
    iter->second->etc.mutable_bip()->set_exposure_time(exposure_param.exposureTime);

    bool boolret;
    m_vz_status = VZ_GetFillHoleFilterEnabled(iter->second->handle, &boolret);
    if (m_vz_status != VzReturnStatus::VzRetOK) {
        LOG(WARNING) << ip << " VZ_GetFillHoleFilterEnabled failed status:" << m_vz_status;
        return {};  //TODO
    }
    iter->second->etc.mutable_bip()->set_enable_filter_fill_hole(boolret);

    m_vz_status = VZ_GetSpatialFilterEnabled(iter->second->handle, &boolret);
    if (m_vz_status != VzReturnStatus::VzRetOK) {
        LOG(WARNING) << ip << " VZ_GetSpatialFilterEnabled failed status:" << m_vz_status;
        return {};  //TODO
    }
    iter->second->etc.mutable_bip()->set_enable_filter_spatial(boolret);

    VzFlyingPixelFilterParams fly;
    m_vz_status = VZ_GetFlyingPixelFilterParams(iter->second->handle, &fly);
    if (m_vz_status != VzReturnStatus::VzRetOK) {
        LOG(WARNING) << ip << " VZ_GetFlyingPixelFilterParams failed status:" << m_vz_status;
        return {};  //TODO
    }
    iter->second->etc.mutable_bip()->set_enable_flying_pixel_filter(fly.enable);
    iter->second->etc.mutable_bip()->set_flying_pixel_filter_value(fly.threshold);

    VzConfidenceFilterParams confidence;
    m_vz_status = VZ_GetConfidenceFilterParams(iter->second->handle, &confidence);
    if (m_vz_status != VzReturnStatus::VzRetOK) {
        LOG(WARNING) << ip << " VZ_GetConfidenceFilterParams failed status:" << m_vz_status;
        return {};  //TODO
    }
    iter->second->etc.mutable_bip()->set_enable_confidence_filter(confidence.enable);
    iter->second->etc.mutable_bip()->set_confidence_filter_value(confidence.threshold);

    VzTimeFilterParams time_filter;
    m_vz_status = VZ_GetTimeFilterParams(iter->second->handle, &time_filter);
    if (m_vz_status != VzReturnStatus::VzRetOK) {
        LOG(WARNING) << ip << " VZ_GetTimeFilterParams failed status:" << m_vz_status;
        return {};  //TODO
    }
    iter->second->etc.mutable_bip()->set_enable_time_filter(time_filter.enable);
    iter->second->etc.mutable_bip()->set_time_filter_value(time_filter.threshold);

    return Error_manager();
}

Error_manager DeviceTof3D::segFrame2CarAndWheel(VzFrame &depth_frame, std::vector<cv::Point> &wheel_cv_cloud,
                                                pcl::PointCloud<pcl::PointXYZ>::Ptr &car_cloud,
                                                pcl::PointCloud<pcl::PointXYZ>::Ptr &wheel_cloud) {

    auto iter = mp_device_info.find("192.168.2.102");
    if (iter->second->handle != nullptr) {
        VzSensorIntrinsicParameters cameraParam = {};
        VZ_GetSensorIntrinsicParameters(iter->second->handle, VzToFSensor, &cameraParam);

        const uint16_t *pDepthFrameData = (uint16_t *) depth_frame.pFrameData;
        for (auto &pt: wheel_cv_cloud) {
            VzDepthVector3 depthPoint = {pt.x, pt.y, pDepthFrameData[pt.y * depth_frame.width + pt.x]};
            VzVector3f worldV = {};
            VZ_ConvertDepthToPointCloud(iter->second->handle, &depthPoint, &worldV, 1, &cameraParam);
            wheel_cloud->emplace_back(worldV.x, worldV.y, worldV.z);
        }
//        LOG(INFO) << wheel_cloud->size();

        VzFrame &srcFrame = depth_frame;
        const int len = srcFrame.width * srcFrame.height;
        VzVector3f *worldV = new VzVector3f[len];

        m_vz_status = VZ_ConvertDepthFrameToPointCloudVector(iter->second->handle, &srcFrame,
                                                             worldV);

        if (m_vz_status == VzRetOK) {
            car_cloud->clear();
            for (int i = 0; i < len; i++) {
                if (worldV[i].x == 0 && worldV[i].y == 0 && worldV[i].z == 0) {
                    continue;
                }
                car_cloud->points.emplace_back(worldV[i].x, worldV[i].y, worldV[i].z);
            }
            delete[] worldV;
            worldV = nullptr;
//            LOG(INFO) << "Save point cloud successful to cloud: " << car_cloud->size();
        }

//        for (int i = (depth_frame.height - WINDOW_SIZE)/2, offset = i * depth_frame.width; i < (depth_frame.height + WINDOW_SIZE)/2; i++)
//        {
//            for (int j = (depth_frame.width - WINDOW_SIZE)/2; j < (depth_frame.width + WINDOW_SIZE)/2; j++)
//            {
//                VzDepthVector3 depthPoint = {j, i, pDepthFrameData[offset + j]};
//                VzVector3f worldV = {};
//                VZ_ConvertDepthToPointCloud(iter->second->handle, &depthPoint, &worldV, 1, &cameraParam);
//                if (0 < worldV.z && worldV.z < 0xFFFF)
//                {
//                    LOG(INFO) << worldV.x << "\t" << worldV.y << "\t" << worldV.z << std::endl;
//                }
//            }
//            offset += depth_frame.width;
//        }
    }
    return Error_manager();
}

Error_manager DeviceTof3D::WheelCloudOptimize(std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> &vct_wheel_cloud, DetectResult &result) {
    // 1、检测模型识别结果, 如果没有结、只有一个车轮、同侧车轮的结果, 返回无法策略
//    char seg_ret = 0;
//    for (int i = 0; i < DeviceAzimuth::MAX; i++) {
//        seg_ret |= result.wheel[i].segmentOk << i;
//    }
//    if (seg_ret == 0x1 || seg_ret == 0x2 || seg_ret == 0x4 || seg_ret == 0x8 ||
//        seg_ret == 0x0 || seg_ret == 0x5 || seg_ret == 0xa) {
//        return {};
//    }

    //下采样
    pcl::VoxelGrid<pcl::PointXYZ> vox;                     //创建滤波对象
    vox.setLeafSize(0.03f, 0.03f, 0.03f);     //设置滤波时创建的体素体积为1cm的立方体
    //离群点过滤
    pcl::StatisticalOutlierRemoval<pcl::PointXYZ> sor;
    sor.setMeanK(10);            //K近邻搜索点个数
    sor.setStddevMulThresh(2.0); //标准差倍数
    sor.setNegative(false);      //保留未滤波点（内点）

    // 2、
    pcl::PointCloud<pcl::PointXYZ>::Ptr filtered_cloud(new pcl::PointCloud<pcl::PointXYZ>);
    for (int device_index = 0; device_index < DeviceAzimuth::MAX; device_index++) {
        // vct_wheel_cloud[device_index]->clear();
        // Point3D_tool::ReadTxtCloud(ETC_PATH PROJECT_NAME "/data/save/wheel_" + std::to_string(device_index) + "_cloud.txt", vct_wheel_cloud[device_index]);
        if (vct_wheel_cloud[device_index]->empty()) { continue;}

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

        sor.setInputCloud(vct_wheel_cloud[device_index]);
        sor.filter(*vct_wheel_cloud[device_index]);                         //保存滤波结果到cloud_filter

        // filtered_cloud->clear();
        // WheelCeresDetector::get_instance_references().detect(vct_wheel_cloud[device_index], filtered_cloud, result.wheel[device_index], device_index % 2);
        // vct_wheel_cloud[device_index]->clear();
        // vct_wheel_cloud[device_index]->operator+=(*filtered_cloud);
        // Point3D_tool::WriteTxtCloud(ETC_PATH PROJECT_NAME "/data/filtered_wheel_" + std::to_string(device_index) + "_cloud.txt", filtered_cloud);
        // Point3D_tool::WriteTxtCloud(ETC_PATH PROJECT_NAME "/data/wheel_" + std::to_string(device_index) + "_cloud.txt", vct_wheel_cloud[device_index]);
    }
//    WheelCeresDetector::get_instance_references().detect(vct_wheel_cloud, filtered_cloud, result.wheel);
    // filtered_cloud->clear();
    // vct_wheel_cloud[DeviceAzimuth::LR]->clear();
    // WheelCeresDetector::get_instance_references().detect(vct_wheel_cloud[DeviceAzimuth::LR], filtered_cloud, result.wheel[DeviceAzimuth::LR],
    //                                                      vct_wheel_cloud[DeviceAzimuth::RR], filtered_cloud, result.wheel[DeviceAzimuth::RR]);
    // Point3D_tool::WriteTxtCloud(ETC_PATH PROJECT_NAME "/data/filtered_wheel_r_cloud.txt", filtered_cloud);

    // if (fabs(result.wheel[DeviceAzimuth::RR].theta) > 2.2) {exit(1);}
    return Error_manager();
}

Error_manager DeviceTof3D::CarPoseOptimize(pcl::visualization::PCLVisualizer &viewer, int *view_port, pcl::PointCloud<pcl::PointXYZ>::Ptr &car_cloud, DetectResult &result) {
    result.car.wheel_width = result.wheel[DeviceAzimuth::RR].center.x - result.wheel[DeviceAzimuth::LR].center.x;
    result.car.theta = (result.wheel[DeviceAzimuth::LR].theta + result.wheel[DeviceAzimuth::RR].theta) * 0.5;
    result.car.wheels_center_x = (result.wheel[DeviceAzimuth::LF].center.x + result.wheel[DeviceAzimuth::RF].center.x + result.wheel[DeviceAzimuth::LR].center.x  + result.wheel[DeviceAzimuth::LR].center.x) * 0.25;
    result.car.wheels_center_y = (result.wheel[DeviceAzimuth::LF].center.y + result.wheel[DeviceAzimuth::RF].center.y + result.wheel[DeviceAzimuth::LR].center.y  + result.wheel[DeviceAzimuth::LR].center.y) * 0.25;
    result.car.front_wheels_theta = (result.wheel[DeviceAzimuth::LF].theta + result.wheel[DeviceAzimuth::RF].theta) * 0.5 - result.car.theta;
    result.car.wheel_base = result.wheel[DeviceAzimuth::LF].center.y - result.wheel[DeviceAzimuth::LR].center.y;
    LOG(INFO) << result.car.info();

    return Error_manager();
}

Error_manager DeviceTof3D::grpcVzenseMat(JetStream::ResFrame &frame, const DeviceAzimuth &azimuth,cv::Mat &mat) {
    switch (azimuth) {
        case LF:
            frame.mutable_images()->mutable_img1()->CopyFrom(StreamRpcServer::cvMat2JetImage(mat));
            break;
        case RF:
            frame.mutable_images()->mutable_img2()->CopyFrom(StreamRpcServer::cvMat2JetImage(mat));
            break;
        case LR:
            frame.mutable_images()->mutable_img3()->CopyFrom(StreamRpcServer::cvMat2JetImage(mat));
            break;
        case RR:
            frame.mutable_images()->mutable_img4()->CopyFrom(StreamRpcServer::cvMat2JetImage(mat));
            break;
        default:
            break;
    }
    return {};
}

Error_manager DeviceTof3D::grpcVzensePointMat(JetStream::ResFrame &frame, const DeviceAzimuth &azimuth,cv::Mat &mat) {
    switch (azimuth) {
        case LF:
            frame.mutable_clouds()->mutable_cloud1()->CopyFrom(StreamRpcServer::cvPointMat2JetPointCloud(mat));
//            LOG(INFO) << "frame.clouds().cloud1().size(): " << frame.clouds().cloud1().size();
            break;
        case RF:
            frame.mutable_clouds()->mutable_cloud2()->CopyFrom(StreamRpcServer::cvPointMat2JetPointCloud(mat));
//            LOG(INFO) << "frame.clouds().cloud2().size(): " << frame.clouds().cloud2().size();
            break;
        case LR:
            frame.mutable_clouds()->mutable_cloud3()->CopyFrom(StreamRpcServer::cvPointMat2JetPointCloud(mat));
//            LOG(INFO) << "frame.clouds().cloud3().size(): " << frame.clouds().cloud3().size();
            break;
        case RR:
            frame.mutable_clouds()->mutable_cloud4()->CopyFrom(StreamRpcServer::cvPointMat2JetPointCloud(mat));
//            LOG(INFO) << "frame.clouds().cloud4().size(): " << frame.clouds().cloud4().size();
            break;
        default:
            break;
    }
    return {};
}

Error_manager DeviceTof3D::grpcCloud(JetStream::ResFrame &frame, const DeviceAzimuth &azimuth, pcl::PointCloud<pcl::PointXYZI>::Ptr &cloud) {
    // 传递识别结果
    switch (azimuth) {
        case LF:
            frame.mutable_clouds()->mutable_cloud1()->CopyFrom(StreamRpcServer::pcl2JetPointCloud(cloud));
            break;
        case RF:
            frame.mutable_clouds()->mutable_cloud2()->CopyFrom(StreamRpcServer::pcl2JetPointCloud(cloud));
            break;
        case LR:
            frame.mutable_clouds()->mutable_cloud3()->CopyFrom(StreamRpcServer::pcl2JetPointCloud(cloud));
            break;
        case RR:
            frame.mutable_clouds()->mutable_cloud4()->CopyFrom(StreamRpcServer::pcl2JetPointCloud(cloud));
            break;
        default:
            break;
    }

    return {};
}