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

#pragma once

#include <thread>
#include <boost/chrono/duration.hpp>
#include <opencv2/opencv.hpp>
#include <Eigen/Core>
#include <Eigen/Geometry>
#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#include <pcl/filters/statistical_outlier_removal.h>
#include <pcl/filters/approximate_voxel_grid.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/visualization/pcl_visualizer.h>

#include "log/log.h"
#include "error_code/error_code.hpp"
#include "VzenseNebula_api.h"
#include "thread/thread_condition.h"
#include "tool/timedlockdata.hpp"
#include "pcl/point3D_tool.h"
#include "proto/vzense.pb.h"
#include "communication/transitData.h"

class DeviceTof3D {
public:
    struct DeviceTof3DSaveInfo {
        cv::Mat irMat;      // 灰度图
        cv::Mat pointMat;   // 点云坐标信息

        DeviceTof3DSaveInfo() {
            irMat = cv::Mat::zeros(480, 640, CV_8UC1);
            pointMat = cv::Mat::zeros(480, 640, CV_32FC4);
        }

        DeviceTof3DSaveInfo& operator=(const DeviceTof3DSaveInfo &p) {
            this->pointMat = p.pointMat.clone();
            this->irMat = p.irMat.clone();
            return *this;
        }
    };

public:
    static DeviceTof3D *iter() {
        static DeviceTof3D *instance = nullptr;
        if (instance == nullptr) {
            instance = new DeviceTof3D();
        }
        return instance;
    }

    ~DeviceTof3D() = default;

    Error_manager Init(const google::protobuf::RepeatedPtrField<tof3dVzenseEtc> &tof_devices);

    Error_manager Release();

    Error_manager updateTrans(const DeviceAzimuth &azimuth, const CoordinateTransformation3D &trans);

    cv::Mat getDeviceIrMat();

    cv::Mat getDevicePointsMat(const DeviceAzimuth &azimuth);

    DeviceTof3DSaveInfo getDeviceSaveInfo(const DeviceAzimuth &azimuth);

    Error_manager getDeviceStatus();
protected:
//    bool
    Error_manager SearchDevice(const double &time = 10);

    bool isAllDevicesFound(uint32_t deviceCount);

    Error_manager setTof3dParams(VzDeviceHandle handle, const Tof3dVzenseBuiltInParams &params);

    Error_manager getTof3dParams(VzDeviceHandle handle, Tof3dVzenseBuiltInParams &params);

    Error_manager ConnectDevice(const DeviceAzimuth &azimuth);

    Error_manager DisConnectDevice(VzDeviceHandle handle);

    Error_manager getDepthAndIrPicture(VzDeviceHandle handle, VzFrame &depthFrame, VzFrame &irFrame);

    static Error_manager DepthFrame2Mat(VzFrame &frame, cv::Mat &mat);

    static Error_manager IrFrame2Mat(VzFrame &frame, cv::Mat &mat);

    static Error_manager Frame2Mat(VzFrame &frame, cv::Mat &mat);

    static void HotPlugStateCallback(const VzDeviceInfo *pInfo, int status, void *contex);

    void run(const DeviceAzimuth &azimuth);

    void stop();
private:
    struct DeviceTof3DStatusInfo {
        bool connected = false;
        bool found = false;
    };

    struct DeviceTof3DParamsInfo {
        DeviceTof3DStatusInfo status;
        std::thread *t = nullptr;
        VzDeviceHandle handle = nullptr;
        Thread_condition *condit = nullptr;
        const tof3dVzenseEtc *etc = nullptr;
        std::mutex *handle_mutex = nullptr;

        void Release() {
            condit->kill_all();
            t->join();

            delete t;
            t = nullptr;

            delete condit;
            condit = nullptr;

            if (handle != nullptr) {
                delete handle;
                handle = nullptr;
            }

            delete etc;
            etc = nullptr;
        }
    };

private:
    TimedLockData<DeviceTof3DSaveInfo> m_device_mat[DeviceAzimuth::DEVICE_AZIMUTH_MAX];
    std::vector<DeviceTof3DParamsInfo> m_devices_list;
    TimedLockData<int> m_devices_status[DeviceAzimuth::DEVICE_AZIMUTH_MAX];

    std::mutex rotation_mutex;
    std::mutex all_device_mutex;
    Eigen::Matrix3f rotation_matrix3[4];
    Eigen::Vector3f move[4];
};