1、优化轮胎点云获取方式；

2、优化多结果融合；

communication/communication_manager.cpp

@@ -114,7 +114,7 @@ Error_manager CommunicationManager::sendRabbitmqData(const MeasureStatu &measure
         }
         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_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);
@@ -180,15 +180,15 @@ Range_status CommunicationManager::outOfRangeDetection(const MeasureStatu &measu
     }
 
     // 判断车辆旋转角超界情况
-    double t_dtheta = measure_result.car.theta;
-    if (measure_result.car.theta < out_info.turnplate_angle_limit_min_clockwise()) {
+    float theta = -measure_result.car.theta * 180 / M_PI;
+    if (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()) {
+    } else if (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()) {
+    } else if (theta > out_info.turnplate_angle_limit_min_clockwise() &&
+            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);
     }
@@ -200,12 +200,14 @@ Range_status CommunicationManager::outOfRangeDetection(const MeasureStatu &measu
     }
 
     // 方向盘调正
-    if (fabs(measure_result.car.front_wheels_theta) > 10) {
+    float front_wheels_theta = measure_result.car.front_wheels_theta * 180 / M_PI;
+    DLOG(INFO) << "front_wheels_theta " << front_wheels_theta;
+    if (fabs(front_wheels_theta) > 10) {
         last_range_statu |= Range_status::Range_steering_wheel_nozero;
-    } else if (fabs(measure_result.car.front_wheels_theta) < 7) {
+    } else if (fabs(front_wheels_theta) < 7) {
         last_range_statu &= (~Range_status::Range_steering_wheel_nozero);
     }
-    if (last_range_statu & Range_status::Range_angle_anti_clock) {
+    if (last_range_statu & Range_status::Range_steering_wheel_nozero) {
         return Range_status::Range_steering_wheel_nozero;
     }
 

detect/ceres_detect/car_pose_detector.h

@@ -40,6 +40,7 @@ public:
         float uniform_x = 0;
         float uniform_y = 0;
         float forward_dis = 0;
+        bool correct = false;
 
         void reset() {
             wheels_center_x = 0;
@@ -55,12 +56,13 @@ public:
             uniform_x = 0;
             uniform_y = 0;
             forward_dis = 0;
+            correct = false;
         }
 
         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 * 180.0 / M_PI, 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 * 180.0 / M_PI, wheel_base, loss);
             return str;
         }
     };
@@ -85,4 +87,4 @@ private:
 
 };
 
-#endif // !Z_DETECTOR_HH
+#endif // !Z_DETECTOR_HH

detect/ceres_detect/detect_wheel_ceres.h

@@ -17,6 +17,7 @@
 class detect_wheel_ceres {
 public:
     detect_wheel_ceres();
+
     ~detect_wheel_ceres();
 
     static detect_wheel_ceres *iter() {
@@ -27,71 +28,83 @@ public:
         return instance;
     }
 
-    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_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_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 recorrect_width(pcl::PointCloud<pcl::PointXYZ>::Ptr left_cl,
+                         pcl::PointCloud<pcl::PointXYZ>::Ptr right_cl,
+                         float theta, float &width);
 
     /*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);
-
+    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);
 
 
     /*
      * 只检测动态参数， 固定参数已知
      */
-    bool detect_dynP(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_dynP(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);
 
 private:
 
-    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);
+    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);
 
-    bool 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,float& front_theta,float& loss);
+    bool 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, float &front_theta,
+                    float &loss);
 
-    bool ransicLine(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud,pcl::PointXYZ& center,double& theta);
+    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);
+    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);
+                                                           float theta, float y);
 
+    bool compute_width(pcl::PointCloud<pcl::PointXYZ>::Ptr cl3, pcl::PointCloud<pcl::PointXYZ>::Ptr cl4,
+                       int NUM, float &theta, float &width);
 
 
-    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;     //右后点云
+    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:
-    cv::Mat                                 m_map;                  //保存一份地图用作匹配
-    cv::Mat                                 m_mapr;
+    cv::Mat m_map;                  //保存一份地图用作匹配
+    cv::Mat m_mapr;
 
-    std::vector<double>                     m_line_l;
-    std::vector<double>                     m_line_r;
-    std::vector<double>                     m_line_y_2stp;
-    std::vector<double>                     m_line_y;
+    std::vector<double> m_line_l;
+    std::vector<double> m_line_r;
+    std::vector<double> m_line_y_2stp;
+    std::vector<double> m_line_y;
 
-    std::vector<double>                     m_line_x;
+    std::vector<double> m_line_x;
 
 };
 

detect/detect_manager.cpp

@@ -99,6 +99,9 @@ void DetectManager::run() {
     LOG(INFO) << "detect thread running in " << std::this_thread::get_id();
 
     DetectResult m_detect_result_record;
+    stableValue detect_result_theta_memery_box;
+    stableValue detect_result_width_memery_box;
+    stableValue detect_result_wheel_base_memery_box;
     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++) {
@@ -200,22 +203,21 @@ void DetectManager::run() {
             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();
             // 轮毂平面检测
             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);
+            wheel_ransic_ret.correct = 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);
+            tires_ransic_ret.correct = 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;
@@ -228,7 +230,7 @@ void DetectManager::run() {
             }
             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],
+                wheel_2step_ret.correct = 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],
@@ -253,7 +255,7 @@ void DetectManager::run() {
                     }
                 }
             } else {
-                ceres_detect_ret = wheel_detect->detect_dynP(vct_wheel_cloud[DeviceAzimuth::DEVICE_AZIMUTH_LF],
+                wheel_2step_ret.correct = 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],
@@ -268,24 +270,26 @@ void DetectManager::run() {
             // 综合测量结果
             if (MergeAllMeaureResult(detect_result.car, wheel_ransic_ret, tires_ransic_ret, wheel_2step_ret) == MeasureStatu::Measure_Failture) {
                 detect_statu = MeasureStatu::Measure_Failture;
-            }
+            } else {
+                // 如果车辆运动，不断刷新运动时间，如果由运动转为静止则刷新运动状态到静止
+                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;
+                }
 
-            // 如果车辆运动，不断刷新运动时间，如果由运动转为静止则刷新运动状态到静止
-            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 (ceres_detect_ret) {
-                // 记录下测量结果
                 m_detect_result_record = detect_result;
+                detect_result_theta_memery_box.save(detect_result.car.theta);
+                detect_result_width_memery_box.save(detect_result.car.wheel_width);
+                detect_result_wheel_base_memery_box.save(detect_result.car.wheel_base);
+                m_detect_result_record.car.theta = detect_result_theta_memery_box.value();
+                m_detect_result_record.car.wheel_width = detect_result_width_memery_box.value();
+                m_detect_result_record.car.wheel_base = detect_result_wheel_base_memery_box.value();
+                LOG(INFO) << m_detect_result_record.car.info();
                 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.";
@@ -309,10 +313,14 @@ Error_manager DetectManager::WheelCloudOptimize(std::vector<pcl::PointCloud<pcl:
     pcl::PointCloud<pcl::PointXYZ>::Ptr filtered_cloud(new pcl::PointCloud<pcl::PointXYZ>);
     for (int device_index = 0; device_index < DeviceAzimuth::DEVICE_AZIMUTH_MAX; device_index++) {
         if (vct_wheel_cloud[device_index]->empty()) { continue; }
-
         vox.setInputCloud(vct_wheel_cloud[device_index]);                   //设置需要过滤的点云给滤波对象
         vox.filter(*vct_wheel_cloud[device_index]);                         //执行滤波处理，存储输出
+//        Eigen::Vector4f centroid;  //质心
+//        pcl::compute3DCentroid(*vct_wheel_cloud[device_index], centroid); // 计算质心
+//        result.wheel[device_index].center = pcl::PointXYZ(centroid[0], centroid[1], 0);
     }
+//    LOG(INFO) << "wheel base: " << result.wheel[0].center.y - result.wheel[2].center.y;
+//    LOG(INFO) << "wheel base: " << result.wheel[1].center.y - result.wheel[3].center.y;
 
     return {};
 }
@@ -367,48 +375,71 @@ Error_manager DetectManager::YoloSegMergeData(cv::Mat &merge_mat, JetStream::Lab
         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));
+        if (iter->prob > 0.6) {            
+            struct segResultColInfo {
+                int max = -1e8;
+                int min = 1e8;
+            };
+            segResultColInfo col_info[1280];
+            for (auto &point: seg_points) {
+                col_info[point.x].max = MAX(point.y, col_info[point.x].max);
+                col_info[point.x].min = MIN(point.y, col_info[point.x].min);
+                seg_results.mutable_boxes(device_index)->add_wheels()->set_point(getPixelIndex(point.y, point.x));
+            }
+            for (int col_index = 0; col_index < 1280; col_index++) {
+                if (col_info[col_index].max < 0 || col_info[col_index].min > 960) {
+                    continue;
+                }
+                for (int row_index = col_info[col_index].max; row_index < col_info[col_index].max + 10; row_index++) {
+                    seg_results.mutable_boxes(device_index)->add_tires()->set_point(getPixelIndex(row_index, col_index));
+                }
+                for (int row_index = col_info[col_index].min; row_index > col_info[col_index].min - 10; row_index--) {
+                    seg_results.mutable_boxes(device_index)->add_tires()->set_point(getPixelIndex(row_index, col_index));
                 }
             }
+            // // 内外椭圆参数
+            // 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));
+            //     }
+            // }
         }
     }
+    // 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";
     return {};
@@ -427,37 +458,72 @@ MeasureStatu DetectManager::MergeAllMeaureResult(Car_pose_detector::CarDetectRes
                                                  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;
+    LOG(INFO) << wheel_ransic.info();
+    LOG(INFO) << tires_ransic.info();
+    LOG(INFO) << wheel_2step.info();
+    if (!wheel_ransic.correct && !tires_ransic.correct) {
+        LOG(INFO) << "----------------------------------------------------";
+        return MeasureStatu::Measure_Failture;
+    } else if (wheel_ransic.correct && !tires_ransic.correct) {
+        merge_result.wheel_width = wheel_ransic.wheel_width;
+        merge_result.front_wheels_theta = wheel_ransic.front_wheels_theta;
+        if (wheel_2step.correct && !isCarDetectResultNearly(wheel_2step, wheel_ransic)) {
+            merge_result.wheels_center_x = wheel_ransic.wheels_center_x;
+            merge_result.wheels_center_y = wheel_ransic.wheels_center_y;
+            merge_result.wheel_base = wheel_ransic.wheel_base;
+            merge_result.theta = wheel_ransic.theta;
+        } else if (wheel_2step.correct && 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_base = (wheel_ransic.wheel_base + wheel_2step.wheel_base) * 0.5;
+            merge_result.theta = wheel_ransic.theta;
+        } else {
+            LOG(INFO) << "----------------------------------------------------";
+            return MeasureStatu::Measure_Failture;
+        }
+    } else if (tires_ransic.correct && !wheel_ransic.correct) {
+        merge_result.wheel_width = tires_ransic.wheel_width;
         merge_result.front_wheels_theta = tires_ransic.front_wheels_theta;
-    } else  {
-        if (isCarDetectResultNearly(wheel_2step, tires_ransic)) {
+        if (wheel_2step.correct && !isCarDetectResultNearly(wheel_2step, tires_ransic)) {
+            merge_result.wheels_center_x = tires_ransic.wheels_center_x;
+            merge_result.wheels_center_y = tires_ransic.wheels_center_y;
+            merge_result.wheel_base = tires_ransic.wheel_base;
+            merge_result.theta = tires_ransic.theta;
+        } else if (wheel_2step.correct && 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 {
+            LOG(INFO) << "----------------------------------------------------";
             return MeasureStatu::Measure_Failture;
         }
+    } else {
+        merge_result.wheel_width = wheel_ransic.wheel_width * 0.7 + tires_ransic.wheel_width * 0.3;
+        merge_result.front_wheels_theta = (wheel_ransic.front_wheels_theta + tires_ransic.front_wheels_theta) * 0.5;
+        if (fabs(wheel_ransic.theta - tires_ransic.theta) * 180.0 / M_PI < 0.7) {
+            merge_result.wheels_center_x = wheel_ransic.wheels_center_x;
+            merge_result.wheels_center_y = wheel_ransic.wheels_center_y;
+            merge_result.wheel_base = wheel_ransic.wheel_base * 0.7 + tires_ransic.wheel_base * 0.3;
+            merge_result.theta = wheel_ransic.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_base = (tires_ransic.wheel_base + wheel_2step.wheel_base) * 0.5;
+                merge_result.theta = tires_ransic.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_base = (wheel_ransic.wheel_base + wheel_2step.wheel_base) * 0.5;
+                merge_result.theta = wheel_ransic.theta;
+            } else {
+                LOG(INFO) << "----------------------------------------------------";
+                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;
 }

detect/detect_manager.h

@@ -5,6 +5,7 @@
 #include <Eigen/Geometry>
 
 #include "tool/error_code.hpp"
+#include "tool/memory_box.hpp"
 #include "ceres_detect/detect_wheel_ceres.h"
 #include "proto/enum_type.pb.h"
 #include "proto/detect_message.pb.h"

vzense/NebulaSDK/x86_64/lib/libNebula_api.so

@@ -1 +0,0 @@
-libNebula_api.so.1.2

vzense/NebulaSDK/x86_64/lib/libNebula_api.so.1.2

@@ -1 +0,0 @@
-libNebula_api.so.1.2.7

vzense/device_tof3d.cpp

@@ -139,7 +139,7 @@ bool DeviceTof3D::isAllDevicesFound(uint32_t deviceCount) {
 //        return false;
 //    }
 
-    auto *pDeviceListInfo = new VzDeviceInfo[4];
+    auto *pDeviceListInfo = new VzDeviceInfo[16];
     VZ_GetDeviceInfoList(deviceCount, pDeviceListInfo);
 
     for (auto &device: m_devices_list) {