1、将识别模块融合到算法模块，减少延时；

2、通信模块大改；

communication/communication_manager.cpp

@@ -5,7 +5,7 @@
 #include "communication_manager.h"
 
 Error_manager CommunicationManager::Init(const CommunicationManagerConfig &config) {
-
+    m_config = config;
     if (config.grpc_enable()) {
         m_grpc_server = new StreamRpcServer;
         m_grpc_server->Listenning(config.grpc_server_ip(), 9876);
@@ -20,5 +20,258 @@ Error_manager CommunicationManager::Init(const CommunicationManagerConfig &confi
         std::string file_path = ETC_PATH PROJECT_NAME + config.mqtt_config_file();
         Tof3DMqttAsyncClient::iter()->init(file_path);
     }
+
+//    offset.CopyFrom(config.offset());
+//    out_info.CopyFrom(config.outof());
+
+    condition = new Thread_condition();
+    run_t = new std::thread(&CommunicationManager::run, this);
+    condition->notify_all(true);
+
+    return {};
+}
+
+void CommunicationManager::run() {
+    auto t_start_time = std::chrono::steady_clock::now();
+    std::chrono::duration<double> cost = std::chrono::steady_clock::now() - t_start_time;
+    while (condition->is_alive()) {
+        condition->wait();
+        cost = std::chrono::steady_clock::now() - t_start_time;
+        std::this_thread::sleep_for(std::chrono::milliseconds(100 - std::min<int>(99, cost.count() * 1000)));
+        t_start_time = std::chrono::steady_clock::now();
+        if (condition->is_alive()) {
+            if (++heart > 999) {
+                heart = 0;
+            }
+
+            DetectManager::DetectResult detect_measure_info;
+
+            MeasureStatu measure_status = getMeasureData(detect_measure_info);
+
+            detect_measure_info.car.uniform_x = detect_measure_info.car.wheels_center_x * cos(detect_measure_info.car.theta) - detect_measure_info.car.wheels_center_y * sin(detect_measure_info.car.theta);
+            detect_measure_info.car.uniform_y = detect_measure_info.car.wheels_center_x * sin(detect_measure_info.car.theta) + detect_measure_info.car.wheels_center_y * cos(detect_measure_info.car.theta);
+
+            if (m_config.grpc_enable()) {
+                sendGrpcData(measure_status, detect_measure_info);
+            }
+
+            if (m_config.rabbitmq_enable()) {
+                sendRabbitmqData(measure_status, detect_measure_info);
+            }
+        }
+    }
+}
+
+MeasureStatu CommunicationManager::getMeasureData(DetectManager::DetectResult &detect_measure_info) {
+    // 设备状态
+    if (DeviceTof3D::iter()->getDeviceStatus() != SUCCESS) {
+        return MeasureStatu::Lidar_Disconnect;
+    }
+
+    return DetectManager::iter()->getMeasureResult(detect_measure_info);
+}
+
+Error_manager CommunicationManager::sendGrpcData(const MeasureStatu &measure_statu, DetectManager::DetectResult &measure_result) {
+    // 保存grpc数据中间块
+    TransitData::MeasureInfo grpc_measure_info;
+    grpc_measure_info.x = measure_result.car.wheels_center_x;
+    grpc_measure_info.y = measure_result.car.wheels_center_y;
+    grpc_measure_info.transx = measure_result.car.uniform_x;
+    grpc_measure_info.transy = measure_result.car.uniform_y;
+    grpc_measure_info.theta = measure_result.car.theta;
+    grpc_measure_info.width = measure_result.car.wheel_width;
+    grpc_measure_info.wheelbase = measure_result.car.wheel_base;
+    grpc_measure_info.ftheta = measure_result.car.front_wheels_theta;
+
+    addOffset(measure_result);
+    grpc_measure_info.border_plc = outOfRangeDetection(measure_statu, measure_result, true);
+    grpc_measure_info.border_display = outOfRangeDetection(measure_statu, measure_result, false);
+    removeOffset(measure_result);
+
+    grpc_measure_info.error.append("heart: " + std::to_string(heart));
+    TransitData::get_instance_pointer()->setMeasureInfo(grpc_measure_info);
+
+    return Error_manager();
+}
+
+Error_manager CommunicationManager::sendMqttData(const MeasureStatu &measure_statu, DetectManager::DetectResult &measure_result) {
+    return Error_manager();
+}
+
+Error_manager CommunicationManager::sendRabbitmqData(const MeasureStatu &measure_statu, DetectManager::DetectResult &measure_result) {
+    // 发送rabbitmq消息
+    measure_info info;
+    measure_buffer rabbit_measure_info;
+
+    info.set_ground_status(measure_statu);
+    // 如果地面雷达状态已经被修改过，直接返回
+    if (info.ground_status() == MeasureStatu::Measure_OK || info.ground_status() == MeasureStatu::Measure_Border) {
+        addOffset(measure_result);
+        auto out_of_statu = outOfRangeDetection(measure_statu, measure_result, true);
+        if (out_of_statu != Range_status::Range_correct) {
+            info.set_border_statu(out_of_statu);
+            info.set_ground_status(MeasureStatu::Measure_Border);
+        }
+        info.set_cx(measure_result.car.uniform_x);
+        info.set_cy(measure_result.car.uniform_y);
+        info.set_theta(measure_result.car.theta * 180.0 / M_PI);
+        info.set_length(0);
+        info.set_width(measure_result.car.wheel_width);
+        info.set_wheelbase(measure_result.car.wheel_base);
+        info.set_front_theta(measure_result.car.front_wheels_theta * 180.0 / M_PI);
+        if (DetectManager::iter()->carMoveStatus() == FAILED) {
+            info.set_motion_statu(1);    // 静止3秒
+            info.set_is_stop(1);
+        } else {
+            info.set_motion_statu(0);    // 运动
+            info.set_is_stop(0);
+        }
+        info.set_move_distance(measure_result.car.forward_dis);
+    }
+
+    rabbit_measure_info.mutable_measure_info_to_plc_forward()->CopyFrom(info);
+    rabbit_measure_info.mutable_measure_info_to_plc_reverse()->CopyFrom(info);
+    rabbit_measure_info.mutable_measure_info_to_terminal()->CopyFrom(info);
+    if (info.ground_status() == MeasureStatu::Measure_OK || info.ground_status() == MeasureStatu::Measure_Border) {
+        auto out_of_statu = outOfRangeDetection(measure_statu, measure_result, false);
+        if (out_of_statu != Range_status::Range_correct) {
+            rabbit_measure_info.mutable_measure_info_to_terminal()->set_border_statu(out_of_statu);
+            rabbit_measure_info.mutable_measure_info_to_terminal()->set_ground_status(MeasureStatu::Measure_Border);
+        }
+    }
+
+    RabbitmqCommunicationTof3D::get_instance_pointer()->encapsulate_status_msg(rabbit_measure_info.DebugString(), 0);
+    return {};
+}
+
+Range_status CommunicationManager::outOfRangeDetection(const MeasureStatu &measure_statu, DetectManager::DetectResult &measure_result, bool isPlc) {
+    // 测量给出超界，说明车辆未到达指定区域，需要前进
+    if (measure_statu == MeasureStatu::Measure_Border) {
+        return Range_status::Range_back;
+    }
+    if (measure_statu != MeasureStatu::Measure_OK && measure_statu != MeasureStatu::Measure_Border) {
+        return Range_status::Range_correct;
+    }
+
+    outOfRangeInfo out_info = isPlc ? m_config.plc_out_info() : m_config.display_out_info();
+    // 车位位于y负方向, 判断后轮超界情况, 4.80是搬运器大Y的运动极限, 2.7是同侧两个小y夹持杆中心距离
+    // 判断车辆轴距超界情况
+    if (measure_result.car.wheel_base < out_info.car_min_wheelbase() ||    // 轴距过小
+        measure_result.car.wheel_base > out_info.car_max_wheelbase()) {    // 轴距过长
+        last_range_statu |= Range_status::Range_car_wheelbase;
+    } else if (measure_result.car.wheel_base > out_info.car_min_wheelbase() + 0.01 &&    // 轴距过小
+               measure_result.car.wheel_base < out_info.car_max_wheelbase() - 0.01) {
+        last_range_statu &= (~Range_status::Range_car_wheelbase);
+    }
+    if (last_range_statu & Range_status::Range_car_wheelbase) {
+        return Range_status::Range_car_wheelbase;
+    }
+
+    // 判断车辆宽度超界情况
+    if (measure_result.car.wheel_width < out_info.car_min_width() ||   // 车宽过窄
+        measure_result.car.wheel_width > out_info.car_max_width()) {   // 车宽过宽
+        last_range_statu |= Range_status::Range_car_width;       // 车辆超宽
+    } else if (measure_result.car.wheel_width > out_info.car_min_width() + 0.01 &&   // 车宽过窄
+               measure_result.car.wheel_width < out_info.car_max_width() - 0.01) {
+        last_range_statu &= (~Range_status::Range_car_width);
+    }
+    if (last_range_statu & Range_status::Range_car_width) {
+        return Range_status::Range_car_width;
+    }
+
+    // 判断车辆旋转角超界情况
+    double t_dtheta = measure_result.car.theta;
+    if (measure_result.car.theta < out_info.turnplate_angle_limit_min_clockwise()) {
+        last_range_statu |= Range_status::Range_angle_clock;
+        last_range_statu &= (~Range_status::Range_angle_anti_clock);
+    } else if (measure_result.car.theta > out_info.turnplate_angle_limit_max_clockwise()) {
+        last_range_statu |= Range_status::Range_angle_anti_clock;
+        last_range_statu &= (~Range_status::Range_angle_clock);
+    } else if ( measure_result.car.theta > out_info.turnplate_angle_limit_min_clockwise() &&
+                measure_result.car.theta < out_info.turnplate_angle_limit_max_clockwise()) {
+        last_range_statu &= (~Range_status::Range_angle_clock);
+        last_range_statu &= (~Range_status::Range_angle_anti_clock);
+    }
+    if (last_range_statu & Range_status::Range_angle_clock) {
+        return Range_status::Range_angle_clock;
+    }
+    if (last_range_statu & Range_status::Range_angle_anti_clock) {
+        return Range_status::Range_angle_anti_clock;
+    }
+
+    // 方向盘调正
+    if (fabs(measure_result.car.front_wheels_theta) > 10) {
+        last_range_statu |= Range_status::Range_steering_wheel_nozero;
+    } else if (fabs(measure_result.car.front_wheels_theta) < 7) {
+        last_range_statu &= (~Range_status::Range_steering_wheel_nozero);
+    }
+    if (last_range_statu & Range_status::Range_angle_anti_clock) {
+        return Range_status::Range_steering_wheel_nozero;
+    }
+
+    // 左右超界
+    if (measure_result.car.uniform_x < out_info.border_minx()) {
+        last_range_statu |= Range_status::Range_left;
+        last_range_statu &= (~Range_status::Range_right);
+    } else if (measure_result.car.uniform_x > out_info.border_maxx()) {
+        last_range_statu |= Range_status::Range_right;
+        last_range_statu &= (~Range_status::Range_left);
+    } else if (measure_result.car.uniform_x > out_info.border_minx() + 0.01 &&
+               measure_result.car.uniform_x < out_info.border_maxx() - 0.01) {
+        last_range_statu &= (~Range_status::Range_left);
+        last_range_statu &= (~Range_status::Range_right);
+    }
+    if (last_range_statu & Range_status::Range_left) {
+        return Range_status::Range_left;
+    }
+    if (last_range_statu & Range_status::Range_right) {
+        return Range_status::Range_right;
+    }
+
+    // 后超界
+    double t_center_y = fabs(measure_result.car.uniform_y);
+    DLOG(INFO) << t_center_y - 4.80 + 0.5 * (measure_result.car.wheel_base - 2.7) << " " << out_info.plc_border_maxy();
+    DLOG(INFO) << t_center_y - 4.80 - 0.5 * (measure_result.car.wheel_base - 2.7) << " " << out_info.plc_border_miny();
+    if (t_center_y - 4.80 + 0.5 * (measure_result.car.wheel_base - 2.7) > out_info.plc_border_maxy() ||     // 前轮无法抱到
+        t_center_y - 4.80 - 0.5 * (measure_result.car.wheel_base - 2.7) > out_info.plc_border_miny()) {     // 后轮无法抱到
+        last_range_statu |= Range_status::Range_back;       // 后超界
+    } else if (t_center_y - 4.80 + 0.5 * (measure_result.car.wheel_base - 2.7) < out_info.plc_border_maxy() - 0.010 &&     // 前轮无法抱到
+               t_center_y - 4.80 - 0.5 * (measure_result.car.wheel_base - 2.7) < out_info.plc_border_miny() - 0.010) {
+        last_range_statu &= (~Range_status::Range_back);
+    }
+    if (last_range_statu & Range_status::Range_back) {
+        // 前进距离
+        measure_result.car.forward_dis = MAX((t_center_y - 4.80 + 0.5 * (measure_result.car.wheel_base - 2.7) - (out_info.plc_border_maxy() - 0.010)),
+                                         (t_center_y - 4.80 - 0.5 * (measure_result.car.wheel_base - 2.7) - (out_info.plc_border_miny() - 0.010)));
+        return Range_status::Range_back;
+    } 
+
+    return Range_status::Range_correct;
+}
+
+Error_manager CommunicationManager::addOffset(DetectManager::DetectResult &measure_result) {
+    auto offset = m_config.offset();
+    measure_result.car.uniform_x += offset.x();
+    measure_result.car.wheels_center_x += offset.x();
+    measure_result.car.uniform_y += offset.y();
+    measure_result.car.wheels_center_y += offset.y();
+    measure_result.car.wheel_base += offset.wheelbase();
+    measure_result.car.theta += offset.theta();
+    measure_result.car.width += offset.width();
+    measure_result.car.wheel_width += offset.width();
     return {};
 }
+
+Error_manager CommunicationManager::removeOffset(DetectManager::DetectResult &measure_result) {
+    auto offset = m_config.offset();
+    measure_result.car.uniform_x -= offset.x();
+    measure_result.car.wheels_center_x -= offset.x();
+    measure_result.car.uniform_y -= offset.y();
+    measure_result.car.wheels_center_y -= offset.y();
+    measure_result.car.wheel_base -= offset.wheelbase();
+    measure_result.car.theta -= offset.theta();
+    measure_result.car.width -= offset.width();
+    measure_result.car.wheel_width -= offset.width();
+    return {};
+}
+

communication/communication_manager.h

@@ -17,10 +17,30 @@ public:
 
     Error_manager Init(const CommunicationManagerConfig &config);
 protected:
+    void run();
+
+    MeasureStatu getMeasureData(DetectManager::DetectResult &detect_measure_info);
+
+    Error_manager sendGrpcData(const MeasureStatu &measure_statu, DetectManager::DetectResult &measure_result);
+
+    Error_manager sendMqttData(const MeasureStatu &measure_statu, DetectManager::DetectResult &measure_result);
+
+    Error_manager sendRabbitmqData(const MeasureStatu &measure_statu, DetectManager::DetectResult &measure_result);
+
+    Range_status outOfRangeDetection(const MeasureStatu &measure_statu, DetectManager::DetectResult &measure_result, bool isPlc);
+
+    Error_manager addOffset(DetectManager::DetectResult &measure_result);
+
+    Error_manager removeOffset(DetectManager::DetectResult &measure_result);
 
 private:
     StreamRpcServer *m_grpc_server = nullptr;
 
+    Thread_condition *condition = nullptr;
+    std::thread *run_t = nullptr;
+    CommunicationManagerConfig m_config;
+    int last_range_statu = 0;
+    int heart = 0;
 };
 
 

communication/rabbitmq/Boundary.cpp

@@ -1,160 +0,0 @@
-//
-// Created by zx on 23-11-27.
-//
-
-#include "Boundary.h"
-#include <pcl/point_types.h>
-#include <fcntl.h>
-#include <google/protobuf/io/zero_copy_stream_impl.h>
-#include <google/protobuf/text_format.h>
-
-using google::protobuf::io::FileInputStream;
-using google::protobuf::io::FileOutputStream;
-using google::protobuf::io::ZeroCopyInputStream;
-using google::protobuf::io::CodedInputStream;
-using google::protobuf::io::ZeroCopyOutputStream;
-using google::protobuf::io::CodedOutputStream;
-using google::protobuf::Message;
-
-bool Boundary::read_proto_param(std::string prototxt_path, ::google::protobuf::Message &protobuf_parameter) {
-    int fd = open(prototxt_path.c_str(), O_RDONLY);
-    if (fd == -1) return false;
-    FileInputStream *input = new FileInputStream(fd);
-    bool success = google::protobuf::TextFormat::Parse(input, &protobuf_parameter);
-    delete input;
-    close(fd);
-    return success;
-}
-
-bool Boundary::init(const std::string &file) {
-    return read_proto_param(file, limit_);
-}
-
-bool Boundary::init(JetStream::Limit limit) {
-    limit_ = limit;
-    return true;
-}
-
-Boundary::~Boundary() {}
-
-BoundRet Boundary::limit_boundary(JetStream::MeasureInfo info, JetStream::Boundary bound) {
-    //判断角度
-    if (info.theta() <= bound.minangle() / 180.0 * M_PI) return BoundRetRightAngle;
-    if (info.theta() >= bound.maxangle() / 180.0 * M_PI) return BoundRetLeftAngle;
-    //计算四轮位置
-    pcl::PointXYZ pt[4];
-    pt[0] = pcl::PointXYZ(-info.width() / 2.0, info.wheelbase() / 2.0, 0);
-    pt[1] = pcl::PointXYZ(info.width() / 2.0, info.wheelbase() / 2.0, 0);
-    pt[2] = pcl::PointXYZ(-info.width() / 2.0, -info.wheelbase() / 2.0, 0);
-    pt[3] = pcl::PointXYZ(info.width() / 2.0, -info.wheelbase() / 2.0, 0);
-    for (int i = 0; i < 4; ++i) {
-        //计算四轮位置
-        float x = pt[i].x * cos(info.theta()) - pt[i].y * sin(info.theta()) + info.x();
-        float y = pt[i].x * sin(info.theta()) + pt[i].y * cos(info.theta()) + info.y();
-        //判断是否在盖板上
-        // printf(" %f  %f\n", x * x + y * y, bound.radius() * bound.radius());
-        if (x * x + y * y > bound.radius() * bound.radius()) {
-            if (fabs(y) > fabs(x)) {
-                if (y > 0) return BoundRetFront;
-                else return BoundRetBack;
-            } else {
-                if (x > 0) return BoundRetRight;
-                else return BoundRetLeft;
-            }
-        }
-        //判断左右
-        if (x < bound.left()) return BoundRetLeft;
-        if (x > bound.right()) return BoundRetRight;
-    }
-    //判断摆正后，后轮是否能够的着
-    if (info.trans_y() - 0.5 * info.wheelbase() <= bound.buttom()) return BoundRetBack;
-
-    if (fabs(info.ftheta()) > bound.fwheelangle()) return BoundRetWheelAngle;
-
-    return BoundRetNormal;
-
-}
-
-void Boundary::limit(JetStream::MeasureInfo &info) {
-
-    float trans_x = info.x() * cos(info.theta()) - info.y() * sin(info.theta());
-    float trans_y = info.x() * sin(info.theta()) + info.y() * cos(info.theta());
-    info.set_trans_x(trans_x);
-    info.set_trans_y(trans_y);
-    //判断轴距和宽度是否超
-    if (info.width() >= limit_.maxwidth()) {
-        info.set_border_plc(int(BoundRetWidth));
-        info.set_border_display(int(BoundRetWidth));
-        return;
-    }
-    if (info.wheelbase() >= limit_.maxwheelbase()) {
-        info.set_border_plc(int(BoundRetWheelBase));
-        info.set_border_display(int(BoundRetWheelBase));
-        return;
-    }
-
-    info.set_border_plc(int(limit_boundary(info, limit_.plc_limit())));
-    info.set_border_display(int(limit_boundary(info, limit_.display_limit())));
-
-}
-
-void Boundary::transMeasureInfo(JetStream::MeasureInfo &in, measure_buffer &out) {
-    measure_info info;
-    info.CopyFrom(out.measure_info_to_plc_forward());
-    // 如果地面雷达状态已经被修改过，直接返回
-    if (info.ground_status() != Measure_OK) {
-        out.mutable_measure_info_to_plc_reverse()->CopyFrom(info);
-        out.mutable_measure_info_to_terminal()->CopyFrom(info);
-        return;
-    }
-
-    // 设备状态
-    if (in.border_display() != Range_status::Range_correct || in.border_plc() != Range_status::Range_correct) {
-        info.set_ground_status((MeasureStatu::Measure_Border));
-    }
-
-    info.set_cx(in.trans_x());                                 // 车辆中心坐标x
-    info.set_cy(in.trans_y());
-    info.set_theta(in.theta() * 180.0 / M_PI);
-    info.set_length(0);
-    info.set_width(in.width());
-    info.set_wheelbase(in.wheelbase());
-    info.set_front_theta(in.ftheta() * 180.0 / M_PI);
-    info.set_border_statu(in.border_plc());
-    info.set_is_stop(0);
-
-    if (DetectManager::iter()->carMoveStatus() == FAILED) {
-        info.set_motion_statu(1);    // 静止3秒
-    } else {
-        info.set_motion_statu(0);    // 运动
-    }
-    info.set_move_distance(0);
-
-    out.mutable_measure_info_to_plc_forward()->CopyFrom(info);
-    out.mutable_measure_info_to_plc_reverse()->CopyFrom(info);
-    out.mutable_measure_info_to_terminal()->CopyFrom(info);
-    out.mutable_measure_info_to_terminal()->set_border_statu(in.border_display());
-}
-
-void Boundary::transMeasureInfo(JetStream::MeasureInfo &in, TransitData::MeasureInfo &out) {
-    out.x = in.x();
-    out.y = in.y();
-    out.transx = in.trans_x();
-    out.transy = in.trans_y();
-    out.theta = in.theta();
-    out.width = in.width();
-    out.wheelbase = in.wheelbase();
-    out.ftheta = in.ftheta();
-    out.border_plc = in.border_plc();
-    out.border_display = in.border_display();
-    out.error = in.error();
-}
-
-void Boundary::transMeasureInfo(DetectManager::DetectResult &in, JetStream::MeasureInfo &out) {
-    out.set_x(in.car.wheels_center_x);
-    out.set_y(in.car.wheels_center_y);
-    out.set_theta(in.car.theta);
-    out.set_width(in.car.wheel_width);
-    out.set_wheelbase(in.car.wheel_base);
-    out.set_ftheta(in.car.front_wheels_theta);
-}

communication/rabbitmq/Boundary.h

@@ -1,52 +0,0 @@
-//
-// Created by zx on 23-11-27.
-//
-
-#ifndef CERES_TEST_BOUNDARY_H
-#define CERES_TEST_BOUNDARY_H
-
-#include "proto/def.grpc.pb.h"
-#include "proto/communication.pb.h"
-#include "communication/transitData.h"
-#include "detect/detect_manager.h"
-
-enum BoundRet {
-    BoundRetNormal = Range_status::Range_correct,  //正常
-    BoundRetWidth = Range_status::Range_car_width,
-    BoundRetWheelBase = Range_status::Range_car_wheelbase,
-    BoundRetFront = Range_status::Range_front,   //前超界
-    BoundRetBack = Range_status::Range_back,    //后超界
-    BoundRetLeft = Range_status::Range_left,    //左超界
-    BoundRetRight = Range_status::Range_right,   //右超界
-    BoundRetLeftAngle = Range_status::Range_angle_anti_clock, //左倾角过大
-    BoundRetRightAngle = Range_status::Range_angle_clock,  //右倾角过大
-    BoundRetWheelAngle = Range_status::Range_steering_wheel_nozero   //前轮角未回正
-};
-
-class Boundary {
-public:
-    Boundary() = default;
-
-    bool init(const std::string &file);
-
-    bool init(JetStream::Limit limit);
-
-    ~Boundary();
-
-    void limit(JetStream::MeasureInfo &info);
-
-    void transMeasureInfo(JetStream::MeasureInfo &in, measure_buffer &out);
-
-    void transMeasureInfo(JetStream::MeasureInfo &in, TransitData::MeasureInfo &out);
-
-    void transMeasureInfo(DetectManager::DetectResult &in, JetStream::MeasureInfo &out);
-
-private:
-    BoundRet limit_boundary(JetStream::MeasureInfo info, JetStream::Boundary bound);
-
-    bool read_proto_param(std::string prototxt_path, ::google::protobuf::Message &protobuf_parameter);
-
-    JetStream::Limit limit_;
-};
-
-#endif //CERES_TEST_BOUNDARY_H

communication/rabbitmq/rabbitmq_communication.cpp

@@ -17,34 +17,7 @@ Error_manager RabbitmqCommunicationTof3D::check_executer(Rabbitmq_message *p_msg
 
 //处理消息, 需要子类重载
 Error_manager RabbitmqCommunicationTof3D::execute_msg(Rabbitmq_message *p_msg) {
-    Error_manager ret;
-//    LOG(INFO) << p_msg->m_routing_key;
-//    if (p_msg->m_routing_key == "dispatch_31_statu_port") {
-//        dispatch_node_statu t_dispatch_node_statu;
-//        if(google::protobuf::TextFormat::ParseFromString(p_msg->m_message_buf, &t_dispatch_node_statu))
-//        {
-//            LOG(INFO) << t_dispatch_node_statu.DebugString();
-//            // TODO:开关门控制算法流程
-////            usleep(5 * 1000 * 1000);
-////            DetectManager::iter()->Stop();
-////            DeviceTof3D::iter()->Stop();
-////
-////            usleep(5 * 1000 * 1000);
-////            DeviceTof3D::iter()->Continue();
-////            DetectManager::iter()->Continue();
-//        }
-//    }
-//
-//    //终端点击存车时, 发送存车表单给检查节点, 这里顺便发送给感测节点.
-//    if (p_msg->m_routing_key == "user_park_command_request_port") {
-//        park_table t_park_table_msg; //存车表单
-//        if(google::protobuf::TextFormat::ParseFromString(p_msg->m_message_buf, &t_park_table_msg))
-//        {
-//            LOG(INFO) << t_park_table_msg.DebugString();
-//            // TODO:从算法取数据保存
-//        }
-//    }
-    return ret;
+    return {};
 }
 
 //处理消息, 需要子类重载
@@ -54,49 +27,13 @@ Error_manager RabbitmqCommunicationTof3D::execute_time_consume_msg(Rabbitmq_mess
 
 //定时封装发送消息, 一般为心跳和状态信息, 需要子类重载
 Error_manager RabbitmqCommunicationTof3D::auto_encapsulate_status() {
-    static int heart = 0;
-    if (++heart > 999) {
-        heart = 0;
-    }
-
-    measure_buffer rabbit_measure_info;
-    DetectManager::DetectResult detect_measure_info;
-
-    // 设备状态
-    if (DeviceTof3D::iter()->getDeviceStatus() != SUCCESS) {
-        rabbit_measure_info.mutable_measure_info_to_plc_forward()->set_ground_status(MeasureStatu::Lidar_Disconnect);
-    }
-    MeasureStatu measure_status = DetectManager::iter()->getMeasureResult(detect_measure_info);
-    if (measure_status != MeasureStatu::Measure_OK) {
-        if (measure_status == MeasureStatu::Measure_Border) {
-            rabbit_measure_info.mutable_measure_info_to_plc_forward()->set_border_statu(Range_status::Range_back);
-        }
-        rabbit_measure_info.mutable_measure_info_to_plc_forward()->set_ground_status(measure_status);
-    }
-
-    // 超界判断
-    JetStream::MeasureInfo info;
-    m_boundary.transMeasureInfo(detect_measure_info, info);
-    m_boundary.limit(info);
-
-    // 保存grpc数据中间块
-    TransitData::MeasureInfo grpc_measure_info;
-    m_boundary.transMeasureInfo(info, grpc_measure_info);
-    grpc_measure_info.error.append("heart: " + std::to_string(heart));
-    grpc_measure_info.error.append(detect_measure_info.info());
-    TransitData::get_instance_pointer()->setMeasureInfo(grpc_measure_info);
-
-    // 发送rabbitmq消息
-    m_boundary.transMeasureInfo(info, rabbit_measure_info);
-    encapsulate_status_msg(rabbit_measure_info.DebugString(), 0);
-
     return {};
 }
 
 Error_manager RabbitmqCommunicationTof3D::rabbitmq_init_from_protobuf(std::string prototxt_path) {
-    m_boundary.init(ETC_PATH PROJECT_NAME "/limit.prototxt") ?
-    printf("boundary init success.\n") :
-    printf("boundary init failed form %s.\n", ETC_PATH PROJECT_NAME "/limit.prototxt");
+//    m_boundary.init(ETC_PATH PROJECT_NAME "/limit.prototxt") ?
+//    printf("boundary init success.\n") :
+//    printf("boundary init failed form %s.\n", ETC_PATH PROJECT_NAME "/limit.prototxt");
     return Rabbitmq_base::rabbitmq_init_from_protobuf(prototxt_path);
 }
 

communication/rabbitmq/rabbitmq_communication.h

@@ -3,7 +3,6 @@
 #include "tool/singleton.hpp"
 #include "rabbitmq/rabbitmq_base.h"
 #include "detect/detect_manager.h"
-#include "Boundary.h"
 
 class RabbitmqCommunicationTof3D : public Singleton<RabbitmqCommunicationTof3D>, public Rabbitmq_base {
 // 子类必须把父类设定为友元函数，这样父类才能使用子类的私有构造函数。
@@ -48,5 +47,4 @@ public:
     std::mutex m_lock;                                //锁,
 
 private:
-    Boundary m_boundary;
 };

detect/ceres_detect/car_pose_detector.h

@@ -37,6 +37,9 @@ public:
         float theta = 0;
         float wheel_base = 0;
         float loss = 0;
+        float uniform_x = 0;
+        float uniform_y = 0;
+        float forward_dis = 0;
 
         void reset() {
             wheels_center_x = 0;
@@ -49,12 +52,15 @@ public:
             front_wheels_theta = 0;
             wheel_base = 0;
             loss = 0;
+            uniform_x = 0;
+            uniform_y = 0;
+            forward_dis = 0;
         }
 
         std::string info() {
             char str[512];
             sprintf(str, "x:%0.3f y:%0.3f theta:%0.3f w:%0.3f l:%0.3f ww:%0.3f wl:%0.3f ft:%0.3f wb:%0.3f loss:%0.3f\n",
-                    wheels_center_x, wheels_center_y, theta, width, length, wheel_width, wheel_length, front_wheels_theta, wheel_base, loss);
+                    wheels_center_x, wheels_center_y, theta * 180.0 / M_PI, width, length, wheel_width, wheel_length, front_wheels_theta, wheel_base, loss);
             return str;
         }
     };

detect/ceres_detect/detect_wheel_ceres.cpp

@@ -5,6 +5,10 @@
 #include "detect_wheel_ceres.h"
 #include <ceres/cubic_interpolation.h>
 #include <pcl/common/transforms.h>
+#include <pcl/sample_consensus/ransac.h>
+#include <pcl/sample_consensus/sac_model_line.h> // 拟合直线
+#include <pcl/ModelCoefficients.h>          //导入ModelCoefficients（模型系数）结构
+#include <pcl/filters/project_inliers.h>        //导入ProjectInliers滤波器
 
 class CostDynFunc{
     std::vector<double>   line_l_;
@@ -42,7 +46,7 @@ public:
       T theta_front = variable[3];
 
       //整车旋转
-      Eigen::Rotation2D<T> rotation(-theta);
+      Eigen::Rotation2D<T> rotation(theta);
       Eigen::Matrix<T, 2, 2> rotation_matrix = rotation.toRotationMatrix();
 
       //左前偏移
@@ -70,8 +74,8 @@ public:
       //左前轮
       int left_front_num = m_left_front_cloud->size();
       for (int i = 0; i < m_left_front_cloud->size(); ++i) {
-        const Eigen::Matrix<T, 2, 1> point((T(m_left_front_cloud->points[i].x) - cx),
-                                           (T(m_left_front_cloud->points[i].y) - cy));
+        const Eigen::Matrix<T, 2, 1> point((T(m_left_front_cloud->points[i].x) + cx),
+                                           (T(m_left_front_cloud->points[i].y) + cy));
         //减去经过车辆旋转计算的左前中心
         const Eigen::Matrix<T, 2, 1> tanslate_point = rotation_matrix * point - wheel_center_normal_left_front;
         //旋转
@@ -83,8 +87,8 @@ public:
       //右前轮
       int right_front_num = m_right_front_cloud->size();
       for (int i = 0; i < m_right_front_cloud->size(); ++i) {
-        const Eigen::Matrix<T, 2, 1> point((T(m_right_front_cloud->points[i].x) - cx),
-                                           (T(m_right_front_cloud->points[i].y) - cy));
+        const Eigen::Matrix<T, 2, 1> point((T(m_right_front_cloud->points[i].x) + cx),
+                                           (T(m_right_front_cloud->points[i].y) + cy));
         //减去经过车辆旋转计算的左前中心
         const Eigen::Matrix<T, 2, 1> tanslate_point = rotation_matrix * point - wheel_center_normal_right_front;
         //旋转
@@ -98,8 +102,8 @@ public:
       int left_rear_num = m_left_rear_cloud->size();
 
       for (int i = 0; i < m_left_rear_cloud->size(); ++i) {
-        const Eigen::Matrix<T, 2, 1> point((T(m_left_rear_cloud->points[i].x) - cx),
-                                           (T(m_left_rear_cloud->points[i].y) - cy));
+        const Eigen::Matrix<T, 2, 1> point((T(m_left_rear_cloud->points[i].x) + cx),
+                                           (T(m_left_rear_cloud->points[i].y) + cy));
         //减去经过车辆旋转计算的左前中心
         const Eigen::Matrix<T, 2, 1> tanslate_point = rotation_matrix * point - wheel_center_normal_left_rear;
         interpolator_l.Evaluate(tanslate_point[0]*T(1000.)+T(200.),&residual[2*(left_front_num + right_front_num + i)]);
@@ -110,8 +114,8 @@ public:
 
       //右后轮
       for (int i = 0; i < m_right_rear_cloud->size(); ++i) {
-        const Eigen::Matrix<T, 2, 1> point((T(m_right_rear_cloud->points[i].x) - cx),
-                                           (T(m_right_rear_cloud->points[i].y) - cy));
+        const Eigen::Matrix<T, 2, 1> point((T(m_right_rear_cloud->points[i].x) + cx),
+                                           (T(m_right_rear_cloud->points[i].y) + cy));
         //减去经过车辆旋转计算的左前中心
         const Eigen::Matrix<T, 2, 1> tanslate_point = rotation_matrix * point - wheel_center_normal_right_rear;
 
@@ -138,8 +142,8 @@ private:
 
 public:
     CostRYFunc(pcl::PointCloud<pcl::PointXYZ>::Ptr left_front,pcl::PointCloud<pcl::PointXYZ>::Ptr right_front,
-        pcl::PointCloud<pcl::PointXYZ>::Ptr left_rear,pcl::PointCloud<pcl::PointXYZ>::Ptr right_rear,
-        std::vector<double> liney)
+               pcl::PointCloud<pcl::PointXYZ>::Ptr left_rear,pcl::PointCloud<pcl::PointXYZ>::Ptr right_rear,
+               std::vector<double> liney)
     {
       line_y_=liney;
       m_left_front_cloud=left_front;
@@ -155,7 +159,7 @@ public:
       T length = variable[2];
 
       //整车旋转
-      Eigen::Rotation2D<T> rotation(-theta);
+      Eigen::Rotation2D<T> rotation(theta);
       Eigen::Matrix<T, 2, 2> rotation_matrix = rotation.toRotationMatrix();
 
       //左前偏移
@@ -231,6 +235,97 @@ private:
 
 };
 
+class CostRXFunc{
+private:
+    std::vector<double>   line_x_;
+
+    pcl::PointCloud<pcl::PointXYZ>::Ptr          m_left_front_cloud;     //左前点云
+    pcl::PointCloud<pcl::PointXYZ>::Ptr          m_right_front_cloud;    //右前点云
+    pcl::PointCloud<pcl::PointXYZ>::Ptr          m_left_rear_cloud;      //左后点云
+    pcl::PointCloud<pcl::PointXYZ>::Ptr          m_right_rear_cloud;     //右后点云
+
+public:
+    CostRXFunc(pcl::PointCloud<pcl::PointXYZ>::Ptr left_front,pcl::PointCloud<pcl::PointXYZ>::Ptr right_front,
+               pcl::PointCloud<pcl::PointXYZ>::Ptr left_rear,pcl::PointCloud<pcl::PointXYZ>::Ptr right_rear,
+                 std::vector<double> linex)
+    {
+      line_x_=linex;
+      m_left_front_cloud=left_front;
+      m_right_front_cloud=right_front;
+      m_left_rear_cloud=left_rear;
+      m_right_rear_cloud=right_rear;
+
+    }
+    template <typename T>
+    bool operator()(const T* const variable, T* residual) const {
+      T cx = variable[0];
+      T theta = variable[1];
+
+
+      //整车旋转
+      Eigen::Rotation2D<T> rotation(theta);
+      Eigen::Matrix<T, 2, 2> rotation_matrix = rotation.toRotationMatrix();
+
+
+      ceres::Grid1D<double> grid_x(line_x_.data(),0,line_x_.size());
+      ceres::CubicInterpolator<ceres::Grid1D<double>> interpolator_x(grid_x);
+
+      T weight=T(1.0);
+//左前轮
+
+      int left_front_num = m_left_front_cloud->size();
+      for (int i = 0; i < m_left_front_cloud->size(); ++i) {
+        const Eigen::Matrix<T, 2, 1> point((T(m_left_front_cloud->points[i].x)),
+                                           (T(m_left_front_cloud->points[i].y)));
+        const Eigen::Matrix<T, 2, 1> tp(cx,T(0));
+        //减去经过车辆旋转计算的左前中心
+        const Eigen::Matrix<T, 2, 1> point_rotation = rotation_matrix * point +tp;
+        interpolator_x.Evaluate(point_rotation[0]*T(1000.)+T(1000.),&residual[i]);
+        residual[i]=residual[i]*weight;
+
+      }
+      //右前轮
+      int right_front_num = m_right_front_cloud->size();
+      for (int i = 0; i < m_right_front_cloud->size(); ++i) {
+        const Eigen::Matrix<T, 2, 1> point((T(m_right_front_cloud->points[i].x) ),
+                                           (T(m_right_front_cloud->points[i].y)));
+        const Eigen::Matrix<T, 2, 1> tp(cx,T(0));
+        //减去经过车辆旋转计算的左前中心
+        const Eigen::Matrix<T, 2, 1> point_rotation = rotation_matrix * point +tp;
+        interpolator_x.Evaluate(point_rotation[0]*T(1000.)+T(1000.),&residual[left_front_num+i]);
+        residual[left_front_num+i]=residual[left_front_num+i]*weight;
+      }
+      //左后轮
+      int left_rear_num = m_left_rear_cloud->size();
+
+      for (int i = 0; i < m_left_rear_cloud->size(); ++i) {
+        const Eigen::Matrix<T, 2, 1> point((T(m_left_rear_cloud->points[i].x)),
+                                           (T(m_left_rear_cloud->points[i].y)));
+        const Eigen::Matrix<T, 2, 1> tp(cx,T(0));
+        //减去经过车辆旋转计算的左前中心
+        const Eigen::Matrix<T, 2, 1> tanslate_point = rotation_matrix * point +tp;
+        interpolator_x.Evaluate(tanslate_point[0]*T(1000.)+T(1000.),&residual[(left_front_num+right_front_num)+i]);
+      }
+
+      //右后轮
+
+      for (int i = 0; i < m_right_rear_cloud->size(); ++i) {
+        const Eigen::Matrix<T, 2, 1> point((T(m_right_rear_cloud->points[i].x)),
+                                           (T(m_right_rear_cloud->points[i].y)));
+        const Eigen::Matrix<T, 2, 1> tp(cx,T(0));
+        //减去经过车辆旋转计算的左前中心
+        const Eigen::Matrix<T, 2, 1> tanslate_point = rotation_matrix * point +tp;
+        interpolator_x.Evaluate(tanslate_point[0]*T(1000.)+T(1000.),
+                                &residual[left_front_num+right_front_num+left_rear_num+i]);
+      }
+
+      return true;
+    }
+
+private:
+
+};
+
 class CostXYWFFunc{
     std::vector<double>   line_l_;
     std::vector<double>   line_r_;
@@ -270,7 +365,7 @@ public:
       T theta=T(theta_);
 
       //整车旋转
-      Eigen::Rotation2D<T> rotation(-theta);
+      Eigen::Rotation2D<T> rotation(theta);
       Eigen::Matrix<T, 2, 2> rotation_matrix = rotation.toRotationMatrix();
 
       //左前偏移
@@ -295,10 +390,11 @@ public:
       ceres::Grid1D<double> grid_y(line_y_.data(),0,line_y_.size());
       ceres::CubicInterpolator<ceres::Grid1D<double>> interpolator_y(grid_y);
 
+
       //左前轮
       int left_front_num = m_left_front_cloud->size();
       for (int i = 0; i < m_left_front_cloud->size(); ++i) {
-        const Eigen::Matrix<T, 2, 1> point((T(m_left_front_cloud->points[i].x) -cx),
+        const Eigen::Matrix<T, 2, 1> point((T(m_left_front_cloud->points[i].x) - cx),
                                            (T(m_left_front_cloud->points[i].y) - cy));
         //减去经过车辆旋转计算的左前中心
         const Eigen::Matrix<T, 2, 1> tanslate_point = rotation_matrix * point - wheel_center_normal_left_front;
@@ -353,7 +449,7 @@ public:
                                 &residual[2*(left_front_num+right_front_num+left_rear_num)+2*i+1]);
 
       }
-
+      
       return true;
     }
 
@@ -392,6 +488,16 @@ detect_wheel_ceres::detect_wheel_ceres()
   }
   fclose(filey);
 
+  FILE* filex= fopen("./cuve_x.txt","w");
+  for(int i=0;i<2000;i++){
+    double x=double(i-1000)*0.001;
+    double y=1.0+1.0/(1.0+exp(-49.*(x-0.9)))-1/(1+exp(-49.*(x+0.9)));
+    m_line_x.push_back(y);
+    fprintf(filex,"%f %f 0\n",x,y);
+
+  }
+  fclose(filex);
+
 
 }
 detect_wheel_ceres::~detect_wheel_ceres(){}
@@ -446,67 +552,313 @@ bool detect_wheel_ceres::detect_dynP(pcl::PointCloud<pcl::PointXYZ>::Ptr cl1,pcl
   return true;
 }
 
-bool detect_wheel_ceres::detect_fall(pcl::PointCloud<pcl::PointXYZ>::Ptr cl1,pcl::PointCloud<pcl::PointXYZ>::Ptr cl2,
-                                     pcl::PointCloud<pcl::PointXYZ>::Ptr cl3,pcl::PointCloud<pcl::PointXYZ>::Ptr cl4,
-                                     float& cx,float& cy,float& theta,float& wheel_base,
-                                     float& width,float& front_theta,float& loss){
-    pcl::PointXYZ tp1,tp2,tp3,tp4;
-    float yaw1=0.0;
-    float yaw2=0.0;
-    float yaw3=0.0;
-    float yaw4=0.0;
-    pcl::PointCloud<pcl::PointXYZ>::Ptr out1,out2,out3,out4;
-    FallModel::fall(cl1,out1,tp1,yaw1,eNagtive);
-    FallModel::fall(cl2,out2,tp2,yaw2,ePositive);
-    FallModel::fall(cl3,out3,tp3,yaw3,eNagtive);
-    FallModel::fall(cl4,out4,tp4,yaw4,ePositive);
-
-    printf("yaw: %f %f %f %f\n",yaw1*180.0/M_PI,yaw2*180.0/M_PI,yaw3*180.0/M_PI,yaw4*180.0/M_PI);
-
-    float angle_max = 10 * M_PI / 180.0;
-    if (fabs(yaw1) > angle_max || fabs(yaw1) > angle_max || fabs(yaw1) > angle_max || fabs(yaw1) > angle_max) {
-        static int error_index = 0;
-        LOG(ERROR) << "left front cloud result: tp " << tp1 << " yaw " << yaw1;
-        Point3D_tool::WriteTxtCloud(ETC_PATH PROJECT_NAME "/data/error/cl1_" + std::to_string(error_index) + ".txt", cl1);
-        Point3D_tool::WriteTxtCloud(ETC_PATH PROJECT_NAME "/data/error/out1_" + std::to_string(error_index) + ".txt", out1);
-        LOG(ERROR) << "left front cloud result: tp " << tp2 << " yaw " << yaw2;
-        Point3D_tool::WriteTxtCloud(ETC_PATH PROJECT_NAME "/data/error/cl2_" + std::to_string(error_index) + ".txt", cl2);
-        Point3D_tool::WriteTxtCloud(ETC_PATH PROJECT_NAME "/data/error/out2_" + std::to_string(error_index) + ".txt", out2);
-        LOG(ERROR) << "left front cloud result: tp " << tp3 << " yaw " << yaw3;
-        Point3D_tool::WriteTxtCloud(ETC_PATH PROJECT_NAME "/data/error/cl3_" + std::to_string(error_index) + ".txt", cl3);
-        Point3D_tool::WriteTxtCloud(ETC_PATH PROJECT_NAME "/data/error/out3_" + std::to_string(error_index) + ".txt", out3);
-        LOG(ERROR) << "left front cloud result: tp " << tp4 << " yaw " << yaw4;
-        Point3D_tool::WriteTxtCloud(ETC_PATH PROJECT_NAME "/data/error/cl4_" + std::to_string(error_index) + ".txt", cl4);
-        Point3D_tool::WriteTxtCloud(ETC_PATH PROJECT_NAME "/data/error/out4_" + std::to_string(error_index) + ".txt", out4);
-        error_index++;
-    }
+float cost(pcl::PointCloud<pcl::PointXYZ>::Ptr cl1,pcl::PointCloud<pcl::PointXYZ>::Ptr cl2,
+           pcl::PointCloud<pcl::PointXYZ>::Ptr cl3,pcl::PointCloud<pcl::PointXYZ>::Ptr cl4,
+           std::vector<double>   line_y_,
+           float cy,float wheelbase,float theta){
+  //整车旋转
+  Eigen::Rotation2D<float> rotation(-theta);
+  Eigen::Matrix<float, 2, 2> rotation_matrix = rotation.toRotationMatrix();
+
+  //左前偏移
+  Eigen::Matrix<float, 2, 1> wheel_center_normal_left_front(-0 / 2.0, wheelbase / 2.0);
+  //右前偏移
+  Eigen::Matrix<float, 2, 1> wheel_center_normal_right_front(0 / 2.0, wheelbase / 2.0);
+  //左后偏移
+  Eigen::Matrix<float, 2, 1> wheel_center_normal_left_rear(-0 / 2.0, -wheelbase / 2.0);
+  //右后偏移
+  Eigen::Matrix<float, 2, 1> wheel_center_normal_right_rear(0 / 2.0, -wheelbase / 2.0);
+
+
+  ceres::Grid1D<double> grid_y(line_y_.data(),0,line_y_.size());
+  ceres::CubicInterpolator<ceres::Grid1D<double>> interpolator_y(grid_y);
+  float weight=1;
+//左前轮
+  float loss=0.;
+  int left_front_num = cl1->size();
+  FILE *lf=fopen("./lf.txt","w");
+  FILE *rf=fopen("./rf.txt","w");
+  FILE *lb=fopen("./lb.txt","w");
+  FILE *rb=fopen("./rb.txt","w");
+  for (int i = 0; i < cl1->size(); ++i) {
+    const Eigen::Matrix<float, 2, 1> point(cl1->points[i].x,cl1->points[i].y);
+    const Eigen::Matrix<float, 2, 1> tp((0),-cy);
+    //减去经过车辆旋转计算的左前中心
+    const Eigen::Matrix<float, 2, 1> point_rotation = rotation_matrix * point +tp- wheel_center_normal_left_front;
+    double cost=0.0;
+    interpolator_y.Evaluate(point_rotation[1]*float(1000.)+float(1000.),&cost);
+    cost=cost*weight;
+    loss+= cost*cost;
+    fprintf(lf,"%f %f %f\n",point_rotation[1],cl1->points[i].z,point_rotation[0]);
+
+  }
+
+  //右前轮
+  int right_front_num = cl2->size();
+  for (int i = 0; i < cl2->size(); ++i) {
+    const Eigen::Matrix<float, 2, 1> point(cl2->points[i].x,cl2->points[i].y);
+    const Eigen::Matrix<float, 2, 1> tp(float(0),-cy);
+    //减去经过车辆旋转计算的左前中心
+    const Eigen::Matrix<float, 2, 1> point_rotation = rotation_matrix * point +tp- wheel_center_normal_right_front;
+
+    double cost=0.0;
+    interpolator_y.Evaluate(point_rotation[1]*float(1000.)+float(1000.),&cost);
+    cost=cost*weight;
+    loss+= cost*cost;
+
+    fprintf(rf,"%f %f %f\n",point_rotation[1],cl2->points[i].z,point_rotation[0]);
+
+  }
+  //左后轮
+  int left_rear_num = cl3->size();
+
+  for (int i = 0; i < cl3->size(); ++i) {
+    const Eigen::Matrix<float, 2, 1> point(cl3->points[i].x,cl3->points[i].y);
+    const Eigen::Matrix<float, 2, 1> tp(0,-cy);
+    //减去经过车辆旋转计算的左前中心
+    const Eigen::Matrix<float, 2, 1> tanslate_point = rotation_matrix * point +tp- wheel_center_normal_left_rear;
+
+    double cost=0.0;
+    interpolator_y.Evaluate(tanslate_point[1]*float(1000.)+float(1000.),&cost);
+    cost=cost*weight;
+    loss+= cost*cost;
+    fprintf(lb,"%f %f %f\n",tanslate_point[1],cl3->points[i].z,tanslate_point[0]);
 
+  }
+
+  //右后轮
+
+  for (int i = 0; i < cl4->size(); ++i) {
+    const Eigen::Matrix<float, 2, 1> point(cl4->points[i].x,cl4->points[i].y);
+    const Eigen::Matrix<float, 2, 1> tp(0,-cy);
+    //减去经过车辆旋转计算的左前中心
+    const Eigen::Matrix<float, 2, 1> tanslate_point = rotation_matrix * point +tp- wheel_center_normal_right_rear;
+
+    double cost=0.0;
+    interpolator_y.Evaluate(tanslate_point[1]*float(1000.)+float(1000.),&cost);
+    cost=cost*weight;
+    loss+= cost*cost;
+    fprintf(rb,"%f %f %f\n",tanslate_point[1],cl4->points[i].z,tanslate_point[0]);
+
+  }
+  fclose(lf);
+  fclose(rf);
+  fclose(lb);
+  fclose(rb);
+  return loss;
+
+}
+
+bool detect_wheel_ceres::ransicLine(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud,
+                                    pcl::PointXYZ& center,double& theta){
+  if(cloud->size()<10)
+    return false;
+  for(int i=0;i<cloud->size();++i){
+    cloud->points[i].z=0;
+  }
+
+  pcl::SampleConsensusModelLine<pcl::PointXYZ>::Ptr model_line(new pcl::SampleConsensusModelLine<pcl::PointXYZ>(cloud));
+  pcl::RandomSampleConsensus<pcl::PointXYZ> ransac(model_line);
+  ransac.setDistanceThreshold(0.02);	//内点到模型的最大距离
+  ransac.setMaxIterations(1000);		//最大迭代次数
+  ransac.computeModel();				//直线拟合
+
+  //-------------------------根据索引提取内点--------------------------
+  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_line(new pcl::PointCloud<pcl::PointXYZ>);
+  std::vector<int> inliers;				//存储内点索引的容器
+  ransac.getInliers(inliers);			//提取内点索引
+  pcl::copyPointCloud<pcl::PointXYZ>(*cloud, inliers, *cloud_line);
+  //----------------------------输出模型参数---------------------------
+  Eigen::VectorXf coefficient;
+  ransac.getModelCoefficients(coefficient);
+
+  
+
+
+  theta= atan(coefficient[3]/coefficient[4]);
+
+  pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients());
+  coefficients->values.resize(6);
+  for(int i=0;i<6;++i){
+    coefficients->values[i] = coefficient[i];
+  }
+  // ------------------------------投影到平面---------------------------------------
+  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_projected(new pcl::PointCloud<pcl::PointXYZ>);
+  pcl::ProjectInliers<pcl::PointXYZ> proj;
+  proj.setModelType(pcl::SACMODEL_LINE);   //创建投影类型，投影到直线上
+  proj.setInputCloud(cloud_line);
+  proj.setModelCoefficients(coefficients);
+  proj.filter(*cloud_projected); 
+
+  Eigen::Vector4f centroid;  //质心
+  pcl::compute3DCentroid(*cloud_projected,centroid); // 计算质心
+  center=pcl::PointXYZ(centroid[0],centroid[1],0);
+  return true;
+}
+
+bool detect_wheel_ceres::detect_ransic(pcl::PointCloud<pcl::PointXYZ>::Ptr cl1, pcl::PointCloud<pcl::PointXYZ>::Ptr cl2,
+                                       pcl::PointCloud<pcl::PointXYZ>::Ptr cl3, pcl::PointCloud<pcl::PointXYZ>::Ptr cl4,
+                                       float &cx, float &cy, float &theta, float &wheel_base, float &width,
+                                       float &front_theta, float &loss) {
+  // cl4->clear();
+
+  pcl::PointXYZ center1,center2,center3,center4;
+  double theta1,theta2,theta3,theta4;
+
+  pcl::PointXYZ vertexs[4];
+  std::vector<pcl::PointXYZ> correct_vertexs;
+  for(int i=0;i<4;++i) vertexs[i]=pcl::PointXYZ(0,0,0);
+
+  if(ransicLine(cl1,center1,theta1)){
+    vertexs[0]=(center1);
+    correct_vertexs.push_back(center1);
+  }
+  if(ransicLine(cl2,center2,theta2)){
+    vertexs[1]=(center2);
+    correct_vertexs.push_back(center2);
+  }
+  if(ransicLine(cl3,center3,theta3)){
+    vertexs[2]=(center3);
+    correct_vertexs.push_back(center3);
+  }
+  if(ransicLine(cl4,center4,theta4)){
+    vertexs[3]=(center4);
+    correct_vertexs.push_back(center4);
+  }
+  std::vector<pcl::PointXYZ> rect_points=correct_vertexs;
+  if(!isRect(rect_points))
+    return false;
+  if(correct_vertexs.size()==4) {
+    pcl::PointXYZ fcenter = pcl::PointXYZ((center1.x + center2.x) * 0.5, (center1.y + center2.y) * 0.5, 0);
+    pcl::PointXYZ bcenter = pcl::PointXYZ((center3.x + center4.x) * 0.5, (center3.y + center4.y) * 0.5, 0);
+    cx = (fcenter.x + bcenter.x) * 0.5;
+    cy = (fcenter.y + bcenter.y) * 0.5;
+    double thetal = atan((center1.x - center3.x) / (center1.y - center3.y));
+    double thetar = atan((center2.x - center4.x) / (center2.y - center4.y));
+    theta = (thetal + thetar) * 0.5;
+    width = 0.5 * sqrt(pow(center1.x - center2.x, 2) + pow(center1.y - center2.y, 2)) +
+            0.5 * sqrt(pow(center3.x - center4.x, 2) + pow(center3.y - center4.y, 2));
+    wheel_base = sqrt(pow(fcenter.x - bcenter.x, 2) + pow(fcenter.y - bcenter.y, 2));
+  }else if(correct_vertexs.size()==3){
+    int errorId=0;
+    for(int i=0;i<4;++i) {
+      if (fabs(vertexs[i].y) < 1e-5) {
+        errorId = i;
+        break;
+      }
+    }
+    if(errorId==0){
+      cx=(center2.x+center3.x)*0.5;
+      cy=(center2.y+center3.y)*0.5;
+      theta = atan((center2.x - center4.x) / (center2.y - center4.y));
+      wheel_base = sqrt(pow(center2.x - center4.x, 2) + pow(center2.y - center4.y, 2));
+      width = sqrt(pow(center3.x - center4.x, 2) + pow(center3.y - center4.y, 2));
+      front_theta=theta2;
+    }else if(errorId==1){
+      cx=(center1.x+center4.x)*0.5;
+      cy=(center1.y+center4.y)*0.5;
+      theta = atan((center1.x - center3.x) / (center1.y - center3.y));
+      wheel_base = sqrt(pow(center1.x - center3.x, 2) + pow(center1.y - center3.y, 2));
+      width = sqrt(pow(center3.x - center4.x, 2) + pow(center3.y - center4.y, 2));
+      front_theta=theta1;
+    }else if(errorId==2){
+      cx=(center1.x+center4.x)*0.5;
+      cy=(center1.y+center4.y)*0.5;
+      theta = atan((center2.x - center4.x) / (center2.y - center4.y));
+      wheel_base = sqrt(pow(center2.x - center4.x, 2) + pow(center2.y - center4.y, 2));
+      width = sqrt(pow(center1.x - center2.x, 2) + pow(center1.y - center2.y, 2));
+      front_theta=(theta1+theta2)*0.5;
+    }else if(errorId==3){
+      cx=(center2.x+center3.x)*0.5;
+      cy=(center2.y+center3.y)*0.5;
+      theta = atan((center1.x - center3.x) / (center1.y - center3.y));
+      wheel_base = sqrt(pow(center1.x - center3.x, 2) + pow(center1.y - center3.y, 2));
+      width = sqrt(pow(center1.x - center2.x, 2) + pow(center1.y - center2.y, 2));
+      front_theta=(theta1+theta2)*0.5;
+    }
     return true;
 
+  }else{
+    return false;
+  }
+
+
+  return true;
+
 }
 
 bool detect_wheel_ceres::detect_2Step(pcl::PointCloud<pcl::PointXYZ>::Ptr cl1,pcl::PointCloud<pcl::PointXYZ>::Ptr cl2,
                   pcl::PointCloud<pcl::PointXYZ>::Ptr cl3,pcl::PointCloud<pcl::PointXYZ>::Ptr cl4,
                                       float& cx,float& cy,float& theta,float& wheel_base,float& width,
                                       float& front_theta,float& loss){
-  m_left_front_cloud= filter(cl1,1.5);
-  m_right_front_cloud=filter(cl2,1.5);
-  m_left_rear_cloud= filter(cl3,1.5);
-  m_right_rear_cloud= filter(cl4,1.5);
 
-  bool ryl=detect_RYL(cl1,cl2,cl3,cl4,cx,cy,theta,wheel_base,width,front_theta,loss);
+  //cl3->clear();
+
+  m_left_front_cloud= filter(cl1,1);
+  m_right_front_cloud=filter(cl2,1);
+  m_left_rear_cloud= filter(cl3,1);
+  m_right_rear_cloud= filter(cl4,1);
+
+
+  bool ryl=detect_RYL(m_left_front_cloud,m_right_front_cloud,m_left_rear_cloud,m_right_rear_cloud,
+                      cx,cy,theta,wheel_base,width,front_theta,loss);
   if(ryl==false) return ryl;
-  bool xwf=detect_XWF(cl1,cl2,cl3,cl4,cx,cy,theta,wheel_base,width,front_theta,loss);
-  width += 0.04;
+
+  bool xwf=detect_XWF(m_left_front_cloud,m_right_front_cloud,m_left_rear_cloud,m_right_rear_cloud,
+                      cx,cy,theta,wheel_base,width,front_theta,loss);
   return xwf;
 }
 
-bool detect_wheel_ceres::detect_RYL(pcl::PointCloud<pcl::PointXYZ>::Ptr cl1,pcl::PointCloud<pcl::PointXYZ>::Ptr cl2,
+bool detect_wheel_ceres::detect_RX(pcl::PointCloud<pcl::PointXYZ>::Ptr cl1,pcl::PointCloud<pcl::PointXYZ>::Ptr cl2,
                                     pcl::PointCloud<pcl::PointXYZ>::Ptr cl3,pcl::PointCloud<pcl::PointXYZ>::Ptr cl4,
                                     float& cx,float& cy,float& theta,float& wheel_base,float& width,
                                     float& front_theta,float& loss){
+  m_left_front_cloud= filter(cl1,1);
+  m_right_front_cloud=filter(cl2,1);
+  m_left_rear_cloud= filter(cl3,1);
+  m_right_rear_cloud= filter(cl4,1);
+
+  double variable[] = {cx,theta};
+  auto t1=std::chrono::steady_clock::now();
+  // 第二部分：构建寻优问题
+  ceres::Problem problem;
+  ceres::CostFunction* cost_function =new
+      ceres::AutoDiffCostFunction<CostRXFunc, ceres::DYNAMIC, 2>(
+      new CostRXFunc(m_left_front_cloud,m_right_front_cloud,m_left_rear_cloud,m_right_rear_cloud,m_line_x),
+      (m_left_front_cloud->size()+m_right_front_cloud->size()+m_left_rear_cloud->size()+m_right_rear_cloud->size()));
+  problem.AddResidualBlock(cost_function, NULL, variable); //向问题中添加误差项，本问题比较简单，添加一个就行。
 
 
+  //第三部分： 配置并运行求解器
+  ceres::Solver::Options options;
+  options.use_nonmonotonic_steps=false;
+  options.linear_solver_type = ceres::DENSE_QR; //配置增量方程的解法
+  options.max_num_iterations=100;
+  options.function_tolerance=1e-10;
+  options.num_threads=2;
+  options.minimizer_progress_to_stdout = false;//输出到cout
+  ceres::Solver::Summary summary;//优化信息
+  ceres::Solve(options, &problem, &summary);//求解!!!
+
+  auto t2=std::chrono::steady_clock::now();
+  auto duration = std::chrono::duration_cast<std::chrono::microseconds>(t2 - t1);
+  double time=double(duration.count()) * std::chrono::microseconds::period::num / std::chrono::microseconds::period::den;
+
+  loss=summary.final_cost/(m_left_front_cloud->size()+m_right_front_cloud->size()+m_left_rear_cloud->size()+m_right_rear_cloud->size());
+
+  cx=variable[0];
+  theta=variable[1];
+  if(loss>0.05){
+    return false;
+  }
+  return true;
+
+}
+
+bool detect_wheel_ceres::detect_RYL(pcl::PointCloud<pcl::PointXYZ>::Ptr cl1,pcl::PointCloud<pcl::PointXYZ>::Ptr cl2,
+                                    pcl::PointCloud<pcl::PointXYZ>::Ptr cl3,pcl::PointCloud<pcl::PointXYZ>::Ptr cl4,
+                                    float& cx,float& cy,float& theta,float& wheel_base,float& width,
+                                    float& front_theta,float& loss){
 
 
   double variable[] = {cy,theta,wheel_base};
@@ -525,6 +877,7 @@ bool detect_wheel_ceres::detect_RYL(pcl::PointCloud<pcl::PointXYZ>::Ptr cl1,pcl:
   options.use_nonmonotonic_steps=false;
   options.linear_solver_type = ceres::DENSE_QR; //配置增量方程的解法
   options.max_num_iterations=100;
+  options.function_tolerance=1e-10;
   options.num_threads=2;
   options.minimizer_progress_to_stdout = false;//输出到cout
   ceres::Solver::Summary summary;//优化信息
@@ -535,10 +888,8 @@ bool detect_wheel_ceres::detect_RYL(pcl::PointCloud<pcl::PointXYZ>::Ptr cl1,pcl:
   double time=double(duration.count()) * std::chrono::microseconds::period::num / std::chrono::microseconds::period::den;
 
   loss=summary.final_cost/(m_left_front_cloud->size()+m_right_front_cloud->size()+m_left_rear_cloud->size()+m_right_rear_cloud->size());
-  /*printf("loss: %.5f size:%d  time:%.5fs\n",loss,m_left_front_cloud->size()+m_right_front_cloud->size()+
-                                                 m_left_rear_cloud->size()+m_right_rear_cloud->size(),time);*/
 
-  //cy=variable[0];
+  cy=variable[0];
   theta=variable[1];
   wheel_base=variable[2];
   if(loss>0.05){
@@ -548,6 +899,8 @@ return true;
 
 }
 
+
+
 bool detect_wheel_ceres::detect_XWF(pcl::PointCloud<pcl::PointXYZ>::Ptr cl1,pcl::PointCloud<pcl::PointXYZ>::Ptr cl2,
                                     pcl::PointCloud<pcl::PointXYZ>::Ptr cl3,pcl::PointCloud<pcl::PointXYZ>::Ptr cl4,
                                     float& cx,float& cy,float& theta,float& wheel_base,float& width,
@@ -580,8 +933,6 @@ bool detect_wheel_ceres::detect_XWF(pcl::PointCloud<pcl::PointXYZ>::Ptr cl1,pcl:
   double time=double(duration.count()) * std::chrono::microseconds::period::num / std::chrono::microseconds::period::den;
 
   loss=summary.final_cost/(m_left_front_cloud->size()+m_right_front_cloud->size()+m_left_rear_cloud->size()+m_right_rear_cloud->size());
-  /*printf("loss: %.5f size:%d  time:%.5fs\n",loss,m_left_front_cloud->size()+m_right_front_cloud->size()+
-                                                 m_left_rear_cloud->size()+m_right_rear_cloud->size(),time);*/
 
   cx=variable[0];
   cy=variable[1];
@@ -594,6 +945,22 @@ bool detect_wheel_ceres::detect_XWF(pcl::PointCloud<pcl::PointXYZ>::Ptr cl1,pcl:
 
 }
 
+pcl::PointCloud<pcl::PointXYZ>::Ptr detect_wheel_ceres::transformRY(
+          pcl::PointCloud<pcl::PointXYZ>::Ptr cloud,float theta,float y){
+  //整车旋转
+  Eigen::Rotation2D<float> rotation(theta);
+  Eigen::Matrix<float, 2, 2> rotation_matrix = rotation.toRotationMatrix();
+  pcl::PointCloud<pcl::PointXYZ>::Ptr out(new pcl::PointCloud<pcl::PointXYZ>);
+  for(int i=0;i<cloud->size();++i){
+    const Eigen::Matrix<float, 2, 1> point(cloud->points[i].x ,cloud->points[i].y);
+    const Eigen::Matrix<float, 2, 1> tp(0.0,y);
+    //减去经过车辆旋转计算的左前中心
+    const Eigen::Matrix<float, 2, 1> point_transform = rotation_matrix * point +tp;
+    out->push_back(pcl::PointXYZ(point_transform[0],point_transform[1],cloud->points[i].z));
+  }
+  return out;
+}
+
 pcl::PointCloud<pcl::PointXYZ>::Ptr detect_wheel_ceres::filter(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud,float r){
   if(cloud->size()==0)
     return cloud;
@@ -615,4 +982,31 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr detect_wheel_ceres::filter(pcl::PointCloud<p
       out->push_back(cloud->points[i]);
   }
   return out;
-}
+}
+
+/*
+bool detect_wheel_ceres::detect_fall(pcl::PointCloud<pcl::PointXYZ>::Ptr cl1,pcl::PointCloud<pcl::PointXYZ>::Ptr cl2,
+                              pcl::PointCloud<pcl::PointXYZ>::Ptr cl3,pcl::PointCloud<pcl::PointXYZ>::Ptr cl4,
+                              float& cx,float& cy,float& theta,float& wheel_base,
+                              float& width,float& front_theta,float& loss){
+  pcl::PointXYZ tp1,tp2,tp3,tp4;
+  float yaw1=0.0;
+  float yaw2=0.0;
+  float yaw3=0.0;
+  float yaw4=0.0;
+  pcl::PointCloud<pcl::PointXYZ>::Ptr out1,out2,out3,out4;
+  FallModel::fall(cl1,out1,tp1,yaw1,eNagtive);
+  FallModel::fall(cl2,out2,tp2,yaw2,ePositive);
+  FallModel::fall(cl3,out3,tp3,yaw3,eNagtive);
+  FallModel::fall(cl4,out4,tp4,yaw4,ePositive);
+
+  save_cloud_txt(out1,"./src1.txt");
+  save_cloud_txt(out2,"./src2.txt");
+  save_cloud_txt(out3,"./src3.txt");
+  save_cloud_txt(out4,"./src4.txt");
+
+  printf("yaw: %f %f %f %f\n",yaw1*180.0/M_PI,yaw2*180.0/M_PI,yaw3*180.0/M_PI,yaw4*180.0/M_PI);
+
+  return true;
+
+}*/

detect/ceres_detect/detect_wheel_ceres.h

@@ -11,11 +11,14 @@
 #include <pcl/point_cloud.h>
 #include <pcl/common/centroid.h>
 #include <opencv2/opencv.hpp>
-#include "fallModel.h"
-#include "tool/point3D_tool.hpp"
+#include "wheel_detector.h"
+#include "point_tool.h"
 
 class detect_wheel_ceres {
 public:
+    detect_wheel_ceres();
+    ~detect_wheel_ceres();
+
     static detect_wheel_ceres *iter() {
         static detect_wheel_ceres *instance = nullptr;
         if (instance == nullptr) {
@@ -24,15 +27,25 @@ public:
         return instance;
     }
 
-    ~detect_wheel_ceres();
-    bool detect_fall(pcl::PointCloud<pcl::PointXYZ>::Ptr cl1,pcl::PointCloud<pcl::PointXYZ>::Ptr cl2,
-                     pcl::PointCloud<pcl::PointXYZ>::Ptr cl3,pcl::PointCloud<pcl::PointXYZ>::Ptr cl4,
-                     float& cx,float& cy,float& theta,float& wheel_base,float& width,float& front_theta,float& loss);
+    bool detect_RX(pcl::PointCloud<pcl::PointXYZ>::Ptr cl1,pcl::PointCloud<pcl::PointXYZ>::Ptr cl2,
+                   pcl::PointCloud<pcl::PointXYZ>::Ptr cl3,pcl::PointCloud<pcl::PointXYZ>::Ptr cl4,
+                   float& cx,float& cy,float& theta,float& wheel_base,float& width,float& front_theta,float& loss);
+
+    bool detect_ransic(pcl::PointCloud<pcl::PointXYZ>::Ptr cl1,pcl::PointCloud<pcl::PointXYZ>::Ptr cl2,
+                       pcl::PointCloud<pcl::PointXYZ>::Ptr cl3,pcl::PointCloud<pcl::PointXYZ>::Ptr cl4,
+                       float& cx,float& cy,float& theta,float& wheel_base,float& width,float& front_theta,float& loss);
+
+    /*bool detect_fall(pcl::PointCloud<pcl::PointXYZ>::Ptr cl1,pcl::PointCloud<pcl::PointXYZ>::Ptr cl2,
+                      pcl::PointCloud<pcl::PointXYZ>::Ptr cl3,pcl::PointCloud<pcl::PointXYZ>::Ptr cl4,
+                      float& cx,float& cy,float& theta,float& wheel_base,float& width,float& front_theta,float& loss);
+*/
 
     bool detect_2Step(pcl::PointCloud<pcl::PointXYZ>::Ptr cl1,pcl::PointCloud<pcl::PointXYZ>::Ptr cl2,
                       pcl::PointCloud<pcl::PointXYZ>::Ptr cl3,pcl::PointCloud<pcl::PointXYZ>::Ptr cl4,
                       float& cx,float& cy,float& theta,float& wheel_base,float& width,float& front_theta,float& loss);
 
+
+
     /*
      * 只检测动态参数， 固定参数已知
      */
@@ -41,7 +54,7 @@ public:
                 float& cx,float& cy,float& theta,float& wheel_base,float& width,float& front_theta,float& loss);
 
 private:
-    detect_wheel_ceres();
+
     bool detect_RYL(pcl::PointCloud<pcl::PointXYZ>::Ptr cl1,pcl::PointCloud<pcl::PointXYZ>::Ptr cl2,
                     pcl::PointCloud<pcl::PointXYZ>::Ptr cl3,pcl::PointCloud<pcl::PointXYZ>::Ptr cl4,
                     float& cx,float& cy,float& theta,float& wheel_base,float& width,float& front_theta,float& loss);
@@ -50,7 +63,18 @@ private:
                     pcl::PointCloud<pcl::PointXYZ>::Ptr cl3,pcl::PointCloud<pcl::PointXYZ>::Ptr cl4,
                     float& cx,float& cy,float& theta,float& wheel_base,float& width,float& front_theta,float& loss);
 
-    pcl::PointCloud<pcl::PointXYZ>::Ptr filter(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud,float r);
+    bool ransicLine(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud,pcl::PointXYZ& center,double& theta);
+
+    /*
+     * 对点云进行过滤
+     */
+    static pcl::PointCloud<pcl::PointXYZ>::Ptr filter(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud,float r);
+    /*
+     * 对点云进行旋转和y的平移变换
+     */
+    static pcl::PointCloud<pcl::PointXYZ>::Ptr transformRY(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud,
+                                                           float theta,float y);
+
 
 
     pcl::PointCloud<pcl::PointXYZ>::Ptr          m_left_front_cloud;     //左前点云
@@ -67,6 +91,8 @@ public:
     std::vector<double>                     m_line_y_2stp;
     std::vector<double>                     m_line_y;
 
+    std::vector<double>                     m_line_x;
+
 };
 
 

detect/ceres_detect/fallModel.cpp

@@ -1,119 +0,0 @@
-//
-// Created by zx on 23-12-7.
-//
-
-#include "fallModel.h"
-#include <pcl/filters/voxel_grid.h>
-#include <pcl/common/common.h>
-
-FallModel::FallModel(){
-
-}
-bool FallModel::fall(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud,pcl::PointCloud<pcl::PointXYZ>::Ptr& out,
-                                                    pcl::PointXYZ& tp,float& theta,FallDirect direct){
-
-  auto t1=std::chrono::steady_clock::now();
-  pcl::PointXYZ minp,maxp;
-  pcl::getMinMax3D(*cloud,minp,maxp);
-  float plate=minp.x;
-  if(direct==ePositive)
-    plate=maxp.x;
-
-  Eigen::Vector4f centroid;  //质心
-  pcl::compute3DCentroid(*cloud,centroid); // 计算质心
-  pcl::PointXYZ src_center=pcl::PointXYZ(centroid[0],centroid[1],centroid[2]);
-
-  pcl::PointXYZ new_center, force_center;
-  float dtheta=M_PI,dx=1.0,forceLen=1.0;
-  pcl::PointCloud<pcl::PointXYZ>::Ptr t_cl=cloud;
-  int iter=0;
-  while(  fabs(dx)>1e-5 || fabs(forceLen)>1e-4) {
-    if(iter==1000){
-      printf(" iter 1000 Failed\n");
-      return false;
-    }
-
-    float gravy, force;
-    computeForce(t_cl, new_center, force_center, gravy, force, plate,direct);
-
-    float dis = plate - new_center.x;
-    float r = 1 - exp(-100 * dis * dis);
-    if(direct==ePositive)
-      dx = r * (gravy-force) / float(t_cl->size());
-    else
-      dx = r * (force - gravy) / float(t_cl->size());
-
-    dx = 0.02 * (1 / (1. + exp(-81. * dx)) - 0.5);
-
-    if(direct==ePositive)
-      forceLen = force_center.y - new_center.y;
-    else
-      forceLen = -force_center.y + new_center.y;
-
-    dtheta = force * forceLen * M_PI / 180.0 * 0.01;
-    float dyaw = 0.05 * (1 / (1. + exp(-81. * dtheta)) - 0.5);
-    t_cl = transformCloud(t_cl, new_center, dx, dyaw);
-
-    theta += dyaw;
-    /*printf(" gravy:%f  force:%f force_len:%f  dx :%f  dtheta:%f Yaw:%f\n", gravy, force,
-           forceLen, dx, dtheta * 180.0 / M_PI, theta * 180.0 / M_PI);*/
-    iter++;
-  }
-  auto t2=std::chrono::steady_clock::now();
-  auto duration = std::chrono::duration_cast<std::chrono::microseconds>(t2 - t1);
-  double time=double(duration.count()) * std::chrono::microseconds::period::num / std::chrono::microseconds::period::den;
-
-  printf("iter:%d  time:%f\n",iter,time);
-  tp=pcl::PointXYZ(new_center.x-src_center.x,new_center.y-src_center.y,new_center.z-src_center.z);
-  out= t_cl;
-  return true;
-}
-
-void FallModel::computeForce(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud,pcl::PointXYZ& center,
-                  pcl::PointXYZ& force_center,float& gravy,float& force,float plate ,FallDirect direct){
-  Eigen::Vector4f centroid;  //质心
-  pcl::compute3DCentroid(*cloud,centroid); // 计算质心
-  center=pcl::PointXYZ(centroid[0],centroid[1],centroid[2]);
-  gravy=cloud->size()*1.0;
-  force=0;
-  pcl::PointCloud<pcl::PointXYZ>::Ptr gravy_cl(new pcl::PointCloud<pcl::PointXYZ>);
-  for(int i=0;i<cloud->size();++i){
-    float deep=cloud->points[i].x-plate;
-    if(direct==eNagtive) {
-      if (deep < 0) {
-        float t = pow(36 * deep, 4) * 10.0;
-        if (t > 2) {
-          t = 2;
-        }
-        force += t;
-        gravy_cl->push_back(cloud->points[i]);
-      }
-    }else{
-      if (deep > 0) {
-        float t = pow(36 * deep, 4) * 10.0;
-        if (t > 2) {
-          t = 2;
-        }
-        force += t;
-        gravy_cl->push_back(cloud->points[i]);
-      }
-    }
-  }
-  pcl::compute3DCentroid(*gravy_cl,centroid); // 计算质心
-  force_center=pcl::PointXYZ(centroid[0],centroid[1],centroid[2]);
-}
-
-pcl::PointCloud<pcl::PointXYZ>::Ptr FallModel::transformCloud(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud,
-                                             pcl::PointXYZ center,float dx,float theta) {
-  Eigen::Rotation2D<float> rotation(theta);
-  Eigen::Matrix<float, 2, 2> rotation_matrix = rotation.toRotationMatrix();
-  pcl::PointCloud<pcl::PointXYZ>::Ptr r_cl(new pcl::PointCloud<pcl::PointXYZ>);
-  for (int i = 0; i < cloud->size(); ++i) {
-    const Eigen::Matrix<float, 2, 1> point(cloud->points[i].x, cloud->points[i].y);
-    const Eigen::Matrix<float, 2, 1> tp(center.x, center.y);
-    //减去经过车辆旋转计算的左前中心
-    const Eigen::Matrix<float, 2, 1> tanslate_point = rotation_matrix * (point - tp) +tp;
-    r_cl->push_back(pcl::PointXYZ(tanslate_point[0]+dx, tanslate_point[1], 0));
-  }
-  return r_cl;
-}

detect/ceres_detect/fallModel.h

@@ -1,36 +0,0 @@
-//
-// Created by zx on 23-12-7.
-//
-
-#ifndef CERES_TEST_FALLMODEL_H
-#define CERES_TEST_FALLMODEL_H
-
-#include <ceres/ceres.h>
-#include <glog/logging.h>
-#include <pcl/point_types.h>
-#include <pcl/point_cloud.h>
-#include <pcl/common/centroid.h>
-#include <opencv2/opencv.hpp>
-#include "wheel_detector.h"
-
-enum FallDirect{
-    ePositive=0,    //正方向落体
-    eNagtive=1,     //负方向落体
-
-};
-class FallModel {
-public:
-    FallModel();
-    static  bool fall(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud,pcl::PointCloud<pcl::PointXYZ>::Ptr& out,
-                     pcl::PointXYZ& tp,float& theta,FallDirect direct);
-
-    static pcl::PointCloud<pcl::PointXYZ>::Ptr transformCloud(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud,
-                                                              pcl::PointXYZ center,float dx,float theta);
-protected:
-    static void computeForce(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud,pcl::PointXYZ& center,
-                        pcl::PointXYZ& force_center,float& gravy,float& force,float minx,FallDirect direct);
-
-
-};
-
-#endif //CERES_TEST_FALLMODEL_H

detect/ceres_detect/point_tool.cpp

@@ -0,0 +1,299 @@
+//
+// Created by zx on 2021/5/11.
+//
+
+#include "point_tool.h"
+
+void rotation_matrix_to_rpy()
+{
+    //2#  92.0084   112.664  179.263
+    //3#  85.2597  73.8993  -1.70915
+    //4#  91.93   112.146   -179.65
+    //5#  32.9435  67.8446  -52.9897
+    //6#  124.098  119.42  -150.147
+    //8#  37.3443  111.887  130.948
+
+    /*
+    mat_r00:0.0229183
+    mat_r01:-0.9985044
+    mat_r02:0.0496393
+    mat_r03:5287.829
+    mat_r10:0.2763783
+    mat_r11:0.0540452
+    mat_r12:0.9595283
+    mat_r13:-2399.535
+    mat_r20:-0.9607757
+    mat_r21:-0.0082715
+    mat_r22:0.2772034
+    mat_r23:6674.29500*/
+    Eigen::Matrix3d mat;
+    mat<<0.0229183,
+            -0.9985044,
+    0.0496393,
+            0.2763783,
+    0.0540452,
+    0.9595283,
+            -0.9607757,
+                    -0.0082715,
+    0.2772034;
+    Eigen::Vector3d euler_angles = mat.eulerAngles(2, 1, 0);
+    std::cout<<euler_angles*180.0/M_PI<<std::endl;
+}
+
+void rpy_to_rotation_matrix()
+{
+    //
+    float ea[3]={-172,105.42,99.38};
+    Eigen::Matrix3d rotation_matrix3;
+    rotation_matrix3 = Eigen::AngleAxisd(ea[2]*M_PI/180.0, Eigen::Vector3d::UnitZ()) *
+            Eigen::AngleAxisd(ea[1]*M_PI/180.0, Eigen::Vector3d::UnitY()) *
+            Eigen::AngleAxisd(ea[0]*M_PI/180.0, Eigen::Vector3d::UnitX());
+    std::cout << "rotation matrix3 =\n" << rotation_matrix3 << std::endl;
+}
+
+void reverse_test(){
+    Eigen::Matrix3d mat;
+    mat<<-0.2963634,0.9547403,-0.0252988,-0.2261306,-0.0958803,-0.9693665,-0.9279189,-0.2815638,0.2443113;
+    Eigen::Vector3d euler_angles = mat.eulerAngles(2, 1, 0);
+    std::cout<<euler_angles*180.0/M_PI<<std::endl;
+    Eigen::Matrix3d rotation_matrix3;
+    rotation_matrix3 = Eigen::AngleAxisd(euler_angles[0], Eigen::Vector3d::UnitZ()) *
+            Eigen::AngleAxisd(euler_angles[1], Eigen::Vector3d::UnitY()) *
+            Eigen::AngleAxisd(euler_angles[2], Eigen::Vector3d::UnitX());
+    std::cout << "origin matrix3 =\n" << mat << std::endl;
+    std::cout << "rotation matrix3 =\n" << rotation_matrix3 << std::endl;
+}
+
+bool string2point(std::string str,pcl::PointXYZ& point)
+{
+    std::istringstream iss;
+    iss.str(str);
+    float value;
+    float data[3]={0};
+    for(int i=0;i<3;++i)
+    {
+        if(iss>>value)
+        {
+            data[i]=value;
+        }
+        else
+        {
+            return false;
+        }
+    }
+    point.x=data[0];
+    point.y=data[1];
+    point.z=data[2];
+    return true;
+}
+
+pcl::PointCloud<pcl::PointXYZ>::Ptr ReadTxtCloud(std::string file)
+{
+    std::ifstream fin(file.c_str());
+    const int line_length=255;
+    char str[line_length]={0};
+    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
+    while(fin.getline(str,line_length))
+    {
+        pcl::PointXYZ point;
+        if(string2point(str,point))
+        {
+            cloud->push_back(point);
+        }
+    }
+    return cloud;
+}
+
+
+void save_cloud_txt(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud,std::string file)
+{
+    FILE* pf=fopen(file.c_str(),"w");
+    for(int i=0;i<cloud->size();++i)
+    {
+        pcl::PointXYZ point=cloud->points[i];
+        fprintf(pf,"%.3f %.3f %.3f\n",point.x,point.y,point.z);
+    }
+    fclose(pf);
+}
+
+void save_cloud_txt(pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud,std::string file)
+{
+    FILE* pf=fopen(file.c_str(),"w");
+    for(int i=0;i<cloud->size();++i)
+    {
+        pcl::PointXYZRGB point=cloud->points[i];
+        fprintf(pf,"%.3f %.3f %.3f %d %d %d\n",point.x,point.y,point.z,point.r,point.g,point.b);
+    }
+    fclose(pf);
+}
+
+//欧式聚类*******************************************************
+std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> segmentation(pcl::PointCloud<pcl::PointXYZ>::Ptr sor_cloud)
+{
+    std::vector<pcl::PointIndices> ece_inlier;
+    pcl::search::KdTree<pcl::PointXYZ>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZ>);
+    pcl::EuclideanClusterExtraction<pcl::PointXYZ> ece;
+    ece.setInputCloud(sor_cloud);
+    ece.setClusterTolerance(0.2);
+    ece.setMinClusterSize(20);
+    ece.setMaxClusterSize(20000);
+    ece.setSearchMethod(tree);
+    ece.extract(ece_inlier);
+
+    std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr>  segmentation_clouds;
+    for (int i = 0; i < ece_inlier.size(); i++)
+    {
+        pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_copy(new pcl::PointCloud<pcl::PointXYZ>);
+        std::vector<int> ece_inlier_ext = ece_inlier[i].indices;
+        copyPointCloud(*sor_cloud, ece_inlier_ext, *cloud_copy);//按照索引提取点云数据
+        segmentation_clouds.push_back(cloud_copy);
+    }
+    return segmentation_clouds;
+}
+
+double distance(pcl::PointXYZ p1, pcl::PointXYZ p2)
+{
+    return sqrt(pow(p1.x - p2.x, 2.0) + pow(p1.y - p2.y, 2.0));
+}
+
+
+bool isRect(std::vector<pcl::PointXYZ>& points)
+{
+    if (points.size() == 4)
+    {
+        double L[3] = {0.0};
+        L[0] = distance(points[0], points[1]);
+        L[1] = distance(points[1], points[2]);
+        L[2] = distance(points[0], points[2]);
+        double max_l = L[0];
+        double l1 = L[1];
+        double l2 = L[2];
+      pcl::PointXYZ ps = points[0], pt = points[1];
+      pcl::PointXYZ pc = points[2];
+        for (int i = 1; i < 3; ++i) {
+            if (L[i] > max_l)
+            {
+                max_l = L[i];
+                l1 = L[abs(i + 1) % 3];
+                l2 = L[abs(i + 2) % 3];
+                ps = points[i % 3];
+                pt = points[(i + 1) % 3];
+                pc = points[(i + 2) % 3];
+            }
+        }
+
+        //直角边与坐标轴的夹角 <20°
+        float thresh=20.0*M_PI/180.0;
+        cv::Point2f vct(pt.x-pc.x,pt.y-pc.y);
+        float angle=atan2(vct.y,vct.x);
+        if(!(fabs(angle)<thresh || (M_PI_2-fabs(angle)<thresh)))
+        {
+            //std::cout<<" 4 wheel axis angle : "<<angle<<std::endl;
+            return false;
+        }
+
+        double cosa = (l1 * l1 + l2 * l2 - max_l * max_l) / (2.0 * l1 * l2);
+        if (fabs(cosa) >= 0.15) {
+            char description[255]={0};
+            sprintf(description,"angle cos value(%.2f) >0.13 ",cosa);
+            std::cout<<description<<std::endl;
+            return false;
+        }
+
+        float width=std::min(l1,l2);
+        float length=std::max(l1,l2);
+        if(width<1.400 || width >1.900 || length >3.300 ||length < 2.200)
+        {
+            char description[255]={0};
+            sprintf(description,"width<1400 || width >1900 || length >3300 ||length < 2200 l:%.1f,w:%.1f",length,width);
+            std::cout<<description<<std::endl;
+            return false;
+        }
+
+        double d = distance(pc, points[3]);
+        pcl::PointXYZ center1 = pcl::PointXYZ((ps.x+pt.x)*0.5,(ps.y+pt.y)*0.5,(ps.z+pt.z)*0.5);//(ps + pt) * 0.5;
+        pcl::PointXYZ center2 = pcl::PointXYZ((pc.x+points[3].x)*0.5,(pc.y+points[3].y)*0.5,(pc.z+points[3].z)*0.5);//(pc + points[3]) * 0.5;
+        if (fabs(d - max_l) > max_l * 0.1 || distance(center1, center2) > 0.150) {
+            std::cout << "d:" << d << " maxl:" << max_l << "  center1:" << center1 << "  center2:" << center2<<std::endl;
+            char description[255]={0};
+            sprintf(description,"Verify failed-4  fabs(d - max_l) > max_l * 0.1 || distance(center1, center2) > 0.150 ");
+            std::cout<<description<<std::endl;
+            return false;
+        }
+        //std::cout << "d:" << d << " maxl:" << max_l << "  center1:" << center1 << "  center2:" << center2<<std::endl;
+
+
+        return true;
+    }
+    else if(points.size()==3)
+    {
+        double L[3] = {0.0};
+        L[0] = distance(points[0], points[1]);
+        L[1] = distance(points[1], points[2]);
+        L[2] = distance(points[0], points[2]);
+        double max_l = L[0];
+        double l1 = L[1];
+        double l2 = L[2];
+        int max_index = 0;
+      pcl::PointXYZ ps = points[0], pt = points[1];
+      pcl::PointXYZ pc = points[2];
+        for (int i = 1; i < 3; ++i) {
+            if (L[i] > max_l) {
+                max_index = i;
+                max_l = L[i];
+                l1 = L[abs(i + 1) % 3];
+                l2 = L[abs(i + 2) % 3];
+                ps = points[i % 3];
+                pt = points[(i + 1) % 3];
+                pc = points[(i + 2) % 3];
+            }
+        }
+
+        //直角边与坐标轴的夹角 <20°
+        float thresh=20.0*M_PI/180.0;
+        pcl::PointXYZ vct(pt.x-pc.x,pt.y-pc.y,pt.z-pc.z);
+        float angle=atan2(vct.y,vct.x);
+        if(!(fabs(angle)<thresh || (M_PI_2-fabs(angle)<thresh)))
+        {
+            std::cout<<" 4 wheel axis angle : "<<angle<<std::endl;
+            return false;
+        }
+
+        double cosa = (l1 * l1 + l2 * l2 - max_l * max_l) / (2.0 * l1 * l2);
+        if (fabs(cosa) >= 0.15) {
+            /*char description[255]={0};
+            sprintf(description,"3 wheels angle cos value(%.2f) >0.13 ",cosa);
+            std::cout<<description<<std::endl;*/
+            return false;
+        }
+
+        double l=std::max(l1,l2);
+        double w=std::min(l1,l2);
+        if(l>2.100 && l<3.300 && w>1.400 && w<2.100)
+        {
+            //生成第四个点
+          pcl::PointXYZ vec1=pcl::PointXYZ(ps.x-pc.x,ps.y-pc.y,ps.z-pc.z);
+          pcl::PointXYZ vec2=pcl::PointXYZ(pt.x-pc.x,pt.y-pc.y,pt.z-pc.z);
+          pcl::PointXYZ point4=pcl::PointXYZ(vec1.x+vec2.x+pc.x,vec1.y+vec2.y+pc.y,vec1.z+vec2.z+pc.z);//(vec1+vec2)+pc;
+            points.push_back(point4);
+
+            char description[255]={0};
+            sprintf(description,"3 wheels rectangle cos angle=%.2f,L=%.1f, w=%.1f",cosa,l,w);
+            std::cout<<description<<std::endl;
+            return true;
+        }
+        else
+        {
+
+            char description[255]={0};
+            sprintf(description,"3 wheels rectangle verify Failed  cos angle=%.2f,L=%.1f, w=%.1f",cosa,l,w);
+            std::cout<<description<<std::endl;
+            return false;
+        }
+
+    }
+    //std::cout<<" default false"<<std::endl;
+    return false;
+
+}
+

detect/ceres_detect/point_tool.h

@@ -0,0 +1,31 @@
+//
+// Created by zx on 2021/5/11.
+//
+
+#ifndef ROBOT_CONTROL_FEATURE_EXTRA_SRC_POINT_TOOL_HPP_
+#define ROBOT_CONTROL_FEATURE_EXTRA_SRC_POINT_TOOL_HPP_
+#include <iostream>
+#include <pcl/point_types.h>
+#include <pcl/point_cloud.h>
+#include <pcl/io/pcd_io.h>
+#include <pcl/segmentation/sac_segmentation.h>
+#include <pcl/ModelCoefficients.h>
+#include <pcl/filters/extract_indices.h>
+#include <pcl/segmentation/extract_clusters.h>
+#include <opencv2/opencv.hpp>
+
+bool string2point(std::string str,pcl::PointXYZ& point);
+pcl::PointCloud<pcl::PointXYZ>::Ptr ReadTxtCloud(std::string file);
+
+void save_cloud_txt(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud,std::string file);
+void save_cloud_txt(pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud,std::string file);
+
+double distance(cv::Point2f p1, cv::Point2f p2);
+bool isRect(std::vector<pcl::PointXYZ>& points);
+std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> segmentation(pcl::PointCloud<pcl::PointXYZ>::Ptr sor_cloud);
+
+
+void rpy_to_rotation_matrix();
+void rotation_matrix_to_rpy();
+void reverse_test();
+#endif //ROBOT_CONTROL_FEATURE_EXTRA_SRC_POINT_TOOL_HPP_

detect/detect_manager.cpp

@@ -6,10 +6,16 @@
 
 Error_manager DetectManager::Init(const google::protobuf::RepeatedPtrField<tof3dVzenseEtc> &tof_devices) {
     for (int device_index = 0; device_index < DeviceAzimuth::DEVICE_AZIMUTH_MAX; device_index++) {
-        TofTransformationManager::iter()->set((DeviceAzimuth)device_index, tof_devices[device_index].trans());
+        TofTransformationManager::iter()->set((DeviceAzimuth) device_index, tof_devices[device_index].trans());
         updateTrans((DeviceAzimuth) device_index, tof_devices[device_index].trans());
     }
 
+#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
+
     m_thread = new std::thread(&DetectManager::run, this);
     m_condit.notify_all(true);
 
@@ -38,20 +44,20 @@ Error_manager DetectManager::updateTrans(const DeviceAzimuth &azimuth, const Coo
 
 #include "tool/time.hpp"
 
-bool boxIsValid(const JetStream::SegBox& box, int min ,bool reverse=false){
-    if(box.confidence()<0.8){
+bool boxIsValid(const JetStream::SegBox &box, int min, bool reverse = false) {
+    if (box.confidence() < 0.8) {
         return false;
     }
     int left_side = box.x();
-    if(reverse){
-        left_side=640-(box.x()+box.width());
+    if (reverse) {
+        left_side = 640 - (box.x() + box.width());
     }
 
     return left_side > min;
 }
 
 int IsInImageValidRegion(const JetStream::LabelYolo &labels) {
-    if(labels.boxes_size()!=4){
+    if (labels.boxes_size() != 4) {
         return -1; //没有识别框
     }
 
@@ -59,30 +65,30 @@ int IsInImageValidRegion(const JetStream::LabelYolo &labels) {
         // LOG(INFO) << device_index << ": "<< labels.boxes(device_index).confidence() << " " << labels.boxes(device_index).x();
     }
 
-    JetStream::SegBox box2=labels.boxes(1);
-    JetStream::SegBox box4=labels.boxes(3);
+    JetStream::SegBox box2 = labels.boxes(1);
+    JetStream::SegBox box4 = labels.boxes(3);
     bool frontExsit = labels.boxes(0).confidence() > 0.8 || labels.boxes(1).confidence() > 0.8;
     bool backExist = labels.boxes(2).confidence() > 0.8 || labels.boxes(3).confidence() > 0.8;
-    if(!frontExsit){
-        if(backExist){  //后有前没有
+    if (!frontExsit) {
+        if (backExist) {  //后有前没有
             return 1;  //往前
-        }else{  //前后都没有
+        } else {  //前后都没有
             return -1;
         }
-    }else{
-        if(backExist){
-           bool front_OK = boxIsValid(labels.boxes(1), 20) || boxIsValid(labels.boxes(0), 20,true);
-            bool back_OK = boxIsValid(labels.boxes(2), 20,true) || boxIsValid(labels.boxes(3), 20);
-            if(front_OK && back_OK){
+    } else {
+        if (backExist) {
+            bool front_OK = boxIsValid(labels.boxes(1), 20) || boxIsValid(labels.boxes(0), 20, true);
+            bool back_OK = boxIsValid(labels.boxes(2), 20, true) || boxIsValid(labels.boxes(3), 20);
+            if (front_OK && back_OK) {
                 return 0;
-            }else{
+            } else {
                 return 1;
             }
-        }else{  // 前有后没有
-            bool front_OK=boxIsValid(labels.boxes(1),320) || boxIsValid(labels.boxes(0),320,true);
-            if(!front_OK){
+        } else {  // 前有后没有
+            bool front_OK = boxIsValid(labels.boxes(1), 320) || boxIsValid(labels.boxes(0), 320, true);
+            if (!front_OK) {
                 return 1;
-            }else{
+            } else {
                 return -1;
             }
         }
@@ -94,8 +100,10 @@ void DetectManager::run() {
 
     DetectResult m_detect_result_record;
     std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> vct_wheel_cloud;
+    std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> vct_tires_cloud;
     for (int i = 0; i < DeviceAzimuth::DEVICE_AZIMUTH_MAX; i++) {
         vct_wheel_cloud.emplace_back(new pcl::PointCloud<pcl::PointXYZ>);
+        vct_tires_cloud.emplace_back(new pcl::PointCloud<pcl::PointXYZ>);
     }
 
     auto t_start_time = std::chrono::steady_clock::now();
@@ -105,23 +113,36 @@ void DetectManager::run() {
         m_condit.wait();
         // 参数初始化
         cost = std::chrono::steady_clock::now() - t_start_time;
-        std::this_thread::sleep_for(std::chrono::milliseconds(50 - std::min<int>(49, cost.count() * 1000)));
+        DLOG(INFO) << "last while cost time is " << cost.count() * 1000 << " ms";
+        std::this_thread::sleep_for(std::chrono::milliseconds(100 - std::min<int>(49, cost.count() * 1000)));
         t_start_time = std::chrono::steady_clock::now();
 
         if (m_condit.is_alive()) {
             std::vector<int> have_cloud;
             DetectResult detect_result;
-            JetStream::LabelYolo seg_results;
             for (int i = 0; i < DeviceAzimuth::DEVICE_AZIMUTH_MAX; i++) {
                 vct_wheel_cloud[i]->clear();
+                vct_tires_cloud[i]->clear();
             }
-            if (!LabelYoloDataBuffer::iter()->lock_exe(&LabelYoloDataBuffer::get, seg_results, 10)) {
-                continue;
+
+            cv::Mat merge_mat = cv::Mat::zeros(480 * 2, 640 * 2, CV_8UC1);
+            DeviceTof3D::DeviceTof3DSaveInfo devices_info[DeviceAzimuth::DEVICE_AZIMUTH_MAX];
+            for (int device_index = 0; device_index < DeviceAzimuth::DEVICE_AZIMUTH_MAX; device_index++) {
+                devices_info[device_index] = DeviceTof3D::iter()->getDeviceSaveInfo((DeviceAzimuth) device_index);
+                devices_info[device_index].irMat.copyTo(merge_mat(cv::Rect((device_index & 0x1) * 640,
+                                                                           ((device_index & 0x2) >> 1) * 480, 640, 480)));
             }
+
+            cost = std::chrono::steady_clock::now() - t_start_time;
+            DLOG(INFO) << "cost time is " << cost.count() * 1000 << " ms";
+            JetStream::LabelYolo seg_results;
+            YoloSegMergeData(merge_mat, seg_results);
+            cost = std::chrono::steady_clock::now() - t_start_time;
+            DLOG(INFO) << "cost time is " << cost.count() * 1000 << " ms";
+
             // 判断识别结果数量以及是否存图
             int seg_num = 0;
             for (int device_index = 0; device_index < DeviceAzimuth::DEVICE_AZIMUTH_MAX; device_index++) {
-                // LOG(INFO) << device_index << ": " << seg_results.boxes(device_index).confidence();
                 if (seg_results.boxes(device_index).confidence() > 0.7) {
                     seg_num++;
                 }
@@ -131,133 +152,140 @@ void DetectManager::run() {
                 m_detect_result_record.reset();
                 continue;
             }
+
             // 取高置信度识别结果
             for (int device_index = 0; device_index < DeviceAzimuth::DEVICE_AZIMUTH_MAX; device_index++) {
                 if (seg_results.boxes(device_index).confidence() < 0.9) {
-                    DLOG(INFO) << device_index << " empty";
                     continue;
                 }
-                cv::Mat pointMat;
-                //LOG(INFO) << "b get data" << " empty";
-                if (MatDataBuffer::iter()->lock_exe(&MatDataBuffer::get, device_index, seg_results.label(), pointMat,
-                                                    10)) {
-                    have_cloud.push_back(device_index);
-                    std::vector<cv::Point> seg_points;
-                    for (int row = 0; row < 480; row++) {
-                        for (int col = seg_results.boxes(device_index).lines(row).begin();
-                             col < seg_results.boxes(device_index).lines(row).end(); col++) {
-                            seg_points.emplace_back(col, row);
-                            vct_wheel_cloud[device_index]->emplace_back(pointMat.at<cv::Vec3f>(row, col)[0] * 0.001,
-                                                                        pointMat.at<cv::Vec3f>(row, col)[1] * 0.001,
-                                                                        pointMat.at<cv::Vec3f>(row, col)[2] * 0.001);
-                        }
-                    }
-                    TransitData::get_instance_pointer()->setMask(seg_points, device_index);
-                    CoordinateTransformation3D device_trans;
-                    // 更新旋转坐标
-                    if (TofTransformationManager::iter()->get((DeviceAzimuth)device_index, device_trans)) {
-                        updateTrans((DeviceAzimuth)device_index, device_trans);
-                    }
-                    pcl::transformPointCloud(*vct_wheel_cloud[device_index], *vct_wheel_cloud[device_index],
-                                             rotation_matrix4[device_index]());
-                    // Point3D_tool::WriteTxtCloud(ETC_PATH PROJECT_NAME "/data/cloud_" + std::to_string(device_index) + ".txt", vct_wheel_cloud[device_index]);
+                cv::Mat pointMat = devices_info[device_index].pointMat;
+                have_cloud.push_back(device_index);
+                std::vector<cv::Point> seg_points;
+                for (int i = 0; i < seg_results.boxes(device_index).wheels_size(); i++) {
+                    int index = seg_results.boxes(device_index).wheels(i).point();
+                    int col = index % 640;
+                    int row = index / 640;
+                    seg_points.emplace_back(col, row);
+                    vct_wheel_cloud[device_index]->emplace_back(pointMat.at<cv::Vec4f>(row, col)[0],
+                                                                pointMat.at<cv::Vec4f>(row, col)[1],
+                                                                pointMat.at<cv::Vec4f>(row, col)[2]);
                 }
+                for (int i = 0; i < seg_results.boxes(device_index).tires_size(); i++) {
+                    int index = seg_results.boxes(device_index).tires(i).point();
+                    int col = index % 640;
+                    int row = index / 640;
+                    seg_points.emplace_back(col, row);
+                    vct_tires_cloud[device_index]->emplace_back(pointMat.at<cv::Vec4f>(row, col)[0],
+                                                                pointMat.at<cv::Vec4f>(row, col)[1],
+                                                                pointMat.at<cv::Vec4f>(row, col)[2]);
+                }
+                TransitData::get_instance_pointer()->setMask(seg_points, device_index);
             }
+            cost = std::chrono::steady_clock::now() - t_start_time;
+            DLOG(INFO) << "cost time is " << cost.count() * 1000 << " ms";
+
+            // 判断当前识别状态是否需要进行测量
             int flag = IsInImageValidRegion(seg_results);
             if (flag == 1) {
-                LOG(INFO) << "向前";
                 detect_statu = MeasureStatu::Measure_Border;
                 continue;
-            } else if (flag == -1){
-                LOG(WARNING) << "缺少轮子";
+            } else if (flag == -1) {
+                detect_statu = MeasureStatu::Measure_Failture;
                 continue;
             }
 
-            // 点云处理
-            if (!have_cloud.empty()) {
-                WheelCloudOptimize(vct_wheel_cloud, detect_result);
-                cost = std::chrono::steady_clock::now() - t_start_time;
-                DLOG(INFO) << "cost time is " << cost.count() * 1000 << " ms";
-
-                bool ceres_detect_ret = false;
-                auto wheel_detect = detect_wheel_ceres::iter();
-                // 采用上次测量结果的数据
-                if (!m_detect_result_time_lock_data.timeout(0.5)) {
-                    detect_result = m_detect_result_time_lock_data();
-                    detect_result.car.wheel_width = MIN(detect_result.car.wheel_width + 0.4, 2.5);
-                } else {
-                    detect_result.car.wheel_width = 3;
-                    detect_result.car.wheel_base = 2.7;
-                }
-
-                // 根据车轮数采用不同的优化方式
-                if (m_detect_result_record.car.wheel_width < 1.0 || m_detect_result_record.car.wheel_base < 2.0 ||
-                    have_cloud.size() > 2) {
-                    ceres_detect_ret = wheel_detect->detect_2Step(vct_wheel_cloud[DeviceAzimuth::DEVICE_AZIMUTH_LF],
-                                                                  vct_wheel_cloud[DeviceAzimuth::DEVICE_AZIMUTH_RF],
-                                                                  vct_wheel_cloud[DeviceAzimuth::DEVICE_AZIMUTH_LR],
-                                                                  vct_wheel_cloud[DeviceAzimuth::DEVICE_AZIMUTH_RR],
-                                                                  detect_result.car.wheels_center_x,
-                                                                  detect_result.car.wheels_center_y,
-                                                                  detect_result.car.theta, detect_result.car.wheel_base,
-                                                                  detect_result.car.wheel_width,
-                                                                  detect_result.car.front_wheels_theta,
-                                                                  detect_result.car.loss);
-                    if ((m_detect_result_record.car.wheel_width < 1.0 && m_detect_result_record.car.wheel_base < 1.0 &&
-                         have_cloud.size() == 2) || have_cloud.size() < 2) {
-                        detect_result.car.wheel_width = m_detect_result_record.car.wheel_width;
-                        detect_result.car.wheel_base = m_detect_result_record.car.wheel_base;
-                    } else if (have_cloud.size() == 2) {
-                        for (int device_index = 0; device_index < DeviceAzimuth::DEVICE_AZIMUTH_MAX; device_index++) {
-                            if (!vct_wheel_cloud[device_index]->empty()) {
-                                have_cloud.push_back(device_index);
-                            }
-                        }
+            // 下采样点云
+            WheelCloudOptimize(vct_wheel_cloud, detect_result);
+            WheelCloudOptimize(vct_tires_cloud, detect_result);
+            cost = std::chrono::steady_clock::now() - t_start_time;
+            DLOG(INFO) << "cost time is " << cost.count() * 1000 << " ms";
 
-                        if (have_cloud[0] % 2 == have_cloud[1] % 2) {
-                            detect_result.car.wheel_width = m_detect_result_record.car.wheel_width;
-                        } else {
-                            detect_result.car.wheel_base = m_detect_result_record.car.wheel_base;
+            bool ceres_detect_ret = false;
+            auto wheel_detect = detect_wheel_ceres::iter();
+            // 轮毂平面检测
+            Car_pose_detector::CarDetectResult wheel_ransic_ret;
+            wheel_detect->detect_ransic(vct_wheel_cloud[DeviceAzimuth::DEVICE_AZIMUTH_LF], vct_wheel_cloud[DeviceAzimuth::DEVICE_AZIMUTH_RF],
+                                        vct_wheel_cloud[DeviceAzimuth::DEVICE_AZIMUTH_LR], vct_wheel_cloud[DeviceAzimuth::DEVICE_AZIMUTH_RR],
+                                        wheel_ransic_ret.wheels_center_x, wheel_ransic_ret.wheels_center_y,
+                                        wheel_ransic_ret.theta, wheel_ransic_ret.wheel_base, wheel_ransic_ret.wheel_width,
+                                        wheel_ransic_ret.front_wheels_theta, wheel_ransic_ret.loss);
+            // 轮胎平面检测
+            Car_pose_detector::CarDetectResult tires_ransic_ret;
+            wheel_detect->detect_ransic(vct_tires_cloud[DeviceAzimuth::DEVICE_AZIMUTH_LF], vct_tires_cloud[DeviceAzimuth::DEVICE_AZIMUTH_RF],
+                                        vct_tires_cloud[DeviceAzimuth::DEVICE_AZIMUTH_LR], vct_tires_cloud[DeviceAzimuth::DEVICE_AZIMUTH_RR],
+                                        tires_ransic_ret.wheels_center_x, tires_ransic_ret.wheels_center_y,
+                                        tires_ransic_ret.theta, tires_ransic_ret.wheel_base, tires_ransic_ret.wheel_width,
+                                        tires_ransic_ret.front_wheels_theta, tires_ransic_ret.loss);
+
+            // 双模型检测：采用上次测量结果的数据
+            Car_pose_detector::CarDetectResult wheel_2step_ret;
+            if (!m_detect_result_time_lock_data.timeout(0.5)) {
+                wheel_2step_ret = m_detect_result_time_lock_data().car;
+                wheel_2step_ret.wheel_width = MIN(wheel_2step_ret.wheel_width + 0.4, 2.5);
+            } else {
+                wheel_2step_ret.wheel_width = 3;
+                wheel_2step_ret.wheel_base = 2.7;
+            }
+            if (m_detect_result_record.car.wheel_width < 1.0 || m_detect_result_record.car.wheel_base < 2.0 ||
+                have_cloud.size() > 2) {
+                ceres_detect_ret = wheel_detect->detect_2Step(vct_wheel_cloud[DeviceAzimuth::DEVICE_AZIMUTH_LF],
+                                                              vct_wheel_cloud[DeviceAzimuth::DEVICE_AZIMUTH_RF],
+                                                              vct_wheel_cloud[DeviceAzimuth::DEVICE_AZIMUTH_LR],
+                                                              vct_wheel_cloud[DeviceAzimuth::DEVICE_AZIMUTH_RR],
+                                                              wheel_2step_ret.wheels_center_x, wheel_2step_ret.wheels_center_y,
+                                                              wheel_2step_ret.theta, wheel_2step_ret.wheel_base,
+                                                              wheel_2step_ret.wheel_width,
+                                                              wheel_2step_ret.front_wheels_theta, wheel_2step_ret.loss);
+                if ((m_detect_result_record.car.wheel_width < 1.0 && m_detect_result_record.car.wheel_base < 1.0 &&
+                     have_cloud.size() == 2) || have_cloud.size() < 2) {
+                    wheel_2step_ret.wheel_width = m_detect_result_record.car.wheel_width;
+                    wheel_2step_ret.wheel_base = m_detect_result_record.car.wheel_base;
+                } else if (have_cloud.size() == 2) {
+                    for (int device_index = 0; device_index < DeviceAzimuth::DEVICE_AZIMUTH_MAX; device_index++) {
+                        if (!vct_wheel_cloud[device_index]->empty()) {
+                            have_cloud.push_back(device_index);
                         }
                     }
-                } else {
-                    ceres_detect_ret = wheel_detect->detect_dynP(vct_wheel_cloud[DeviceAzimuth::DEVICE_AZIMUTH_LF],
-                                                                 vct_wheel_cloud[DeviceAzimuth::DEVICE_AZIMUTH_RF],
-                                                                 vct_wheel_cloud[DeviceAzimuth::DEVICE_AZIMUTH_LR],
-                                                                 vct_wheel_cloud[DeviceAzimuth::DEVICE_AZIMUTH_RR],
-                                                                 detect_result.car.wheels_center_x,
-                                                                 detect_result.car.wheels_center_y,
-                                                                 detect_result.car.theta,
-                                                                 m_detect_result_record.car.wheel_base,
-                                                                 m_detect_result_record.car.wheel_width,
-                                                                 detect_result.car.front_wheels_theta,
-                                                                 detect_result.car.loss);
-                    detect_result.car.wheel_base = m_detect_result_record.car.wheel_base;
-                    detect_result.car.wheel_width = m_detect_result_record.car.wheel_width;
-                }
-
-                // 如果车辆运动，不断刷新运动时间，如果由运动转为静止则刷新运动状态到静止
-                float move_distance = detect_result.car.wheels_center_y - m_detect_result_record.car.wheels_center_y;
-                if (fabs(move_distance) > 0.1) {
-                    move_statu = true;
-                } else if (move_statu()) {
-                    move_statu = false;
+                    if (have_cloud[0] % 2 == have_cloud[1] % 2) {
+                        wheel_2step_ret.wheel_width = m_detect_result_record.car.wheel_width;
+                    } else {
+                        wheel_2step_ret.wheel_base = m_detect_result_record.car.wheel_base;
+                    }
                 }
+            } else {
+                ceres_detect_ret = wheel_detect->detect_dynP(vct_wheel_cloud[DeviceAzimuth::DEVICE_AZIMUTH_LF],
+                                                             vct_wheel_cloud[DeviceAzimuth::DEVICE_AZIMUTH_RF],
+                                                             vct_wheel_cloud[DeviceAzimuth::DEVICE_AZIMUTH_LR],
+                                                             vct_wheel_cloud[DeviceAzimuth::DEVICE_AZIMUTH_RR],
+                                                             wheel_2step_ret.wheels_center_x, wheel_2step_ret.wheels_center_y,
+                                                             wheel_2step_ret.theta, m_detect_result_record.car.wheel_base,
+                                                             m_detect_result_record.car.wheel_width,
+                                                             wheel_2step_ret.front_wheels_theta, wheel_2step_ret.loss);
+                wheel_2step_ret.wheel_base = m_detect_result_record.car.wheel_base;
+                wheel_2step_ret.wheel_width = m_detect_result_record.car.wheel_width;
+            }
 
-                if (ceres_detect_ret) {
-                    // 记录下测量结果
-                    m_detect_result_record = detect_result;
-                    detect_statu = Measure_OK;
-                    m_detect_result_time_lock_data = m_detect_result_record;
-                } else {
-                    detect_statu = Measure_Failture;
-                }
+            // 综合测量结果
+            if (MergeAllMeaureResult(detect_result.car, wheel_ransic_ret, tires_ransic_ret, wheel_2step_ret) == MeasureStatu::Measure_Failture) {
+                detect_statu = MeasureStatu::Measure_Failture;
+            }
 
+            // 如果车辆运动，不断刷新运动时间，如果由运动转为静止则刷新运动状态到静止
+            float move_distance = detect_result.car.wheels_center_y - m_detect_result_record.car.wheels_center_y;
+            if (fabs(move_distance) > 0.1) {
+                move_statu = true;
+            } else if (move_statu()) {
+                move_statu = false;
             }
-            cost = std::chrono::steady_clock::now() - t_start_time;
-            DLOG(INFO) << "cost time is " << cost.count() * 1000 << " ms";
-        } else {
-            cv::destroyAllWindows();
+            if (ceres_detect_ret) {
+                // 记录下测量结果
+                m_detect_result_record = detect_result;
+                detect_statu = Measure_OK;
+                m_detect_result_time_lock_data = m_detect_result_record;
+            } else {
+                detect_statu = Measure_Failture;
+            }
+
         }
     }
     LOG(INFO) << "detect thread " << std::this_thread::get_id() << " closed.";
@@ -289,11 +317,156 @@ Error_manager DetectManager::WheelCloudOptimize(std::vector<pcl::PointCloud<pcl:
     return {};
 }
 
+bool isElliptical(float x, float y, float a2, float b2) {
+    return (x * x / a2 + y * y / b2) < 1;
+}
+
+uint32_t getPixelIndex(int row, int col) {
+    // LOG(INFO) << "col " << col << ", row " << row;
+    col = MAX(0, MIN(1280, col)) % 640;
+    row = MAX(0, MIN(960, row)) % 480;
+    // LOG(INFO) << "col " << col << ", row " << row;
+    return row * 640 + col;
+}
+
+Error_manager DetectManager::YoloSegMergeData(cv::Mat &merge_mat, JetStream::LabelYolo &seg_results) {
+    auto t_start_time = std::chrono::steady_clock::now();
+    std::chrono::duration<double> cost = std::chrono::steady_clock::now() - t_start_time;
+    seg_results.add_boxes();
+    seg_results.add_boxes();
+    seg_results.add_boxes();
+    seg_results.add_boxes();
+
+    // 模型识别
+    std::vector<Object> objs;
+    cv::cvtColor(merge_mat, merge_mat, cv::COLOR_GRAY2RGB);
+    detector->detect(merge_mat, objs, merge_mat);
+    // cv::imshow("merge_mat", merge_mat);
+    // cv::waitKey(1);
+    cost = std::chrono::steady_clock::now() - t_start_time;
+    DLOG(INFO) << __FUNCTION__ << "cost time is " << cost.count() * 1000 << " ms";
+
+    // 分离识别结果
+    auto t = std::chrono::steady_clock::now();
+    for (auto iter = objs.begin(); iter != objs.end(); iter++) {
+        auto seg_points = detector->getPointsFromObj(*iter);
+        int device_index = (int(iter->rect.x / 640) * 0x01) | (int(iter->rect.y / 480) << 1);
+        // 校验识别矩形框是否越界
+        int device_index_check = (int((iter->rect.x + iter->rect.width) / 640) * 0x01) |
+                                 (int((iter->rect.y + iter->rect.height) / 480) << 1);
+        if (device_index != device_index_check) {
+            // TODO:存图
+            objs.erase(iter);
+            iter--;
+            continue;
+        }
+        int x_alpha = (device_index & 0x1);
+        int y_alpha = ((device_index & 0x2) >> 1);
+        seg_results.mutable_boxes(device_index)->set_x(iter->rect.x - x_alpha * merge_mat.cols / 2);
+        seg_results.mutable_boxes(device_index)->set_y(iter->rect.y - y_alpha * merge_mat.rows / 2);
+        seg_results.mutable_boxes(device_index)->set_width(iter->rect.width);
+        seg_results.mutable_boxes(device_index)->set_height(iter->rect.height);
+        seg_results.mutable_boxes(device_index)->set_confidence(iter->prob);
+        if (iter->prob > 0.6) {
+            // 内外椭圆参数
+            float a1 = iter->rect.width * 0.5;
+            float b1 = iter->rect.height * 0.5;
+            float a2 = iter->rect.width * 0.57;
+            float b2 = iter->rect.height * 0.57;
+
+            //
+            int center_x = iter->rect.x + iter->rect.width * 0.5;
+            int center_y = iter->rect.y + iter->rect.height * 0.5;
+            int min_x = MAX(0, center_x - a2);
+            int min_y = MAX(0, center_y - b2);
+            float a12 = a1 * a1;
+            float b12 = b1 * b1;
+            float a22 = a2 * a2;
+            float b22 = b2 * b2;
+
+            for (int x_value = min_x; x_value < center_x; x_value++) {
+                auto t_x_value = x_value - center_x;
+                for (int y_value = min_y; y_value < center_y; y_value++) {
+                    auto t_y_value = y_value - center_y;
+
+                    if (!isElliptical(t_x_value, t_y_value, a22, b22)) {
+                        continue;
+                    }
+                    auto x_value_sym = 2 * center_x - x_value;
+                    auto y_value_sym = 2 * center_y - y_value;
+                    if (isElliptical(t_x_value, t_y_value, a12, b12)) {
+                        seg_results.mutable_boxes(device_index)->add_wheels()->set_point(getPixelIndex(y_value, x_value));
+                        seg_results.mutable_boxes(device_index)->add_wheels()->set_point(getPixelIndex(y_value, x_value_sym));
+                        seg_results.mutable_boxes(device_index)->add_wheels()->set_point(getPixelIndex(y_value_sym, x_value));
+                        seg_results.mutable_boxes(device_index)->add_wheels()->set_point(getPixelIndex(y_value_sym, x_value_sym));
+                        continue;
+                    }
+                    seg_results.mutable_boxes(device_index)->add_tires()->set_point(getPixelIndex(y_value, x_value));
+                    seg_results.mutable_boxes(device_index)->add_tires()->set_point(getPixelIndex(y_value, x_value_sym));
+                    seg_results.mutable_boxes(device_index)->add_tires()->set_point(getPixelIndex(y_value_sym, x_value));
+                    seg_results.mutable_boxes(device_index)->add_tires()->set_point(getPixelIndex(y_value_sym, x_value_sym));
+                }
+            }
+        }
+    }
+    cost = std::chrono::steady_clock::now() - t_start_time;
+    DLOG(INFO) << __FUNCTION__ << "cost time is " << cost.count() * 1000 << " ms";
+    return {};
+}
+
+bool isCarDetectResultNearly(Car_pose_detector::CarDetectResult &a, Car_pose_detector::CarDetectResult &b, float x = 0.015, float y = 0.025, float w = 0.025, float wb = 0.02, float tt = 0.7) {
+    bool wheels_center_x = fabs(a.wheels_center_x - b.wheels_center_x) < x * 2 ? true : false;
+    bool wheels_center_y = fabs(a.wheels_center_y - b.wheels_center_y) < y * 2 ? true : false;
+    bool wheel_width = fabs(a.wheel_width - b.wheel_width) < w * 2 ? true : false;
+    bool wheel_base = fabs(a.wheel_base - b.wheel_base) < wb * 2 ? true : false;
+    bool theta = fabs(a.theta - b.theta) * 180 / M_PI < tt * 2 ? true : false;
+    return wheels_center_x && wheels_center_y && wheel_width && wheel_base && theta;
+}
+
+MeasureStatu DetectManager::MergeAllMeaureResult(Car_pose_detector::CarDetectResult &merge_result,
+                                                 Car_pose_detector::CarDetectResult &wheel_ransic,
+                                                 Car_pose_detector::CarDetectResult &tires_ransic,
+                                                 Car_pose_detector::CarDetectResult &wheel_2step) {
+    if (fabs(wheel_ransic.theta - tires_ransic.theta) * 180.0 / M_PI < 0.7) {
+        merge_result.wheels_center_x = tires_ransic.wheels_center_x;
+        merge_result.wheels_center_y = tires_ransic.wheels_center_y;
+        merge_result.wheel_width = wheel_2step.wheel_width;
+        merge_result.wheel_base = wheel_2step.wheel_base;
+        merge_result.theta = tires_ransic.theta;
+        merge_result.front_wheels_theta = tires_ransic.front_wheels_theta;
+    } else  {
+        if (isCarDetectResultNearly(wheel_2step, tires_ransic)) {
+            merge_result.wheels_center_x = (tires_ransic.wheels_center_x + wheel_2step.wheels_center_x) * 0.5;
+            merge_result.wheels_center_y = (tires_ransic.wheels_center_y + wheel_2step.wheels_center_y) * 0.5;
+            merge_result.wheel_width = tires_ransic.wheel_width * 0.3 + wheel_2step.wheels_center_y * 0.7;
+            merge_result.wheel_base = (tires_ransic.wheel_base + wheel_2step.wheel_base) * 0.5;
+            merge_result.theta = tires_ransic.theta;
+            merge_result.front_wheels_theta = tires_ransic.front_wheels_theta;
+        } else if (isCarDetectResultNearly(wheel_2step, wheel_ransic)) {
+            merge_result.wheels_center_x = (wheel_ransic.wheels_center_x + wheel_2step.wheels_center_x) * 0.5;
+            merge_result.wheels_center_y = (wheel_ransic.wheels_center_y + wheel_2step.wheels_center_y) * 0.5;
+            merge_result.wheel_width = wheel_ransic.wheel_width * 0.3 + wheel_2step.wheels_center_y * 0.7;
+            merge_result.wheel_base = (wheel_ransic.wheel_base + wheel_2step.wheel_base) * 0.5;
+            merge_result.theta = wheel_ransic.theta;
+            merge_result.front_wheels_theta = wheel_ransic.front_wheels_theta;
+        } else {
+            return MeasureStatu::Measure_Failture;
+        }
+    }
+
+
+    LOG(INFO) << wheel_ransic.info();
+    LOG(INFO) << tires_ransic.info();
+    LOG(INFO) << wheel_2step.info();
+    LOG(INFO) << merge_result.info();
+    return MeasureStatu::Measure_OK;
+}
+
 MeasureStatu DetectManager::getMeasureResult(DetectManager::DetectResult &result) {
     if (detect_statu() != MeasureStatu::Measure_OK) {
         return detect_statu();
     }
-    if (m_detect_result_time_lock_data.timeout()) {
+    if (m_detect_result_time_lock_data.timeout(0.3)) {
         return MeasureStatu::Measure_Failture;
     }
     result = m_detect_result_time_lock_data();

detect/detect_manager.h

@@ -18,6 +18,12 @@
 #include "communication/transitData.h"
 #include "proto/def.grpc.pb.h"
 
+#ifdef ENABLE_TENSORRT_DETECT
+#include "tensorrt_detect/wheel-detector.h"
+#else
+#include "onnx_detect/wheel-detector.h"
+#endif
+
 class DetectManager {
 public:
     struct DetectResult {
@@ -83,9 +89,16 @@ protected:
     // 车轮点云优化
     Error_manager WheelCloudOptimize(std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> &vct_wheel_cloud, DetectResult &result);
 
+    Error_manager YoloSegMergeData(cv::Mat &merge_mat, JetStream::LabelYolo &seg_results);
+
+    MeasureStatu MergeAllMeaureResult(Car_pose_detector::CarDetectResult &merge_result,
+                                      Car_pose_detector::CarDetectResult &wheel_ransic,
+                                      Car_pose_detector::CarDetectResult &tires_ransic,
+                                      Car_pose_detector::CarDetectResult &wheel_2step);
 private:
     std::thread *m_thread;
     Thread_condition m_condit;
+    TensorrtWheelDetector *detector = nullptr;
 
     TimedLockData<bool> move_statu;
     TimedLockData<MeasureStatu> detect_statu;

detect/onnx_detect/inference.cpp

@@ -0,0 +1,183 @@
+#include "inference.h"
+
+Inference::Inference(const std::string &onnxModelPath, const cv::Size &modelInputShape, const std::string &classesTxtFile, const bool &runWithCuda)
+{
+    modelPath = onnxModelPath;
+    modelShape = modelInputShape;
+    classesPath = classesTxtFile;
+    cudaEnabled = runWithCuda;
+
+    loadOnnxNetwork();
+    // loadClassesFromFile(); The classes are hard-coded for this example
+}
+
+std::vector<Detection> Inference::runInference(const cv::Mat &input)
+{
+    cv::Mat modelInput = input;
+    if (letterBoxForSquare && modelShape.width == modelShape.height)
+        modelInput = formatToSquare(modelInput);
+
+    cv::Mat blob;
+    cv::dnn::blobFromImage(modelInput, blob, 1.0/255.0, modelShape, cv::Scalar(), true, false);
+    net.setInput(blob);
+
+    std::vector<cv::Mat> outputs;
+    net.forward(outputs, net.getUnconnectedOutLayersNames());
+
+    int rows = outputs[0].size[1];
+    int dimensions = outputs[0].size[2];
+
+    bool yolov8 = false;
+    // yolov5 has an output of shape (batchSize, 25200, 85) (Num classes + box[x,y,w,h] + confidence[c])
+    // yolov8 has an output of shape (batchSize, 84,  8400) (Num classes + box[x,y,w,h])
+    if (dimensions > rows) // Check if the shape[2] is more than shape[1] (yolov8)
+    {
+        yolov8 = true;
+        rows = outputs[0].size[2];
+        dimensions = outputs[0].size[1];
+
+        outputs[0] = outputs[0].reshape(1, dimensions);
+        cv::transpose(outputs[0], outputs[0]);
+    }
+    float *data = (float *)outputs[0].data;
+
+    float x_factor = modelInput.cols / modelShape.width;
+    float y_factor = modelInput.rows / modelShape.height;
+
+    std::vector<int> class_ids;
+    std::vector<float> confidences;
+    std::vector<cv::Rect> boxes;
+
+    for (int i = 0; i < rows; ++i)
+    {
+        if (yolov8)
+        {
+            float *classes_scores = data+4;
+
+            cv::Mat scores(1, classes.size(), CV_32FC1, classes_scores);
+            cv::Point class_id;
+            double maxClassScore;
+
+            minMaxLoc(scores, 0, &maxClassScore, 0, &class_id);
+
+            if (maxClassScore > modelScoreThreshold)
+            {
+                confidences.push_back(maxClassScore);
+                class_ids.push_back(class_id.x);
+
+                float x = data[0];
+                float y = data[1];
+                float w = data[2];
+                float h = data[3];
+
+                int left = int((x - 0.5 * w) * x_factor);
+                int top = int((y - 0.5 * h) * y_factor);
+
+                int width = int(w * x_factor);
+                int height = int(h * y_factor);
+
+                boxes.push_back(cv::Rect(left, top, width, height));
+            }
+        }
+        else // yolov5
+        {
+            float confidence = data[4];
+
+            if (confidence >= modelConfidenceThreshold)
+            {
+                float *classes_scores = data+5;
+
+                cv::Mat scores(1, classes.size(), CV_32FC1, classes_scores);
+                cv::Point class_id;
+                double max_class_score;
+
+                minMaxLoc(scores, 0, &max_class_score, 0, &class_id);
+
+                if (max_class_score > modelScoreThreshold)
+                {
+                    confidences.push_back(confidence);
+                    class_ids.push_back(class_id.x);
+
+                    float x = data[0];
+                    float y = data[1];
+                    float w = data[2];
+                    float h = data[3];
+
+                    int left = int((x - 0.5 * w) * x_factor);
+                    int top = int((y - 0.5 * h) * y_factor);
+
+                    int width = int(w * x_factor);
+                    int height = int(h * y_factor);
+
+                    boxes.push_back(cv::Rect(left, top, width, height));
+                }
+            }
+        }
+
+        data += dimensions;
+    }
+
+    std::vector<int> nms_result;
+    cv::dnn::NMSBoxes(boxes, confidences, modelScoreThreshold, modelNMSThreshold, nms_result);
+
+    std::vector<Detection> detections{};
+    for (unsigned long i = 0; i < nms_result.size(); ++i)
+    {
+        int idx = nms_result[i];
+
+        Detection result;
+        result.class_id = class_ids[idx];
+        result.confidence = confidences[idx];
+
+        std::random_device rd;
+        std::mt19937 gen(rd());
+        std::uniform_int_distribution<int> dis(100, 255);
+        result.color = cv::Scalar(dis(gen),
+                                  dis(gen),
+                                  dis(gen));
+
+        result.className = classes[result.class_id];
+        result.box = boxes[idx];
+
+        detections.push_back(result);
+    }
+
+    return detections;
+}
+
+void Inference::loadClassesFromFile()
+{
+    std::ifstream inputFile(classesPath);
+    if (inputFile.is_open())
+    {
+        std::string classLine;
+        while (std::getline(inputFile, classLine))
+            classes.push_back(classLine);
+        inputFile.close();
+    }
+}
+
+void Inference::loadOnnxNetwork()
+{
+    net = cv::dnn::readNetFromONNX(modelPath);
+    if (cudaEnabled)
+    {
+        net.setPreferableBackend(cv::dnn::DNN_BACKEND_CUDA);
+        net.setPreferableTarget(cv::dnn::DNN_TARGET_CUDA);
+    }
+    else
+    {
+        net.setPreferableBackend(cv::dnn::DNN_BACKEND_OPENCV);
+        net.setPreferableTarget(cv::dnn::DNN_TARGET_CPU);
+    }
+}
+
+cv::Mat Inference::formatToSquare(const cv::Mat &source)
+{
+    int col = source.cols;
+    int row = source.rows;
+    int _max = MAX(col, row);
+    cv::Mat result = cv::Mat::zeros(_max, _max, CV_8UC3);
+    source.copyTo(result(cv::Rect(0, 0, col, row)));
+    return result;
+}

detect/onnx_detect/inference.h

@@ -0,0 +1,50 @@
+#pragma once
+
+// Cpp native
+#include <fstream>
+#include <vector>
+#include <string>
+#include <random>
+
+// OpenCV / DNN / Inference
+#include <opencv2/imgproc.hpp>
+#include <opencv2/opencv.hpp>
+#include <opencv2/dnn.hpp>
+
+struct Detection
+{
+    int class_id{0};
+    std::string className{};
+    float confidence{0.0};
+    cv::Scalar color{};
+    cv::Rect box{};
+};
+
+class Inference
+{
+public:
+    Inference(const std::string &onnxModelPath, const cv::Size &modelInputShape = {640, 640}, const std::string &classesTxtFile = "", const bool &runWithCuda = false);
+    std::vector<Detection> runInference(const cv::Mat &input);
+
+private:
+    void loadClassesFromFile();
+    void loadOnnxNetwork();
+    cv::Mat formatToSquare(const cv::Mat &source);
+
+    std::string modelPath{};
+    std::string classesPath{};
+    bool cudaEnabled{};
+
+    std::vector<std::string> classes{"car", "wheel"};
+
+    cv::Size2f modelShape{};
+
+    float modelConfidenceThreshold {0.25};
+    float modelScoreThreshold      {0.45};
+    float modelNMSThreshold        {0.50};
+
+    bool letterBoxForSquare = true;
+
+    cv::dnn::Net net;
+};
+

detect/onnx_detect/wheel-detector.cpp

@@ -0,0 +1,66 @@
+#include "wheel-detector.h"
+
+TensorrtWheelDetector::TensorrtWheelDetector(const std::string &model_file, const std::string &class_file){
+    yolov8_ = new Inference(model_file, cv::Size{640, 640}, class_file, false);
+}
+
+TensorrtWheelDetector::~TensorrtWheelDetector(){
+}
+
+bool TensorrtWheelDetector::detect(cv::Mat& img,std::vector<Object>& objs){
+    std::vector<Detection> rets = yolov8_->runInference(img);
+
+    for (auto &ret: rets) {
+        Object obj;
+       obj.rect = ret.box;
+        obj.label = ret.class_id;
+        obj.prob = ret.confidence;
+        objs.push_back(obj);
+    }
+    return true;
+}
+
+bool TensorrtWheelDetector::detect(cv::Mat& img,std::vector<Object>& objs,cv::Mat& res){
+    std::vector<Detection> rets = yolov8_->runInference(img);
+
+    for (auto &ret: rets) {
+        Object obj;
+        obj.rect = ret.box;
+        obj.label = ret.class_id;
+        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;
+}
+
+std::vector<cv::Point> TensorrtWheelDetector::getPointsFromObj(const Object &obj){
+    std::vector<cv::Point> ret;
+
+    int x=int(obj.rect.x+0.5);
+    int y=int(obj.rect.y+0.5);
+    int width=int(obj.rect.width);
+    int height=int(obj.rect.height);
+
+    for(int i=0;i<height;++i){
+        for(int j=0;j<width;++j){
+            ret.emplace_back(x+j,y+i);
+        }
+    }
+
+    return ret;
+}

detect/onnx_detect/wheel-detector.h

@@ -0,0 +1,29 @@
+#pragma once
+
+#include "inference.h"
+
+struct Object {
+    cv::Rect_<float> rect;
+    int              label = 0;
+    float            prob  = 0.0;
+    cv::Mat          boxMask;
+};
+    
+class TensorrtWheelDetector{
+public:
+    TensorrtWheelDetector(const std::string &model_file, const std::string &class_file);
+    ~TensorrtWheelDetector();
+
+    bool detect(cv::Mat& img,std::vector<Object>& objs);
+    bool detect(cv::Mat& img,std::vector<Object>& objs,cv::Mat& res);
+
+    static std::vector<cv::Point> getPointsFromObj(const Object &obj);
+
+private:
+    Inference*  yolov8_;
+    cv::Size     imgsz_;
+    int      seg_h_        = 120;
+    int      seg_w_        = 160;
+    int      seg_channels_ = 32;
+};
+

detect/tensorrt_detect/common.hpp

@@ -0,0 +1,142 @@
+//
+// Created by ubuntu on 3/16/23.
+//
+
+#ifndef JETSON_SEGMENT_COMMON_HPP
+#define JETSON_SEGMENT_COMMON_HPP
+#include "NvInfer.h"
+#include "opencv2/opencv.hpp"
+#include <sys/stat.h>
+#include <unistd.h>
+
+#define CHECK(call)                                                                                                    \
+    do {                                                                                                               \
+        const cudaError_t error_code = call;                                                                           \
+        if (error_code != cudaSuccess) {                                                                               \
+            printf("CUDA Error:\n");                                                                                   \
+            printf("    File:       %s\n", __FILE__);                                                                  \
+            printf("    Line:       %d\n", __LINE__);                                                                  \
+            printf("    Error code: %d\n", error_code);                                                                \
+            printf("    Error text: %s\n", cudaGetErrorString(error_code));                                            \
+            exit(1);                                                                                                   \
+        }                                                                                                              \
+    } while (0)
+
+class Logger: public nvinfer1::ILogger {
+public:
+    nvinfer1::ILogger::Severity reportableSeverity;
+
+    explicit Logger(nvinfer1::ILogger::Severity severity = nvinfer1::ILogger::Severity::kINFO):
+        reportableSeverity(severity)
+    {
+    }
+
+    void log(nvinfer1::ILogger::Severity severity, const char* msg) noexcept override
+    {
+        if (severity > reportableSeverity) {
+            return;
+        }
+        switch (severity) {
+            case nvinfer1::ILogger::Severity::kINTERNAL_ERROR:
+                std::cerr << "INTERNAL_ERROR: ";
+                break;
+            case nvinfer1::ILogger::Severity::kERROR:
+                std::cerr << "ERROR: ";
+                break;
+            case nvinfer1::ILogger::Severity::kWARNING:
+                std::cerr << "WARNING: ";
+                break;
+            case nvinfer1::ILogger::Severity::kINFO:
+                std::cerr << "INFO: ";
+                break;
+            default:
+                std::cerr << "VERBOSE: ";
+                break;
+        }
+        std::cerr << msg << std::endl;
+    }
+};
+
+inline int get_size_by_dims(const nvinfer1::Dims& dims)
+{
+    int size = 1;
+    for (int i = 0; i < dims.nbDims; i++) {
+        size *= dims.d[i];
+    }
+    return size;
+}
+
+inline int type_to_size(const nvinfer1::DataType& dataType)
+{
+    switch (dataType) {
+        case nvinfer1::DataType::kFLOAT:
+            return 4;
+        case nvinfer1::DataType::kHALF:
+            return 2;
+        case nvinfer1::DataType::kINT32:
+            return 4;
+        case nvinfer1::DataType::kINT8:
+            return 1;
+        case nvinfer1::DataType::kBOOL:
+            return 1;
+        default:
+            return 4;
+    }
+}
+
+inline static float clamp(float val, float min, float max)
+{
+    return val > min ? (val < max ? val : max) : min;
+}
+
+inline bool IsPathExist(const std::string& path)
+{
+    if (access(path.c_str(), 0) == F_OK) {
+        return true;
+    }
+    return false;
+}
+
+inline bool IsFile(const std::string& path)
+{
+    if (!IsPathExist(path)) {
+        printf("%s:%d %s not exist\n", __FILE__, __LINE__, path.c_str());
+        return false;
+    }
+    struct stat buffer;
+    return (stat(path.c_str(), &buffer) == 0 && S_ISREG(buffer.st_mode));
+}
+
+inline bool IsFolder(const std::string& path)
+{
+    if (!IsPathExist(path)) {
+        return false;
+    }
+    struct stat buffer;
+    return (stat(path.c_str(), &buffer) == 0 && S_ISDIR(buffer.st_mode));
+}
+
+namespace seg {
+struct Binding {
+    size_t         size  = 1;
+    size_t         dsize = 1;
+    nvinfer1::Dims dims;
+    std::string    name;
+};
+
+struct Object {
+    cv::Rect_<float> rect;
+    int              label = 0;
+    float            prob  = 0.0;
+    cv::Mat          boxMask;
+};
+
+struct PreParam {
+    float ratio  = 1.0f;
+    float dw     = 0.0f;
+    float dh     = 0.0f;
+    float height = 0;
+    float width  = 0;
+};
+}  // namespace seg
+#endif  // JETSON_SEGMENT_COMMON_HPP

detect/tensorrt_detect/wheel-detector.cpp

@@ -0,0 +1,65 @@
+#include "wheel-detector.h"
+
+TensorrtWheelDetector::TensorrtWheelDetector(const std::string &model_file, const std::string &class_file){
+    cudaSetDevice(0);
+    
+    yolov8_ = new YOLOv8_seg(model_file);
+    yolov8_->make_pipe(false);
+    imgsz_=cv::Size{640, 480};
+    seg_h_        = 120;
+    seg_w_        = 160;
+    seg_channels_ = 32;
+}
+
+TensorrtWheelDetector::~TensorrtWheelDetector(){
+    if(yolov8_!=nullptr){
+        delete yolov8_;
+        yolov8_=nullptr;
+    }
+
+}
+
+bool TensorrtWheelDetector::detect(cv::Mat& img,std::vector<Object>& objs){
+    if(yolov8_==nullptr){
+        return false;
+    }
+    
+    yolov8_->copy_from_Mat(img, imgsz_);
+    yolov8_->infer();
+    float score_thres=0.9;
+    float iou_thres=0.65;
+    int topk=10;
+    yolov8_->postprocess(objs, score_thres, iou_thres, topk, seg_channels_, seg_h_, seg_w_);
+    return true;
+
+}
+
+bool TensorrtWheelDetector::detect(cv::Mat& img,std::vector<Object>& objs,cv::Mat& res){
+    if(detect(img,objs))
+    {
+        const std::vector<std::string> classes={"none","wheel"};
+        const std::vector<std::vector<unsigned int>> colors = {{0, 114, 189},   {0, 255, 0}};
+        const std::vector<std::vector<unsigned int>> mask_colors = {{255, 56, 56},  {255, 0, 0}};
+        yolov8_->draw_objects(img, res, objs, classes, colors, mask_colors);
+        return true;
+    }else{
+        return false;
+    }
+}
+
+std::vector<cv::Point> TensorrtWheelDetector::getPointsFromObj(Object obj){
+    std::vector<cv::Point> ret;
+    int x=int(obj.rect.x+0.5);
+    int y=int(obj.rect.y+0.5);
+    int width=int(obj.rect.width);
+    int height=int(obj.rect.height);
+    
+    for(int i=0;i<height;++i){
+        for(int j=0;j<width;++j){
+            if(obj.boxMask.at<uchar>(i,j)!=0){
+                ret.push_back(cv::Point((x+j),y+i));
+            }
+        }
+    }
+    return ret;
+}

detect/tensorrt_detect/wheel-detector.h

@@ -0,0 +1,25 @@
+#ifndef WHELL_DETECTOR__HH___
+#define WWHELL_DETECTOR__HH___
+#include "yolov8-seg.h"
+
+class TensorrtWheelDetector{
+public:
+    TensorrtWheelDetector(const std::string &model_file, const std::string &class_file);
+    ~TensorrtWheelDetector();
+
+
+    bool detect(cv::Mat& img,std::vector<Object>& objs);
+    bool detect(cv::Mat& img,std::vector<Object>& objs,cv::Mat& res);
+
+    static std::vector<cv::Point> getPointsFromObj(Object obj);
+
+private:
+    YOLOv8_seg*  yolov8_;
+    cv::Size     imgsz_;
+    int      seg_h_        = 120;
+    int      seg_w_        = 160;
+    int      seg_channels_ = 32;
+
+};
+
+#endif

detect/tensorrt_detect/yolov8-seg.cpp

@@ -0,0 +1,319 @@
+//
+// Created by ubuntu on 3/16/23.
+//
+
+#include "yolov8-seg.h"
+
+
+using namespace seg;
+
+YOLOv8_seg::YOLOv8_seg(const std::string& engine_file_path)
+{
+    std::ifstream file(engine_file_path, std::ios::binary);
+    assert(file.good());
+    file.seekg(0, std::ios::end);
+    auto size = file.tellg();
+    file.seekg(0, std::ios::beg);
+    char* trtModelStream = new char[size];
+    assert(trtModelStream);
+    file.read(trtModelStream, size);
+    file.close();
+    initLibNvInferPlugins(&this->gLogger, "");
+    this->runtime = nvinfer1::createInferRuntime(this->gLogger);
+    assert(this->runtime != nullptr);
+
+    this->engine = this->runtime->deserializeCudaEngine(trtModelStream, size);
+    assert(this->engine != nullptr);
+    delete[] trtModelStream;
+    this->context = this->engine->createExecutionContext();
+
+    assert(this->context != nullptr);
+    cudaStreamCreate(&this->stream);
+    this->num_bindings = this->engine->getNbBindings();
+
+    for (int i = 0; i < this->num_bindings; ++i) {
+        Binding            binding;
+        nvinfer1::Dims     dims;
+        nvinfer1::DataType dtype = this->engine->getBindingDataType(i);
+        std::string        name  = this->engine->getBindingName(i);
+        binding.name             = name;
+        binding.dsize            = type_to_size(dtype);
+
+        bool IsInput = engine->bindingIsInput(i);
+        if (IsInput) {
+            this->num_inputs += 1;
+            dims         = this->engine->getProfileDimensions(i, 0, nvinfer1::OptProfileSelector::kMAX);
+            binding.size = get_size_by_dims(dims);
+            binding.dims = dims;
+            this->input_bindings.push_back(binding);
+            // set max opt shape
+            this->context->setBindingDimensions(i, dims);
+        }
+        else {
+            dims         = this->context->getBindingDimensions(i);
+            binding.size = get_size_by_dims(dims);
+            binding.dims = dims;
+            this->output_bindings.push_back(binding);
+            this->num_outputs += 1;
+        }
+        // printf("name: %s, size: %ld, dims: %d %d %d %d %d\n", 
+        //         name.c_str(), binding.dsize, dims.nbDims, dims.d[0], dims.d[1], dims.d[2], dims.d[3]);
+    }
+}
+
+YOLOv8_seg::~YOLOv8_seg()
+{
+    this->context->destroy();
+    this->engine->destroy();
+    this->runtime->destroy();
+    cudaStreamDestroy(this->stream);
+    for (auto& ptr : this->device_ptrs) {
+        CHECK(cudaFree(ptr));
+    }
+
+    for (auto& ptr : this->host_ptrs) {
+        CHECK(cudaFreeHost(ptr));
+    }
+}
+
+void YOLOv8_seg::make_pipe(bool warmup)
+{
+
+    for (auto& bindings : this->input_bindings) {
+        void* d_ptr;
+        CHECK(cudaMalloc(&d_ptr, bindings.size * bindings.dsize));
+        this->device_ptrs.push_back(d_ptr);
+    }
+
+    for (auto& bindings : this->output_bindings) {
+        void * d_ptr, *h_ptr;
+        size_t size = bindings.size * bindings.dsize;
+        CHECK(cudaMalloc(&d_ptr, size));
+        CHECK(cudaHostAlloc(&h_ptr, size, 0));
+        this->device_ptrs.push_back(d_ptr);
+        this->host_ptrs.push_back(h_ptr);
+    }
+
+    if (warmup) {
+        for (int i = 0; i < 10; i++) {
+            for (auto& bindings : this->input_bindings) {
+                size_t size  = bindings.size * bindings.dsize;
+                void*  h_ptr = malloc(size);
+                memset(h_ptr, 0, size);
+                CHECK(cudaMemcpyAsync(this->device_ptrs[0], h_ptr, size, cudaMemcpyHostToDevice, this->stream));
+                free(h_ptr);
+            }
+            this->infer();
+        }
+        printf("model warmup 10 times\n");
+    }
+}
+
+void YOLOv8_seg::letterbox(const cv::Mat& image, cv::Mat& out, cv::Size& size)
+{
+    const float inp_h  = size.height;
+    const float inp_w  = size.width;
+    float       height = image.rows;
+    float       width  = image.cols;
+
+    float r    = std::min(inp_h / height, inp_w / width);
+    int   padw = std::round(width * r);
+    int   padh = std::round(height * r);
+
+    cv::Mat tmp;
+    if ((int)width != padw || (int)height != padh) {
+        cv::resize(image, tmp, cv::Size(padw, padh));
+    }
+    else {
+        tmp = image.clone();
+    }
+
+    float dw = inp_w - padw;
+    float dh = inp_h - padh;
+
+    dw /= 2.0f;
+    dh /= 2.0f;
+    int top    = int(std::round(dh - 0.1f));
+    int bottom = int(std::round(dh + 0.1f));
+    int left   = int(std::round(dw - 0.1f));
+    int right  = int(std::round(dw + 0.1f));
+
+    cv::copyMakeBorder(tmp, tmp, top, bottom, left, right, cv::BORDER_CONSTANT, {114, 114, 114});
+
+    cv::dnn::blobFromImage(tmp, out, 1 / 255.f, cv::Size(), cv::Scalar(0, 0, 0), true, false, CV_32F);
+    this->pparam.ratio  = 1 / r;
+    this->pparam.dw     = dw;
+    this->pparam.dh     = dh;
+    this->pparam.height = height;
+    this->pparam.width  = width;
+}
+
+void YOLOv8_seg::copy_from_Mat(const cv::Mat& image)
+{
+    cv::Mat  nchw;
+    auto&    in_binding = this->input_bindings[0];
+    auto     width      = in_binding.dims.d[3];
+    auto     height     = in_binding.dims.d[2];
+    cv::Size size{width, height};
+    this->letterbox(image, nchw, size);
+
+    this->context->setBindingDimensions(0, nvinfer1::Dims{4, {1, 3, height, width}});
+
+    CHECK(cudaMemcpyAsync(
+        this->device_ptrs[0], nchw.ptr<float>(), nchw.total() * nchw.elemSize(), cudaMemcpyHostToDevice, this->stream));
+}
+
+void YOLOv8_seg::copy_from_Mat(const cv::Mat& image, cv::Size& size)
+{
+    cv::Mat nchw;
+    this->letterbox(image, nchw, size);
+    this->context->setBindingDimensions(0, nvinfer1::Dims{4, {1, 3, size.height, size.width}});
+    CHECK(cudaMemcpyAsync(
+        this->device_ptrs[0], nchw.ptr<float>(), nchw.total() * nchw.elemSize(), cudaMemcpyHostToDevice, this->stream));
+}
+
+void YOLOv8_seg::infer()
+{
+
+    this->context->enqueueV2(this->device_ptrs.data(), this->stream, nullptr);
+    for (int i = 0; i < this->num_outputs; i++) {
+        size_t osize = this->output_bindings[i].size * this->output_bindings[i].dsize;
+        CHECK(cudaMemcpyAsync(
+            this->host_ptrs[i], this->device_ptrs[i + this->num_inputs], osize, cudaMemcpyDeviceToHost, this->stream));
+    }
+    cudaStreamSynchronize(this->stream);
+}
+
+void YOLOv8_seg::postprocess(
+    std::vector<Object>& objs, float score_thres, float iou_thres, int topk, int seg_channels, int seg_h, int seg_w)
+{
+    objs.clear();
+    auto input_h      = this->input_bindings[0].dims.d[2];
+    auto input_w      = this->input_bindings[0].dims.d[3];
+    auto num_anchors  = this->output_bindings[0].dims.d[1];
+    auto num_channels = this->output_bindings[0].dims.d[2];
+
+    auto& dw     = this->pparam.dw;
+    auto& dh     = this->pparam.dh;
+    auto& width  = this->pparam.width;
+    auto& height = this->pparam.height;
+    auto& ratio  = this->pparam.ratio;
+
+    auto*   output = static_cast<float*>(this->host_ptrs[0]);
+    cv::Mat protos = cv::Mat(seg_channels, seg_h * seg_w, CV_32F, static_cast<float*>(this->host_ptrs[1]));
+
+    std::vector<int>      labels;
+    std::vector<float>    scores;
+    std::vector<cv::Rect> bboxes;
+    std::vector<cv::Mat>  mask_confs;
+    std::vector<int>      indices;
+
+    for (int i = 0; i < num_anchors; i++) {
+        float* ptr   = output + i * num_channels;
+        float  score = *(ptr + 4);
+		
+         /*if (score > score_thres) {
+             printf("num_channels: %d, score: %f\n", num_channels, score);
+         }*/
+	
+        if (score > score_thres) {
+            float x0 = *ptr++ - dw;
+            float y0 = *ptr++ - dh;
+            float x1 = *ptr++ - dw;
+            float y1 = *ptr++ - dh;
+
+            x0 = clamp(x0 * ratio, 0.f, width);
+            y0 = clamp(y0 * ratio, 0.f, height);
+            x1 = clamp(x1 * ratio, 0.f, width);
+            y1 = clamp(y1 * ratio, 0.f, height);
+
+            int     label     = *(++ptr);
+            cv::Mat mask_conf = cv::Mat(1, seg_channels, CV_32F, ++ptr);
+            mask_confs.push_back(mask_conf);
+            labels.push_back(label);
+            scores.push_back(score);
+            bboxes.push_back(cv::Rect_<float>(x0, y0, x1 - x0, y1 - y0));
+        }
+    }
+
+#if defined(BATCHED_NMS)
+    cv::dnn::NMSBoxesBatched(bboxes, scores, labels, score_thres, iou_thres, indices);
+#else
+    cv::dnn::NMSBoxes(bboxes, scores, score_thres, iou_thres, indices);
+#endif
+
+    cv::Mat masks;
+    int     cnt = 0;
+    for (auto& i : indices) {
+        if (cnt >= topk) {
+            break;
+        }
+        cv::Rect tmp = bboxes[i];
+        Object   obj;
+        obj.label = labels[i];
+        obj.rect  = tmp;
+        obj.prob  = scores[i];
+        masks.push_back(mask_confs[i]);
+        objs.push_back(obj);
+        cnt += 1;
+    }
+    if (masks.empty()) {
+        // masks is empty
+    }
+    else {
+        cv::Mat matmulRes = (masks * protos).t();
+        cv::Mat maskMat   = matmulRes.reshape(indices.size(), {seg_h, seg_w});
+
+        std::vector<cv::Mat> maskChannels;
+        cv::split(maskMat, maskChannels);
+        int scale_dw = dw / input_w * seg_w;
+        int scale_dh = dh / input_h * seg_h;
+
+        cv::Rect roi(scale_dw, scale_dh, seg_w - 2 * scale_dw, seg_h - 2 * scale_dh);
+
+        for (int i = 0; i < indices.size(); i++) {
+            cv::Mat dest, mask;
+            cv::exp(-maskChannels[i], dest);
+            dest = 1.0 / (1.0 + dest);
+            dest = dest(roi);
+            cv::resize(dest, mask, cv::Size((int)width, (int)height), cv::INTER_LINEAR);
+            objs[i].boxMask = mask(objs[i].rect) > 0.5f;
+        }
+    }
+}
+
+void YOLOv8_seg::draw_objects(const cv::Mat&                                image,
+                              cv::Mat&                                      res,
+                              const std::vector<Object>&                    objs,
+                              const std::vector<std::string>&               CLASS_NAMES,
+                              const std::vector<std::vector<unsigned int>>& COLORS,
+                              const std::vector<std::vector<unsigned int>>& MASK_COLORS)
+{
+    res          = image.clone();
+    cv::Mat mask = image.clone();
+    for (auto& obj : objs) {
+        int        idx   = obj.label;
+        cv::Scalar color = cv::Scalar(COLORS[idx][0], COLORS[idx][1], COLORS[idx][2]);
+        cv::Scalar mask_color =
+            cv::Scalar(MASK_COLORS[idx % 20][0], MASK_COLORS[idx % 20][1], MASK_COLORS[idx % 20][2]);
+        cv::rectangle(res, obj.rect, color, 2);
+
+        char text[256];
+        sprintf(text, "%s %.1f%%", CLASS_NAMES[idx].c_str(), obj.prob * 100);
+        mask(obj.rect).setTo(mask_color, obj.boxMask);
+
+        int      baseLine   = 0;
+        cv::Size label_size = cv::getTextSize(text, cv::FONT_HERSHEY_SIMPLEX, 0.4, 1, &baseLine);
+
+        int x = (int)obj.rect.x;
+        int y = (int)obj.rect.y + 1;
+
+        if (y > res.rows)
+            y = res.rows;
+
+        cv::rectangle(res, cv::Rect(x, y, label_size.width, label_size.height + baseLine), {0, 0, 255}, -1);
+
+        cv::putText(res, text, cv::Point(x, y + label_size.height), cv::FONT_HERSHEY_SIMPLEX, 0.4, {255, 255, 255}, 1);
+    }
+    cv::addWeighted(res, 0.5, mask, 0.8, 1, res);
+}

detect/tensorrt_detect/yolov8-seg.h

@@ -0,0 +1,53 @@
+//
+// Created by ubuntu on 3/16/23.
+//
+#ifndef JETSON_SEGMENT_YOLOV8_SEG_HPP
+#define JETSON_SEGMENT_YOLOV8_SEG_HPP
+#include "NvInferPlugin.h"
+#include "common.hpp"
+#include <fstream>
+
+using namespace seg;
+
+class YOLOv8_seg {
+public:
+    explicit YOLOv8_seg(const std::string& engine_file_path);
+    ~YOLOv8_seg();
+
+    void                 make_pipe(bool warmup = true);
+    void                 copy_from_Mat(const cv::Mat& image);
+    void                 copy_from_Mat(const cv::Mat& image, cv::Size& size);
+    void                 letterbox(const cv::Mat& image, cv::Mat& out, cv::Size& size);
+    void                 infer();
+    void                 postprocess(std::vector<Object>& objs,
+                                     float                score_thres  = 0.25f,
+                                     float                iou_thres    = 0.65f,
+                                     int                  topk         = 100,
+                                     int                  seg_channels = 32,
+                                     int                  seg_h        = 160,
+                                     int                  seg_w        = 160);
+    static void          draw_objects(const cv::Mat&                                image,
+                                      cv::Mat&                                      res,
+                                      const std::vector<Object>&                    objs,
+                                      const std::vector<std::string>&               CLASS_NAMES,
+                                      const std::vector<std::vector<unsigned int>>& COLORS,
+                                      const std::vector<std::vector<unsigned int>>& MASK_COLORS);
+    int                  num_bindings;
+    int                  num_inputs  = 0;
+    int                  num_outputs = 0;
+    std::vector<Binding> input_bindings;
+    std::vector<Binding> output_bindings;
+    std::vector<void*>   host_ptrs;
+    std::vector<void*>   device_ptrs;
+
+    PreParam pparam;
+
+private:
+    nvinfer1::ICudaEngine*       engine  = nullptr;
+    nvinfer1::IRuntime*          runtime = nullptr;
+    nvinfer1::IExecutionContext* context = nullptr;
+    cudaStream_t                 stream  = nullptr;
+    Logger                       gLogger{nvinfer1::ILogger::Severity::kERROR};
+};
+
+#endif  // JETSON_SEGMENT_YOLOV8_SEG_HPP

proto/communication.proto

@@ -18,6 +18,27 @@ message measure_info {
     required float move_distance=13;                     // 前进距离
 }
 
+message PlcOffsetInfo {
+    optional float x = 1 [default = 0];
+    optional float y = 2 [default = 0];
+    optional float theta = 3 [default = 0];
+    optional float width = 4 [default = 0];
+    optional float wheelbase = 5 [default = 0];
+}
+
+message outOfRangeInfo {
+  required float border_minx = 1; // 最小边界x，左超界提示
+  required float border_maxx = 2; // 最大边界x，右超界提示
+  required float plc_border_miny = 3;// plc后夹持y方向极限值
+  required float plc_border_maxy = 4;// plc后夹持y方向极限值
+  required float car_min_width = 5; // 最小车宽
+  required float car_max_width = 6; // 最大车宽
+  required float car_min_wheelbase = 7; // 最小轴距
+  required float car_max_wheelbase = 8; // 最大轴距
+  required float turnplate_angle_limit_min_clockwise = 9; // 转盘逆时针角度极限
+  required float turnplate_angle_limit_max_clockwise = 10; // 转盘顺时针角度极限
+}
+
 message CommunicationManagerConfig {
     optional bool rabbitmq_enable = 1 [default = true];
     optional string rabbitmq_config_file = 2 [default = "/rabbitmq.prototxt"];   // 安装目录下的路径，并非绝对路径或者bin文件开始的相对路径
@@ -26,6 +47,9 @@ message CommunicationManagerConfig {
     optional int32 grpc_server_port = 5 [default = 9876];
     optional bool mqtt_enable = 6 [default = true];
     optional string mqtt_config_file = 7 [default = "/mqtt.json"];   // 安装目录下的路径，并非绝对路径或者bin文件开始的相对路径
+    optional PlcOffsetInfo offset = 8;
+    optional outOfRangeInfo plc_out_info = 9;
+    optional outOfRangeInfo display_out_info = 10;
 }
 
 /*分配的车位信息*/

proto/def.grpc.proto

@@ -34,9 +34,8 @@ message LabelImage{
   int32 label=1;
   Image ir=2;
 }
-message Line{
-  int32 begin=1;
-  int32 end=2;
+message Pixel{
+  uint32 point = 1;
 }
 message SegBox{
   int32 x = 1;
@@ -44,7 +43,8 @@ message SegBox{
   int32 width = 3;
   int32 height = 4;
   float confidence = 5;
-  repeated Line lines= 6;
+  repeated Pixel wheels= 6;
+  repeated Pixel tires= 7;
 }
 message LabelYolo{
   int32 label=1;

vzense/device_tof3d.cpp

@@ -566,11 +566,7 @@ void DeviceTof3D::run(const DeviceAzimuth &azimuth) {
             if (vz_ret == SUCCESS) {
                 // 灰度图转换
                 Frame2Mat(irFrame, depthInfo.irMat);
-                memcpy(depthInfo.pointMat.data, worldV, 480 * 640 * sizeof(VzVector3f));
-                m_device_mat[azimuth] = depthInfo;
                 // 点云转换
-                t_ir_img = depthInfo.irMat.clone();
-                t_point_img = cv::Mat::zeros(480, 640, CV_32FC4);
                 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;
@@ -584,15 +580,18 @@ void DeviceTof3D::run(const DeviceAzimuth &azimuth) {
                         if (trans(2) < 0.03 || fabs(trans(0)) > 1.5 || fabs(trans(1)) > 2.7 || trans(2) > 1) {
                             // continue;
                         }
-                        t_point_img.at<cv::Vec4f>(rows, cols)[0] = trans(0);
-                        t_point_img.at<cv::Vec4f>(rows, cols)[1] = trans(1);
-                        t_point_img.at<cv::Vec4f>(rows, cols)[2] = trans(2);
+                        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 {

vzense/device_tof3d.h

@@ -30,7 +30,7 @@ public:
 
         DeviceTof3DSaveInfo() {
             irMat = cv::Mat::zeros(480, 640, CV_8UC1);
-            pointMat = cv::Mat::zeros(480, 640, CV_32FC3);
+            pointMat = cv::Mat::zeros(480, 640, CV_32FC4);
         }
 
         DeviceTof3DSaveInfo& operator=(const DeviceTof3DSaveInfo &p) {