1、SDK调用改用单个锁；

detect/detect_manager.cpp

@@ -15,7 +15,14 @@ Error_manager DetectManager::Init() {
     m_thread = new std::thread(&DetectManager::run, this);
     m_condit.notify_all(true);
 
-    return Error_manager();
+    return {};
+}
+
+Error_manager DetectManager::Release() {
+    stop();
+    delete detector;
+    detector = nullptr;
+    return {};
 }
 
 void DetectManager::run() {
@@ -42,6 +49,7 @@ void DetectManager::run() {
             cv::Mat merge_mat = cv::Mat::zeros(480 * 2, 640 * 2, CV_8UC1);
             DeviceTof3D::DeviceTof3DSaveInfo t_device_mat[DeviceAzimuth::DEVICE_AZIMUTH_MAX];
             for (int device_index = 0; device_index < DeviceAzimuth::DEVICE_AZIMUTH_MAX; device_index++) {
+                vct_wheel_cloud[device_index]->clear();
                 t_device_mat[device_index] = DeviceTof3D::iter()->getDeviceSaveInfo((DeviceAzimuth)device_index);
                 t_device_mat[device_index].irMat.copyTo(merge_mat(cv::Rect((device_index & 0x1) * 640,
                                                                            ((device_index & 0x2) >> 1) * 480, 640, 480)));
@@ -79,7 +87,6 @@ void DetectManager::run() {
 
                 if (obj.prob > 0.9) {
                     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);
@@ -180,6 +187,15 @@ void DetectManager::run() {
     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) {
     //下采样

detect/detect_manager.h

@@ -54,12 +54,16 @@ public:
 
     Error_manager Init();
 
+    Error_manager Release();
+
     MeasureStatu getMeasureResult(DetectResult &result);
 
     Error_manager carMoveStatus();
 protected:
     void run();
 
+    void stop();
+
     // 车轮点云优化
     Error_manager WheelCloudOptimize(std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> &vct_wheel_cloud, DetectResult &result);
 

vzense/device_tof3d.cpp

@@ -3,7 +3,8 @@
 Error_manager DeviceTof3D::Init(const google::protobuf::RepeatedPtrField<tof3dVzenseEtc> &tof_devices) {
     LOG(INFO) << "---------- Init DeviceTof3D ----------";
     if (tof_devices.size() < DeviceAzimuth::DEVICE_AZIMUTH_MAX) {
-        return {TOF3D_VZENSE_DEVICE_INIT_FAILED, MAJOR_ERROR, "device numbers is not enough: %d < %d", tof_devices.size(), DeviceAzimuth::DEVICE_AZIMUTH_MAX};
+        return {TOF3D_VZENSE_DEVICE_INIT_FAILED, MAJOR_ERROR, "device numbers is not enough: %d < %d",
+                tof_devices.size(), DeviceAzimuth::DEVICE_AZIMUTH_MAX};
     }
 
     // 使能tof3d相机SDK
@@ -14,7 +15,7 @@ Error_manager DeviceTof3D::Init(const google::protobuf::RepeatedPtrField<tof3dVz
 
     // 注册相机列表
     m_devices_list.resize(DeviceAzimuth::DEVICE_AZIMUTH_MAX);
-    for (auto &device : tof_devices) {
+    for (auto &device: tof_devices) {
         // 校验方位
         if (device.azimuth() >= DeviceAzimuth::DEVICE_AZIMUTH_MAX) {
             return {ERROR, NORMAL, "azimuth should smaller than %d.", DeviceAzimuth::DEVICE_AZIMUTH_MAX};
@@ -39,6 +40,7 @@ Error_manager DeviceTof3D::Init(const google::protobuf::RepeatedPtrField<tof3dVz
             ConnectDevice(device.etc->azimuth());
             m_devices_status[device.etc->azimuth()] = 0;
             setTof3dParams(device.handle, device.etc->bip());
+            device.handle_mutex = &all_device_mutex;
             device.condit = new Thread_condition();
             device.t = new std::thread(&DeviceTof3D::run, this, device.etc->azimuth());
         }
@@ -72,7 +74,7 @@ cv::Mat DeviceTof3D::getDeviceIrMat() {
     cv::Mat merge_mat = cv::Mat::zeros(480 * 2, 640 * 2, CV_8UC1);
     for (int device_index = 0; device_index < DeviceAzimuth::DEVICE_AZIMUTH_MAX; device_index++) {
         if (!m_device_mat[device_index].timeout()) {
-            auto t_device_mat  = m_device_mat[device_index]();
+            auto t_device_mat = m_device_mat[device_index]();
             // 拼接图像
             cv::Mat merge_mat_lf = merge_mat(cv::Rect(0, 0, 640, 480));
             t_device_mat.irMat.copyTo(
@@ -99,6 +101,15 @@ DeviceTof3D::DeviceTof3DSaveInfo DeviceTof3D::getDeviceSaveInfo(const DeviceAzim
     return azimuth < DeviceAzimuth::DEVICE_AZIMUTH_MAX ? m_device_mat[azimuth]() : DeviceTof3DSaveInfo{};
 }
 
+Error_manager DeviceTof3D::getDeviceStatus() {
+    for (auto &statu: m_devices_status) {
+        if (statu() != 0) {
+            return {FAILED, NORMAL};
+        }
+    }
+    return {};
+}
+
 Error_manager DeviceTof3D::SearchDevice(const double &time) {
     uint32_t deviceCount = 0;
 
@@ -132,7 +143,7 @@ bool DeviceTof3D::isAllDevicesFound(uint32_t deviceCount) {
     auto *pDeviceListInfo = new VzDeviceInfo[4];
     VZ_GetDeviceInfoList(deviceCount, pDeviceListInfo);
 
-    for (auto & device : m_devices_list) {
+    for (auto &device: m_devices_list) {
         for (int search_index = 0; search_index < deviceCount; search_index++) {
             device.status.found = device.etc->ip() == pDeviceListInfo[search_index].ip;
             if (device.status.found) {
@@ -141,7 +152,7 @@ bool DeviceTof3D::isAllDevicesFound(uint32_t deviceCount) {
         }
     }
 
-    for (auto & device : m_devices_list) {
+    for (auto &device: m_devices_list) {
         if (device.etc->enable_device() && !device.status.found) {
             return false;
         }
@@ -386,7 +397,7 @@ Error_manager DeviceTof3D::getDepthAndIrPicture(VzDeviceHandle handle, VzFrame &
         if (t_vz_status == VzRetOK && irFrame.pFrameData != nullptr) {
             // LOG(INFO) << ip << " frameIndex :" << irFrame.frameIndex;
         } else {
-            LOG(INFO)<< "VZ_GetFrame VzIrFrame status:" << t_vz_status;
+            LOG(INFO) << "VZ_GetFrame VzIrFrame status:" << t_vz_status;
             return {FAILED, MINOR_ERROR, "Device VZ_GetFrame VzIrFrame failed status: %d.", t_vz_status};
         }
     } else {
@@ -438,7 +449,7 @@ Error_manager DeviceTof3D::IrFrame2Mat(VzFrame &frame, cv::Mat &mat) {
     return {};
 }
 
-  Error_manager DeviceTof3D::Frame2Mat(VzFrame &frame, cv::Mat &mat) {
+Error_manager DeviceTof3D::Frame2Mat(VzFrame &frame, cv::Mat &mat) {
     switch (frame.frameType) {
         case VzDepthFrame:
             return DepthFrame2Mat(frame, mat);
@@ -465,11 +476,15 @@ void DeviceTof3D::HotPlugStateCallback(const VzDeviceInfo *pInfo, int status, vo
         pDeviceTof3D->m_devices_list[device_index].handle_mutex->lock();
         if (pDeviceTof3D->m_devices_list[device_index].etc->ip() == pInfo->ip) {
             if (status == 0) {
-                LOG(WARNING) << pInfo->ip << " VZ_OpenDevice " << VZ_OpenDeviceByUri(pInfo->uri, &pDeviceTof3D->m_devices_list[device_index].handle);
-                LOG(WARNING) << pInfo->ip << " VZ_StartStream " << VZ_StartStream(pDeviceTof3D->m_devices_list[device_index].handle);
+                LOG(WARNING) << pInfo->ip << " VZ_OpenDevice "
+                             << VZ_OpenDeviceByUri(pInfo->uri, &pDeviceTof3D->m_devices_list[device_index].handle);
+                LOG(WARNING) << pInfo->ip << " VZ_StartStream "
+                             << VZ_StartStream(pDeviceTof3D->m_devices_list[device_index].handle);
             } else {
-                LOG(WARNING) << pInfo->ip << " VZ_StopStream " << VZ_StopStream(pDeviceTof3D->m_devices_list[device_index].handle);
-                LOG(WARNING) << pInfo->ip << " VZ_CloseDevice " << VZ_CloseDevice(&pDeviceTof3D->m_devices_list[device_index].handle);
+                LOG(WARNING) << pInfo->ip << " VZ_StopStream "
+                             << VZ_StopStream(pDeviceTof3D->m_devices_list[device_index].handle);
+                LOG(WARNING) << pInfo->ip << " VZ_CloseDevice "
+                             << VZ_CloseDevice(&pDeviceTof3D->m_devices_list[device_index].handle);
             }
             pDeviceTof3D->m_devices_status[device_index] = status;
         }
@@ -482,6 +497,7 @@ void DeviceTof3D::run(const DeviceAzimuth &azimuth) {
 
     cv::Mat t_ir_img;
     cv::Mat t_point_img;
+    VzVector3f *worldV = new VzVector3f[480 * 640];
 
     auto t_start_time = std::chrono::steady_clock::now();
     std::chrono::duration<double> cost = std::chrono::steady_clock::now() - t_start_time;
@@ -496,70 +512,71 @@ void DeviceTof3D::run(const DeviceAzimuth &azimuth) {
 
         if (device.condit->is_alive()) {
             Error_manager ret;
+            DeviceTof3DSaveInfo depthInfo;
+            VzFrame depthFrame = {0};
+            VzFrame irFrame = {0};
+            Error_manager vz_ret = {FAILED, NORMAL};
+
             device.handle_mutex->lock();
             if (device.handle) {
-                DeviceTof3DSaveInfo depthInfo;
-                cv::Mat depthMat = cv::Mat::zeros(480, 640, CV_16UC1);
-                VzFrame depthFrame = {0};
-                VzFrame irFrame = {0};
-
-                // 图像获取
-                if (getDepthAndIrPicture(device.handle, depthFrame, irFrame) == SUCCESS &&
-                    Frame2Mat(depthFrame, depthMat) == SUCCESS &&
-                    Frame2Mat(irFrame, depthInfo.irMat) == SUCCESS) {
-                    // 点云转换
-                    VzSensorIntrinsicParameters cameraParam = {};
-                    VZ_GetSensorIntrinsicParameters(device.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(device.handle, &depthPoint, &worldV, 1, &cameraParam);
-                            if (sqrt(worldV.x * worldV.x + worldV.y * worldV.y + worldV.z * worldV.z) * 0.001f > 5) {
-                                continue;
-                            }
-                            Eigen::Vector3f trans;
-                            trans << worldV.x * 0.001f, worldV.y * 0.001f, worldV.z * 0.001f;
-                            trans =  rotation_matrix3[azimuth] * trans + move[azimuth];
-                            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);
-                        }
-                    }
-                    // 将图片和点云存储起来
-                    t_ir_img = depthInfo.irMat.clone();
-                    t_point_img = depthInfo.pointMat.clone();
-
-                    TransitData::get_instance_pointer()->setImage(t_ir_img, azimuth);
-                    TransitData::get_instance_pointer()->setImageCL(t_point_img, azimuth);
-                    m_device_mat[azimuth] = depthInfo;
+                vz_ret = getDepthAndIrPicture(device.handle, depthFrame, irFrame);
+                if (vz_ret == SUCCESS) {
+                    VZ_ConvertDepthFrameToPointCloudVector(device.handle, &depthFrame, worldV);
                 } else {
-                    ret.compare_and_merge_up({FAILED, MINOR_ERROR, "Device %s VZ_GetFrame failed.", device.etc->ip().c_str()});
+                    ret.compare_and_merge_up(
+                            {FAILED, MINOR_ERROR, "Device %s VZ_GetFrame failed.", device.etc->ip().c_str()});
                 }
-            } else {
-                LOG_EVERY_N(WARNING, 1000) << device.etc->ip() << " device handle is null.";
             }
             device.handle_mutex->unlock();
 
-            if (!ret.get_error_description().empty()) {
-                LOG(WARNING) << ret.get_error_description();
+            if (vz_ret == SUCCESS) {
+                // 灰度图转换
+                Frame2Mat(irFrame, depthInfo.irMat);
+
+                // 点云转换
+                for (int cols = 0; cols < depthInfo.pointMat.cols - 0; cols++) {
+                    for (int rows = 0; rows < depthInfo.pointMat.rows - 0; rows++) {
+                        int i = rows * depthInfo.pointMat.cols + cols;
+                        if (sqrt(worldV[i].x * worldV[i].x + worldV[i].y * worldV[i].y + worldV[i].z * worldV[i].z) *
+                            0.001f > 5) {
+                            continue;
+                        }
+                        Eigen::Vector3f trans;
+                        trans << worldV[i].x * 0.001f, worldV[i].y * 0.001f, worldV[i].z * 0.001f;
+                        trans = rotation_matrix3[azimuth] * trans + move[azimuth];
+                        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);
+                    }
+                }
+
+                // 将图片和点云存储起来
+                t_ir_img = depthInfo.irMat.clone();
+                t_point_img = depthInfo.pointMat.clone();
+
+                TransitData::get_instance_pointer()->setImage(t_ir_img, azimuth);
+                TransitData::get_instance_pointer()->setImageCL(t_point_img, azimuth);
+                m_device_mat[azimuth] = depthInfo;
             }
+        } else {
+            LOG_EVERY_N(WARNING, 1000) << device.etc->ip() << " device handle is null.";
         }
     }
 
     LOG(INFO) << device.etc->ip() << " thread end " << std::this_thread::get_id();
 }
 
-Error_manager DeviceTof3D::getDeviceStatus() {
-    for (auto &statu: m_devices_status) {
-        if (statu() != 0) {
-            return {FAILED, NORMAL};
-        }
+void DeviceTof3D::stop() {
+    for (auto &device: m_devices_list) {
+        DisConnectDevice(device.handle);
+        device.condit->kill_all();
+        device.t->join();
+        delete device.t;
+        delete device.condit;
+        device.t = nullptr;
+        device.t = nullptr;
     }
-    return {};
 }

vzense/device_tof3d.h

@@ -52,6 +52,8 @@ public:
 
     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();
@@ -86,6 +88,8 @@ protected:
     static void HotPlugStateCallback(const VzDeviceInfo *pInfo, int status, void *contex);
 
     void run(const DeviceAzimuth &azimuth);
+
+    void stop();
 private:
     struct DeviceTof3DStatusInfo {
         bool connected = false;
@@ -98,7 +102,26 @@ private:
         VzDeviceHandle handle = nullptr;
         Thread_condition *condit = nullptr;
         const tof3dVzenseEtc *etc = nullptr;
-        std::mutex *handle_mutex = new std::mutex();
+        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:
@@ -107,6 +130,7 @@ private:
     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];
 };