Vzense-Tof-3D/vzense/device_tof3d.cpp

#include "device_tof3d.h"

Error_manager DeviceTof3D::Init(const VzEtcMap &tp_tof3d, bool search) {
    LOG(INFO) << "Init DeviceTof3D.";
    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};
    }

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

    cv::namedWindow("ret", cv::WINDOW_AUTOSIZE);

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

    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, iter->second->etc.bip().exposure_time(), &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, 100, &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, 100, &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, 100, &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 <sys/stat.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);

    pcl::PointCloud<pcl::PointXYZ>::Ptr t_car_cloud(new pcl::PointCloud<pcl::PointXYZ>);
    pcl::PointCloud<pcl::PointXYZ>::Ptr t_wheel_cloud(new pcl::PointCloud<pcl::PointXYZ>);
    std::vector<Object> segment_results;

    const std::string path = ETC_PATH PROJECT_NAME "/image/";
    std::vector<std::string> imagePathList;
    int image_index = 0;
   if (IsFile(path)) {
        std::string suffix = path.substr(path.find_last_of('.') + 1);
        if (suffix == "jpg" || suffix == "jpeg" || suffix == "png") {
            imagePathList.push_back(path);
        } else {
            printf("suffix %s is wrong !!!\n", suffix.c_str());
            std::abort();
        }
    } else if (IsFolder(path)) {
        cv::glob(path + "/*.jpg", imagePathList);
    }

    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) {
                t_car_cloud->clear();
                t_wheel_cloud->clear();
                VzFrame depthFrame = {0};
                cv::Mat depthMat(480, 640, CV_16UC1);
                if (getDepthFrame(ip, depthFrame) == SUCCESS) {
                    segment_results.clear();
                    if (Frame2Mat(depthFrame, depthMat) == SUCCESS) {
                        image_index = (image_index + 1) > (imagePathList.size() - 1) ? 0 : image_index + 1;
                        depthMat = cv::imread(imagePathList[image_index]);
                        detector->detect(depthMat, segment_results, depthMat);
                        cv::imshow("ret", depthMat);
                        cv::waitKey(1);
                    }

                    std::vector<cv::Point> mat_seg_ret;
                    for (auto &result: segment_results) {
                        LOG(INFO) << result.prob;
                        if (result.prob > 0.5) {
                            mat_seg_ret = detector->getPointsFromObj(result);
                            for (auto &pt: mat_seg_ret) {
                                depthMat.at<cv::Vec3b>(pt) = cv::Vec3b(255, 255, 255) - depthMat.at<cv::Vec3b>(pt);
                            }
                            cv::imshow("ret", depthMat);
                            cv::waitKey(1);
                        }
                    }

                    if (!mat_seg_ret.empty()) {
                        segFrame2CarAndWheel(depthFrame, mat_seg_ret, t_car_cloud, t_wheel_cloud);
                        //                        viewer.removeAllPointClouds(view_port[0]);
                        //                        viewer.removeAllPointClouds(view_port[1]);
                        //                        viewer.addPointCloud(t_car_cloud, "car_cloud_", view_port[0]);
                        //                        viewer.addPointCloud(t_wheel_cloud, "wheel_cloud_", view_port[1]);
                        //                        viewer.spinOnce();
                    }
                } else {
                    ret.compare_and_merge_up({FAILED, MINOR_ERROR, "Device %s VZ_GetFrame failed.", ip.c_str()});
                }
            } else {
                ret.compare_and_merge_up({FAILED, MINOR_ERROR, "Device %s handle is null.", ip.c_str()});
            }

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

#include <pcl/visualization/pcl_visualizer.h>

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

    // int view_port[2];
    // pcl::visualization::PCLVisualizer viewer("viewer_0");
    // viewer.createViewPort(0.0, 0.0, 0.5, 1, view_port[0]);
    // viewer.createViewPort(0.5, 0.0, 1, 1, view_port[1]);
    // viewer.addText("car_cloud", 10, 10, 20, 0, 1, 0, "car_cloud", view_port[0]);
    // viewer.addText("wheel_cloud", 10, 10, 20, 0, 1, 0, "wheel_cloud", view_port[1]);
    // viewer.addCoordinateSystem(1, "car_axis", view_port[0]);
    // viewer.addCoordinateSystem(1, "wheel_axis", view_port[1]);

    pcl::PointCloud<pcl::PointXYZ>::Ptr t_car_cloud(new pcl::PointCloud<pcl::PointXYZ>);
    pcl::PointCloud<pcl::PointXYZ>::Ptr t_wheel_cloud(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 (detect_thread.condit->is_alive()) {
//        detect_thread.condit->wait();
        t_car_cloud->clear();
        t_wheel_cloud->clear();
        cost = std::chrono::steady_clock::now() - t_start_time;
        std::this_thread::sleep_for(std::chrono::milliseconds((int) MAX(100 - cost.count() * 1000, 1)));
        t_start_time = std::chrono::steady_clock::now();
//        LOG(INFO) << "last wheel cost time is " << cost.count() * 1000 << " ms";

        if (detect_thread.condit->is_alive()) {


//            cost = std::chrono::steady_clock::now() - t_start_time;
//            LOG(INFO) << "cost time is " << cost.count() * 1000 << " ms";
            if (!t_car_cloud->empty() && !t_wheel_cloud->empty()) {
                CarPoseOptimize(t_car_cloud);
                WheelCloudOptimize(t_wheel_cloud);
            }

        }
    }
    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(pcl::PointCloud<pcl::PointXYZ>::Ptr &wheel_cloud) {
    return Error_manager();
}

Error_manager DeviceTof3D::CarPoseOptimize(pcl::PointCloud<pcl::PointXYZ>::Ptr &car_cloud) {
    return Error_manager();
}