1、添加本地ip获取功能；

2、grpc服务启动自动监听本地端口；
3、四个图像合并识别；

CMakeLists.txt

@@ -111,7 +111,7 @@ set(SON_PROJECT_DEPEND_LIST ${SON_PROJECT_DEPEND_LIST}
         protobuf::libprotobuf
         gRPC::grpc++_reflection
         ${OpenCV_LIBRARIES}
-        ${PCL_LIBRARIES}
+        libpcl
         ${ALL_LIBS}
 )
 

detect/onnx/wheel-detector.cpp

@@ -30,6 +30,20 @@ bool TensorrtWheelDetector::detect(cv::Mat& img,std::vector<Object>& objs,cv::Ma
         obj.prob = ret.confidence;
         obj.boxMask = img;
         objs.push_back(obj);
+
+        cv::Rect box = ret.box;
+        cv::Scalar color = ret.color;
+
+        // Detection box
+        cv::rectangle(res, box, color, 2);
+
+        // Detection box text
+        std::string classString = ret.className + ' ' + std::to_string(ret.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(res, textBox, color, cv::FILLED);
+        cv::putText(res, classString, cv::Point(box.x + 5, box.y - 10), cv::FONT_HERSHEY_DUPLEX, 1, cv::Scalar(0, 0, 0), 2, 0);
     }
     return true;
 }

vzense/device_tof3d.cpp

@@ -7,6 +7,14 @@ Error_manager DeviceTof3D::Init(const VzEtcMap &tp_tof3d, bool search) {
         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服务
+            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
@@ -18,10 +26,6 @@ Error_manager DeviceTof3D::Init(const VzEtcMap &tp_tof3d, bool search) {
         SearchDevice(5);
     }
 
-    // 启动grpc服务
-    m_grpc_server.Listenning("192.168.2.55", 9876);
-    cv::namedWindow("ret", cv::WINDOW_AUTOSIZE);
-
     ConnectAllEtcDevices();
 
     VZ_SetHotPlugStatusCallback(DeviceTof3D::HotPlugStateCallback, &mp_device_info);
@@ -445,13 +449,38 @@ Error_manager DeviceTof3D::IrFrame2Mat(VzFrame &frame, cv::Mat &mat) {
     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/";
 
+    std::vector<std::string> imagePathList;
+    if (PathCreator::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 (PathCreator::IsFolder(path)) {
+        cv::glob(path + "/*.jpg", imagePathList);
+    }
+
+    CoordinateTransformation transformation;
+    transformation.setTranInfo({{iter->second->etc.trans().x(),
+                                 iter->second->etc.trans().y(),
+                                 iter->second->etc.trans().z()},
+                                iter->second->etc.trans().roll(),
+                                iter->second->etc.trans().pitch(),
+                                iter->second->etc.trans().yaw()});
     while (t_iter->second.condit->is_alive()) {
         t_iter->second.condit->wait();
         if (t_iter->second.condit->is_alive()) {
@@ -462,6 +491,7 @@ void DeviceTof3D::receiveFrameThread(const std::string &ip) {
 
                 // 图像获取
                 if (getDepthFrame(ip, depthFrame) == SUCCESS && Frame2Mat(depthFrame, depthInfo.depthMat) == SUCCESS) {
+                    depthInfo.depthMat = cv::imread(imagePathList[e()%imagePathList.size()]);
                     // 点云转换
                     VzSensorIntrinsicParameters cameraParam = {};
                     VZ_GetSensorIntrinsicParameters(iter->second->handle, VzToFSensor, &cameraParam);
@@ -472,24 +502,28 @@ void DeviceTof3D::receiveFrameThread(const std::string &ip) {
                             VzDepthVector3 depthPoint = {cols, rows, pDepthFrameData[rows * depthInfo.pointMat.cols + cols]};
                             VzVector3f worldV = {};
                             VZ_ConvertDepthToPointCloud(iter->second->handle, &depthPoint, &worldV, 1, &cameraParam);
-                            depthInfo.pointMat.at<cv::Vec4f>(rows, cols)[0] = worldV.x * 0.001;
-                            depthInfo.pointMat.at<cv::Vec4f>(rows, cols)[1] = worldV.y * 0.001;
-                            depthInfo.pointMat.at<cv::Vec4f>(rows, cols)[2] = worldV.z * 0.001;
-//                            if (depthInfo.pointMat.at<cv::Vec4f>(rows, cols)[0] > 1) {
-//                                LOG(INFO) << depthInfo.pointMat.at<cv::Vec4f>(rows, cols)[0];
-//                            }
+                            pcl::PointXYZ trans_point;
+                            transformation.transPoint({worldV.x * 0.001f, worldV.y * 0.001f, worldV.z * 0.001f}, trans_point);
+                            if (trans_point.x > 10 || trans_point.y > 10 || trans_point.z > 10) {
+                                depthInfo.pointMat.at<cv::Vec4f>(rows, cols)[0] = 0;
+                                depthInfo.pointMat.at<cv::Vec4f>(rows, cols)[1] = 0;
+                                depthInfo.pointMat.at<cv::Vec4f>(rows, cols)[2] = 0;
+                            } else {
+                                depthInfo.pointMat.at<cv::Vec4f>(rows, cols)[0] = trans_point.x;
+                                depthInfo.pointMat.at<cv::Vec4f>(rows, cols)[1] = trans_point.y;
+                                depthInfo.pointMat.at<cv::Vec4f>(rows, cols)[2] = trans_point.z;
+                            }
                             depthInfo.pointMat.at<cv::Vec4f>(rows, cols)[3] = 0;
                         }
                     }
-//                    cv::imshow(iter->first, depthInfo.pointMat);
-//                    cv::waitKey(1);
+
                     // 将图片和点云存储起来
                     m_device_mat[iter->second->etc.azimuth()].reset(depthInfo, 1);
                 } 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()});
+                LOG_EVERY_N(WARNING, 100) << ip << " device handle is null.";
             }
 
             if (!ret.get_error_description().empty()) {
@@ -508,76 +542,62 @@ void DeviceTof3D::receiveFrameThread(const std::string &ip) {
 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;
 
-    const std::string path = ETC_PATH PROJECT_NAME "/image/";
-    std::vector<std::string> imagePathList;
-    int image_index = 0;
-    if (PathCreator::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 (PathCreator::IsFolder(path)) {
-        cv::glob(path + "/*.jpg", imagePathList);
-    }
-
+    DetectResult detect_result;
     while (detect_thread.condit->is_alive()) {
-//        detect_thread.condit->wait();
+        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)));
+        detect_result.reset();
         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()) {
-            DetectResult result;
             ::JetStream::ResFrame frame;
-            t_car_cloud->clear();
-            t_wheel_cloud->clear();
+            // 获取最新数据
+            cv::Mat merge_mat = cv::Mat::zeros(480*2, 640*2, CV_8UC3);
             DeviceMat t_device_mat[DeviceAzimuth::MAX];
             for (int device_index = 0; device_index < DeviceAzimuth::MAX; device_index++) {
                 if (!m_device_mat[device_index].timeout()) {
-                    t_device_mat[device_index].empty = false;
+                    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].depthMat.copyTo(
+                            merge_mat(cv::Rect(
+                                    (device_index & 0x1) * t_device_mat[device_index].depthMat.cols,
+                                    ((device_index & 0x2) >> 1) * t_device_mat[device_index].depthMat.rows,
+                                    t_device_mat[device_index].depthMat.cols,
+                                    t_device_mat[device_index].depthMat.rows)));
+
                     grpcVzenseMat(frame, (DeviceAzimuth)device_index, t_device_mat[device_index].depthMat);
                 }
             }
 
-            for (int device_index = 0; device_index < DeviceAzimuth::MAX; device_index++) {
-                if (t_device_mat[device_index].empty) {continue;}
-
-                std::vector<Object> objs;
-//                detector->detect(t_device_mat[device_index].depthMat, objs);
-                image_index = image_index > imagePathList.size() - 2 ? 0 : image_index + 1;
-                t_device_mat[device_index].depthMat = cv::imread(imagePathList[image_index]);
-                detector->detect(t_device_mat[device_index].depthMat, objs);
-                if (objs.size() == 1 && objs[0].prob > 0.5   ) {
-                    LOG(INFO) << device_index << " objs[0].prob " << objs[0].prob;
-                    auto seg_points = detector->getPointsFromObj(objs[0]);
+            cost = std::chrono::steady_clock::now() - t_start_time;
+            LOG(INFO) << "cost time is " << cost.count() * 1000 << " ms";
+
+            // 模型识别
+            std::vector<Object> objs;
+            detector->detect(merge_mat, objs, merge_mat);
+
+            // 提取点云，只保存识别率符合标准的设备数据
+            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;
                     for (auto &pt: seg_points) {
                         t_device_mat[device_index].pointMat.at<cv::Vec4f>(pt)[3] = objs[0].prob;
                     }
-                    LOG(INFO) << device_index << " grpcVzensePointMat";
                     grpcVzensePointMat(frame, (DeviceAzimuth)device_index, t_device_mat[device_index].pointMat);
-                    LOG(INFO) << device_index << " grpcVzensePointMat";
 
+                    // 提取点云数据
                     for (int rows = 0; rows < t_device_mat[device_index].pointMat.rows; rows++) {
                         for (int cols = 0; cols < t_device_mat[device_index].pointMat.cols; cols++) {
                             t_car_cloud->emplace_back(
@@ -594,19 +614,59 @@ void DeviceTof3D::detectThread() {
                     }
                 }
             }
-
-            viewer.removeAllPointClouds(view_port[0]);
-            viewer.removeAllPointClouds(view_port[1]);
-            viewer.addPointCloud(t_car_cloud, "t_car_cloud", view_port[0]);
-            viewer.addPointCloud(t_wheel_cloud, "t_wheel_cloud", view_port[1]);
-            viewer.spinOnce();
-//            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);
+//            // 模型识别，并分离点云
+//            for (int device_index = 0; device_index < DeviceAzimuth::MAX; device_index++) {
+//                if (!detect_result.wheel[device_index].haveCloud) {continue;}
+//
+//                detector->detect(merge_mat, objs);
+//                if (objs.size() == 1 && objs[0].prob > 0.1   ) {    // TODO：识别置信度加入配置文件
+//                    detect_result.wheel[device_index].detectOk = true;
+//                    auto seg_points = detector->getPointsFromObj(objs[0]);
+//                    for (auto &pt: seg_points) {
+//                        t_device_mat[device_index].pointMat.at<cv::Vec4f>(pt)[3] = objs[0].prob;
+//                    }
+//                    grpcVzensePointMat(frame, (DeviceAzimuth)device_index, t_device_mat[device_index].pointMat);
+//                }
+//
+//                cost = std::chrono::steady_clock::now() - t_start_time;
+//                LOG(INFO) << "cost time is " << cost.count() * 1000 << " ms";
+//                // 提取点云数据
+//                for (int rows = 0; rows < t_device_mat[device_index].pointMat.rows; rows++) {
+//                    for (int cols = 0; cols < t_device_mat[device_index].pointMat.cols; cols++) {
+//                        t_car_cloud->emplace_back(
+//                                t_device_mat[device_index].pointMat.at<cv::Vec4f>(rows, cols)[0],
+//                                t_device_mat[device_index].pointMat.at<cv::Vec4f>(rows, cols)[1],
+//                                t_device_mat[device_index].pointMat.at<cv::Vec4f>(rows, cols)[2]);
+//                        if (t_device_mat[device_index].pointMat.at<cv::Vec4f>(rows, cols)[3] > 0) {
+//                            t_wheel_cloud->emplace_back(
+//                                    t_device_mat[device_index].pointMat.at<cv::Vec4f>(rows, cols)[0],
+//                                    t_device_mat[device_index].pointMat.at<cv::Vec4f>(rows, cols)[1],
+//                                    t_device_mat[device_index].pointMat.at<cv::Vec4f>(rows, cols)[2]);
+//                        }
+//                    }
+//                }
+//                cost = std::chrono::steady_clock::now() - t_start_time;
+//                LOG(INFO) << "cost time is " << cost.count() * 1000 << " ms";
 //            }
+//            cv::imshow("mergeImage", merge_mat);
+//            cv::waitKey(0);
+
+            cost = std::chrono::steady_clock::now() - t_start_time;
+            LOG(INFO) << "cost time is " << cost.count() * 1000 << " ms";
+            // 当没有车辆点云的时候，说明规定区域内没有车辆，将重置测量记录
+            if (t_car_cloud->empty()) {
+                m_detect_result_record.reset();
+            }
+            // 点云处理
+            if (!t_car_cloud->empty() && !t_wheel_cloud->empty()) {
+                CarPoseOptimize(t_car_cloud, detect_result);
+                WheelCloudOptimize(t_wheel_cloud, detect_result);
+            }
+
 
+            // 算法结果处理
+
+            // grpc数据传出
             frame.mutable_measure_info()->set_x(1);
             frame.mutable_measure_info()->set_y(1);
             frame.mutable_measure_info()->set_width(1);
@@ -915,11 +975,22 @@ Error_manager DeviceTof3D::segFrame2CarAndWheel(VzFrame &depth_frame, std::vecto
     return Error_manager();
 }
 
-Error_manager DeviceTof3D::WheelCloudOptimize(pcl::PointCloud<pcl::PointXYZ>::Ptr &wheel_cloud) {
+Error_manager DeviceTof3D::WheelCloudOptimize(pcl::PointCloud<pcl::PointXYZ>::Ptr &wheel_cloud, DetectResult &result) {
     return Error_manager();
 }
 
-Error_manager DeviceTof3D::CarPoseOptimize(pcl::PointCloud<pcl::PointXYZ>::Ptr &car_cloud) {
+Error_manager DeviceTof3D::CarPoseOptimize(pcl::PointCloud<pcl::PointXYZ>::Ptr &car_cloud, DetectResult &result) {
+    // 检测模型识别结果, 如果没有结、只有一个车轮、同侧车轮的结果, 返回无法策略
+    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 {};
+    }
+
+
     return Error_manager();
 }
 

vzense/device_tof3d.h

@@ -12,7 +12,9 @@
 #include "thread/thread_condition.h"
 #include "stream/streamServer.h"
 #include "tool/timedlockdata.hpp"
-
+#include "tool/ip.hpp"
+#include "pcl/trans_coor.hpp"
+#include "pcl/point3D_tool.h"
 #include "tof3d_config.pb.h"
 
 #ifdef ENABLE_TENSORRT_DETECT
@@ -26,8 +28,57 @@
 
 class DeviceTof3D {
 public:
-    struct DetectResult {
+    struct WheelDetectResult {
+        bool haveCloud = false;
+        bool segmentOk = false;
+        bool detectOk = false;
+        pcl::PointXYZ center;
+        float confidence = 0;
+
+        void reset() {
+            haveCloud = false;
+            segmentOk = false;
+            detectOk = false;
+            center.x = 0;
+            center.y = 0;
+            center.z = 0;
+            confidence = 0;
+        }
+    };
+    struct CarDetectResult {
+        float wheels_center_x;
+        float wheels_center_y;
+        float width;
+        float wheel_width;
+        float length;
+        float wheel_length;
+        float front_wheels_theta;
+        float theta;
+        float wheel_base;
+
+        void reset() {
+            wheels_center_x = 0;
+            wheels_center_y = 0;
+            width = 0;
+            wheel_width = 0;
+            length = 0;
+            wheel_length = 0;
+            front_wheels_theta = 0;
+            theta = 0;
+            wheel_base = 0;
+        }
+    };
 
+    struct DetectResult {
+        WheelDetectResult wheel[DeviceAzimuth::MAX];
+        CarDetectResult car;
+
+        void reset() {
+            for (int i = 0; i < DeviceAzimuth::MAX; i++) {
+                wheel[i].reset();
+            }
+            car.reset();
+        }
     };
 
     struct tof3dVzenseInfo {
@@ -119,10 +170,10 @@ protected:
                                        pcl::PointCloud<pcl::PointXYZ>::Ptr &wheel_cloud);
 
     // 车轮点云优化
-    Error_manager WheelCloudOptimize(pcl::PointCloud<pcl::PointXYZ>::Ptr &wheel_cloud);
+    Error_manager WheelCloudOptimize(pcl::PointCloud<pcl::PointXYZ>::Ptr &wheel_cloud, DetectResult &result);
 
     // 车身点云优化
-    Error_manager CarPoseOptimize(pcl::PointCloud<pcl::PointXYZ>::Ptr &car_cloud);
+    Error_manager CarPoseOptimize(pcl::PointCloud<pcl::PointXYZ>::Ptr &car_cloud, DetectResult &result);
 
     Error_manager grpcVzenseMat(JetStream::ResFrame &frame, const DeviceAzimuth &azimuth, cv::Mat &mat);
 
@@ -144,12 +195,10 @@ private:
     };
 
     struct DeviceMat {
-        bool empty;
         cv::Mat depthMat;
         cv::Mat pointMat;
 
         DeviceMat() {
-            empty = true;
             depthMat.create(480, 640, CV_16UC1);
             pointMat.create(480, 640, CV_32FC4);
         }
@@ -170,6 +219,8 @@ private:
     ThreadInfo detect_thread;
     TensorrtWheelDetector *detector;
     StreamRpcServer m_grpc_server;
+
+    DetectResult m_detect_result_record;
 };
 
 

vzense/stream/streamServer.cpp

@@ -177,9 +177,7 @@ void StreamRpcServer::Listenning(std::string ip,int port){
 
 void StreamRpcServer::WaitThread(void* p){
     StreamRpcServer* server=(StreamRpcServer*)p;
-    printf(" listenning ........\n");
     server->server_->Wait();
-    printf(" server closesd.....\n");
 }
 
 void StreamRpcServer::ResetData(const ::JetStream::ResFrame& frame){

vzense/tof3d_config.proto

@@ -45,6 +45,15 @@ message MqttCommunicationParams {
   required string mqtt_topic = 8;
 }
 
+message CoordinateTransformation3D {
+  optional float x = 1 [default = 0];
+  optional float y = 2 [default = 0];
+  optional float z = 3 [default = 0];
+  optional float roll = 4 [default = -90];
+  optional float pitch = 5 [default = 0];
+  optional float yaw = 6 [default = 0];
+}
+
 message tof3dVzenseEtc {
   required bool enable_device = 1;  // 设备启动才进行以下处理
   required string ip = 2 [default = "192.168.1.101"];
@@ -52,6 +61,7 @@ message tof3dVzenseEtc {
   required Tof3dVzenseBuiltInParams bip = 4;
   required Yolov8ProcessParams yolo = 5;
   required DeviceAzimuth azimuth = 6;
+  optional CoordinateTransformation3D trans= 7;
 //  optional RabbitmqCommunicationParams rabbitmq_param = 6;
 //  optional MqttCommunicationParams mqtt_param = 7;
 }