1、点云处理

CMakeLists.txt

@@ -56,11 +56,8 @@ else ()
     aux_source_directory(${CMAKE_CURRENT_LIST_DIR}/detect/onnx detect)
 endif ()
 
-#find_package(absl REQUIRED)
 find_package(gRPC CONFIG REQUIRED)
-#find_package(PkgConfig REQUIRED)
-#pkg_search_module(GRPC REQUIRED grpc)
-#pkg_search_module(GRPCPP REQUIRED grpc++)
+find_package(Ceres REQUIRED)
 
 option(OPTION_COMMUNICATION_WITH_PLC "plc通信" OFF)
 message("<=${SON_PROJECT_NAME}=> OPTION_COMMUNICATION_WITH_PLC: " ${OPTION_COMMUNICATION_WITH_PLC})
@@ -85,18 +82,21 @@ include_directories(
         ${PCL_INCLUDE_DIRS}
         ${OpenCV_INCLUDE_DIRS}
         ${PROTOBUF_INCLUDE_DIR}
+        ${CERES_INCLUDE_DIR}
         ${NebulaSDK_PATH}/include
 )
 
 aux_source_directory(${CMAKE_CURRENT_LIST_DIR}/communication communication)
 aux_source_directory(${CMAKE_CURRENT_LIST_DIR}/vzense vzense)
 aux_source_directory(${CMAKE_CURRENT_LIST_DIR}/vzense/stream stream)
+aux_source_directory(${CMAKE_CURRENT_LIST_DIR}/ceres_detect ceres_detect)
 
 set(SON_PROJECT_SOURCE_LIST ${SON_PROJECT_SOURCE_LIST}
         ${detect}
         ${communication}
         ${vzense}
         ${stream}
+        ${ceres_detect}
         ${CMAKE_CURRENT_LIST_DIR}/tof3DMain.cpp
 )
 
@@ -112,6 +112,7 @@ set(SON_PROJECT_DEPEND_LIST ${SON_PROJECT_DEPEND_LIST}
         gRPC::grpc++_reflection
         ${OpenCV_LIBRARIES}
         libpcl
+        ${CERES_LIBRARIES}
         ${ALL_LIBS}
 )
 

ceres_detect/wheel_detector.cpp

@@ -0,0 +1,82 @@
+#include "wheel_detector.h"
+#include <ceres/ceres.h>
+#include <ceres/cubic_interpolation.h>
+
+struct WheelCostFunction {
+    std::vector<double> line_;
+    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_;
+
+    explicit WheelCostFunction(std::vector<double> &line, pcl::PointCloud<pcl::PointXYZ>::Ptr cloud) {
+        line_ = line;
+        cloud_ = cloud;
+    }
+
+    template<typename JetT>
+    bool operator()(const JetT *const x, JetT *residual) const {
+        ceres::Grid1D<double> grid(line_.data(), 0, line_.size());
+        ceres::CubicInterpolator<ceres::Grid1D<double>> cubic(grid);
+
+        JetT delta_x = x[0];
+        JetT theta = x[1];
+        auto cos_t = ceres::cos(theta);
+        auto sin_t = ceres::sin(theta);
+        for (int i = 0; i < cloud_->size(); ++i) {
+            JetT cx = JetT(cloud_->points[i].x) * cos_t - JetT(cloud_->points[i].y) * sin_t - delta_x + 1.3;
+            cubic.Evaluate(cx * 100.,&residual[i]);
+            residual[i]=JetT(1.0)-residual[i];
+        }
+        return true;
+    }
+};
+
+singleWheelDetector::singleWheelDetector() {
+    float width = 0.1;
+    float sigma=100.;
+    float solution = 0.01;
+    for (int i=-130;i<130;++i) {
+        float x = i * solution;
+        double y = 1.0 / (1.0 + ceres::exp(-sigma * (x+width/2.))) -
+                   1.0 / (1.0 + ceres::exp(-sigma * (x-width/2.)));
+        line_.emplace_back(y);
+//            printf("---Debug %s %d : [%d %f %f]. \n", __func__, __LINE__, i, x, y);
+    }
+}
+
+
+bool singleWheelDetector::detect(pcl::PointCloud<pcl::PointXYZ>::Ptr in_cloud_ptr,
+                                 pcl::PointCloud<pcl::PointXYZ>::Ptr out_cloud_ptr,
+                                 singleWheelDetector::WheelDetectResult &result) {
+    double t_vars[2] = {in_cloud_ptr->points[0].x + 0.1, 0};
+    // 构建最小二乘问题
+    ceres::Problem problem;
+
+    problem.AddResidualBlock(new ceres::AutoDiffCostFunction<WheelCostFunction, ceres::DYNAMIC, 2>(
+                    (new WheelCostFunction(line_, in_cloud_ptr)), in_cloud_ptr->size()),
+            nullptr, // 核函数，这里不使用，为空
+            t_vars   // 待估计参数
+    );
+
+//    problem.SetParameterLowerBound(t_vars, 1, (-20) * M_PI / 180.0f);
+//    problem.SetParameterUpperBound(t_vars, 1, (20) * M_PI / 180.0f);
+
+    // 配置求解器
+    ceres::Solver::Options options;                             // 这里有很多配置项可以填
+    options.linear_solver_type = ceres::DENSE_QR;               // 增量方程如何求解
+    options.minimizer_progress_to_stdout = false;               // 输出到cout
+    options.max_num_iterations = 100;
+
+    ceres::Solver::Summary summary;                             // 优化信息
+    ceres::Solve(options, &problem, &summary);                  // 开始优化
+    result.theta = t_vars[1] * 180.0f / M_PI;
+
+    LOG(INFO) << result.info();
+    float sin_t = std::sin(t_vars[1]);
+    float cos_t = std::cos(t_vars[1]);
+    for (auto &point: in_cloud_ptr->points) {
+        out_cloud_ptr->emplace_back(point.x * cos_t - point.y * sin_t - t_vars[0],
+                                    point.x * sin_t + point.y * cos_t,
+                                    point.z);
+    }
+
+    return false;
+}

ceres_detect/wheel_detector.h

@@ -0,0 +1,56 @@
+#pragma once
+
+#include "thread/singleton.h"
+#include <pcl/point_cloud.h>
+#include <pcl/point_types.h>
+#include <pcl/filters/passthrough.h>
+#include <pcl/filters/statistical_outlier_removal.h>
+
+class singleWheelDetector : public Singleton<singleWheelDetector> {
+    friend class Singleton<singleWheelDetector>;
+
+public:
+    struct WheelDetectResult {
+        bool haveCloud = false;
+        bool segmentOk = false;
+        bool detectOk = false;
+        pcl::PointXYZ center;
+        float confidence = 0;
+        float theta = 0;
+
+        void reset() {
+            haveCloud = false;
+            segmentOk = false;
+            detectOk = false;
+            center.x = 0;
+            center.y = 0;
+            center.z = 0;
+            confidence = 0;
+            theta = 0;
+        }
+
+        std::string info() {
+            char str[512];
+            sprintf(str, "x: %.3f, y: %.3f, z: %.3f, theta: %.3f", center.x, center.y, center.z, theta);
+            return str;
+        }
+    };
+
+    singleWheelDetector &operator=(const singleWheelDetector &) = delete;
+
+    ~singleWheelDetector() override = default;
+
+    // 检测底盘z方向值，去中心，使用mat加速
+    bool detect(pcl::PointCloud<pcl::PointXYZ>::Ptr in_cloud_ptr, pcl::PointCloud<pcl::PointXYZ>::Ptr out_cloud_ptr,
+                WheelDetectResult &result);
+
+protected:
+    // 父类的构造函数必须保护，子类的构造函数必须私有。
+    singleWheelDetector();
+
+private:
+    std::vector<double> line_;
+};
+
+
+

detect/tensorrt/wheel-detector.cpp

@@ -23,10 +23,7 @@ bool TensorrtWheelDetector::detect(cv::Mat& img,std::vector<Object>& objs){
     if(yolov8_==nullptr){
         return false;
     }
-    if(img.size()!=imgsz_){
-        printf("imgsz required [%d,%d],but input is [%d,%d]\n",imgsz_.height,imgsz_.width,img.rows,img.cols);
-        return false;
-    }
+    
     yolov8_->copy_from_Mat(img, imgsz_);
     yolov8_->infer();
     float score_thres=0.9;

vzense/device_tof3d.h

@@ -5,6 +5,10 @@
 #include <opencv2/opencv.hpp>
 #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/common/transforms.h>
 
 #include "error_code/error_code.hpp"
 #include "VzenseNebula_api.h"
@@ -16,6 +20,9 @@
 #include "pcl/trans_coor.hpp"
 #include "pcl/point3D_tool.h"
 #include "tof3d_config.pb.h"
+#include <Eigen/Core>
+#include <Eigen/Geometry>
+#include <pcl/visualization/pcl_visualizer.h>
 
 #ifdef ENABLE_TENSORRT_DETECT
 #include "detect/tensorrt/wheel-detector.h"
@@ -24,54 +31,14 @@
 #include "detect/onnx/wheel-detector.h"
 
 #endif
-
+#include "ceres_detect/car_pose_detector.h"
+#include "ceres_detect/wheel_detector.h"
 
 class DeviceTof3D {
 public:
-    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;
+        singleWheelDetector::WheelDetectResult wheel[DeviceAzimuth::MAX];
+        Car_pose_detector::CarDetectResult car;
 
         void reset() {
             for (int i = 0; i < DeviceAzimuth::MAX; i++) {
@@ -79,6 +46,11 @@ public:
             }
             car.reset();
         }
+
+        std::string info() {
+            std::string str = car.info();
+            return str;
+        }
     };
 
     struct tof3dVzenseInfo {
@@ -102,9 +74,6 @@ public:
 
     ~DeviceTof3D() = default;
 
-    // TODO
-    void run();
-
     Error_manager Init(const VzEtcMap &tp_tof3d, bool search = true);
 
     Error_manager reInit(const VzEtcMap &etc);
@@ -170,10 +139,10 @@ protected:
                                        pcl::PointCloud<pcl::PointXYZ>::Ptr &wheel_cloud);
 
     // 车轮点云优化
-    Error_manager WheelCloudOptimize(pcl::PointCloud<pcl::PointXYZ>::Ptr &wheel_cloud, DetectResult &result);
+    Error_manager WheelCloudOptimize(std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> &vct_wheel_cloud, DetectResult &result);
 
     // 车身点云优化
-    Error_manager CarPoseOptimize(pcl::PointCloud<pcl::PointXYZ>::Ptr &car_cloud, DetectResult &result);
+    Error_manager CarPoseOptimize(pcl::visualization::PCLVisualizer &viewer, int *view_port, pcl::PointCloud<pcl::PointXYZ>::Ptr &car_cloud, DetectResult &result);
 
     Error_manager grpcVzenseMat(JetStream::ResFrame &frame, const DeviceAzimuth &azimuth, cv::Mat &mat);
 
@@ -196,16 +165,19 @@ private:
 
     struct DeviceMat {
         cv::Mat depthMat;
+        cv::Mat irMat;
         cv::Mat pointMat;
 
         DeviceMat() {
-            depthMat.create(480, 640, CV_16UC1);
-            pointMat.create(480, 640, CV_32FC4);
+            irMat = cv::Mat::zeros(480, 640, CV_8UC1);
+            depthMat = cv::Mat::zeros(480, 640, CV_16UC1);
+            pointMat = cv::Mat::zeros(480, 640, CV_32FC4);
         }
 
         DeviceMat& operator=(const DeviceMat &p) {
             this->depthMat = p.depthMat.clone();
             this->pointMat = p.pointMat.clone();
+            this->irMat = p.irMat.clone();
             return *this;
         }
     };

vzense/stream/streamServer.cpp

@@ -42,7 +42,7 @@ bool CustomServer::waitForUpdata(float tm){
     openDataStream_=true;
     printf("打开测量数据流\n");
     while(openDataStream_){
-        if(waitForUpdata(0.3)){
+        if(waitForUpdata()){
             writer->Write(frame_.measure_info());
         }else{
             ::JetStream::MeasureInfo info;
@@ -101,7 +101,7 @@ bool CustomServer::waitForUpdata(float tm){
 
 ::grpc::Status CustomServer::Detect(::grpc::ServerContext* context, 
             const ::JetStream::RequestCmd* request, ::JetStream::ResFrame* response){
-    if(waitForUpdata(0.3)){
+    if(waitForUpdata()){
         response->CopyFrom(frame_);
         return ::grpc::Status::OK;
     }else{
@@ -112,7 +112,7 @@ bool CustomServer::waitForUpdata(float tm){
 
 ::grpc::Status CustomServer::GetCloud(::grpc::ServerContext* context, const ::JetStream::RequestCmd* request, 
             ::JetStream::ResCloud* response){
-    if(waitForUpdata(0.3)==false){
+    if(waitForUpdata()==false){
         return ::grpc::Status(::grpc::StatusCode::DATA_LOSS,"0.3s内没有等到最新数据");
     }
     int id=request->id();
@@ -126,14 +126,13 @@ bool CustomServer::waitForUpdata(float tm){
         response->CopyFrom(frame_.clouds());
         break;
     }
-    printf("frame_.clouds().cloud2().size() %d\n", frame_.clouds().cloud2().size());
     return ::grpc::Status::OK;
 
 }
 
 ::grpc::Status CustomServer::GetImage(::grpc::ServerContext* context, const ::JetStream::RequestCmd* request, 
             ::JetStream::ResImage* response){
-    if(waitForUpdata(0.3)==false){
+    if(waitForUpdata()==false){
         return ::grpc::Status(::grpc::StatusCode::DATA_LOSS,"0.3s内没有等到最新数据");
     }
     int id=request->id();

vzense/stream/streamServer.h

@@ -36,7 +36,7 @@ public:
             ::JetStream::ResImage* response);
 
 protected:
-    bool waitForUpdata(float tm);
+    bool waitForUpdata(float tm = 1);
 };
 
 

vzense/tof3d_config.proto

@@ -10,7 +10,7 @@ enum DeviceAzimuth {
 
 message Tof3dVzenseBuiltInParams {
   optional uint32 work_mode = 1 [default = 0x00];
-  optional uint32 irgmmgain = 2 [default = 45];
+  optional uint32 irgmmgain = 2 [default = 255];
   optional uint32 frame_rate = 3 [default = 10];
   optional bool enable_manual_exposure_time = 4 [default = true];
   optional uint32 exposure_time = 5 [default = 2000];
@@ -21,7 +21,7 @@ message Tof3dVzenseBuiltInParams {
   optional bool enable_flying_pixel_filter = 11 [default = true];
   optional uint32 flying_pixel_filter_value = 12 [default = 15];
   optional bool enable_confidence_filter = 13 [default = true];
-  optional uint32 confidence_filter_value = 14 [default = 0];
+  optional uint32 confidence_filter_value = 14 [default = 6];
   optional bool enable_hdr_mode = 15 [default = true];
 }
 
@@ -49,7 +49,7 @@ 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 roll = 4 [default = 0];
   optional float pitch = 5 [default = 0];
   optional float yaw = 6 [default = 0];
 }
@@ -57,11 +57,10 @@ message CoordinateTransformation3D {
 message tof3dVzenseEtc {
   required bool enable_device = 1;  // 设备启动才进行以下处理
   required string ip = 2 [default = "192.168.1.101"];
-//  optional string url = 3 [default = ""];
-  required Tof3dVzenseBuiltInParams bip = 4;
-  required Yolov8ProcessParams yolo = 5;
-  required DeviceAzimuth azimuth = 6;
-  optional CoordinateTransformation3D trans= 7;
+  required Tof3dVzenseBuiltInParams bip = 3;
+  required DeviceAzimuth azimuth = 4;
+  optional CoordinateTransformation3D trans= 5;
+  required Yolov8ProcessParams yolo = 6;
 //  optional RabbitmqCommunicationParams rabbitmq_param = 6;
 //  optional MqttCommunicationParams mqtt_param = 7;
 }