From f34cefe597b43cb81e8137928816bddca044548b Mon Sep 17 00:00:00 2001 From: cyy_mac Date: Thu, 26 Mar 2026 06:59:01 +0800 Subject: [PATCH] add auto binarythres fix logic 2 --- .../src/armor_yolo_detector_node.cpp | 130 ++++++++++-------- 1 file changed, 74 insertions(+), 56 deletions(-) diff --git a/src/rm_auto_aim/armor_yolo_detect/src/armor_yolo_detector_node.cpp b/src/rm_auto_aim/armor_yolo_detect/src/armor_yolo_detector_node.cpp index 93f3586..cd88e51 100644 --- a/src/rm_auto_aim/armor_yolo_detect/src/armor_yolo_detector_node.cpp +++ b/src/rm_auto_aim/armor_yolo_detect/src/armor_yolo_detector_node.cpp @@ -675,76 +675,94 @@ void ArmorYoloDetectorNode::performBinaryThresCalibration( FYT_INFO("armor_yolo_detect", "Calibration searching: thres={}, no detection yet", calib_current_thres_); return; } else { - // Detection succeeded! Switch to binary search mode + // Detection succeeded! Switch to stabilization phase calib_searching_ = false; - calib_frame_count_ = 0; - FYT_INFO("armor_yolo_detect", "Calibration: detection found at thres={}, starting binary search", calib_current_thres_); + calib_frame_count_ = 1; // Start counting from 1 + FYT_INFO("armor_yolo_detect", "Calibration: detection found at thres={}, waiting for stability...", calib_current_thres_); + return; // Wait for next frame to continue stability check } } - // Phase 2: Binary search to find optimal threshold - int low = 30, high = calib_current_thres_, best_thres = calib_current_thres_; - double best_error = std::numeric_limits::max(); - constexpr int max_iterations = 5; - constexpr double error_tolerance = 100.0; // pixels^2 - - for (int iter = 0; iter < max_iterations; ++iter) { - int mid = (low + high) / 2; - - // Temporarily set binary threshold and run detection + // Phase 2: Wait for stable detection (no frame limit) + // Check if detection still succeeds at current threshold + { int original_thres = detector_->binary_thres; - detector_->binary_thres = mid; + detector_->binary_thres = calib_current_thres_; auto armors = detector_->processROIs(img, gray_img, rois); detector_->binary_thres = original_thres; if (armors.empty()) { - // No armors detected at this threshold, need lower threshold - high = mid - 1; - continue; - } - - // Calculate traditional detection average area - double trad_avg_area = 0; - for (const auto& armor : armors) { - double w = cv::norm(armor.left_light.top - armor.right_light.top); - double h = cv::norm(armor.left_light.top - armor.left_light.bottom); - trad_avg_area += w * h; - } - trad_avg_area /= armors.size(); - - // Calculate error (difference between traditional and YOLO area) - double error = std::abs(trad_avg_area - yolo_avg_area); - - FYT_INFO("armor_yolo_detect", "Calibration iter {}: thres={}, yolo_area={:.1f}, trad_area={:.1f}, error={:.1f}", - iter, mid, yolo_avg_area, trad_avg_area, error); - - if (error < best_error) { - best_error = error; - best_thres = mid; - } - - if (error < error_tolerance) { - FYT_INFO("armor_yolo_detect", "Calibration converged: optimal thres={}", best_thres); - break; - } - - // Adjust search range based on which direction reduces error - // If traditional area > YOLO area, need lower threshold (fewer lights) - if (trad_avg_area > yolo_avg_area) { - high = mid - 1; - } else { - low = mid + 1; + // Detection lost, go back to searching phase + FYT_INFO("armor_yolo_detect", "Calibration: detection lost, searching again..."); + calib_searching_ = true; + calib_current_thres_ = 250; + calib_frame_count_ = 0; + return; } } - // Update threshold to best found - calib_current_thres_ = best_thres; - - // Only count frames where we got valid detection + // Detection is stable, increment counter calib_frame_count_++; - FYT_INFO("armor_yolo_detect", "Calibration progress: {}/10 (thres={})", calib_frame_count_, best_thres); + FYT_INFO("armor_yolo_detect", "Calibration stability: {}/5 (thres={})", calib_frame_count_, calib_current_thres_); - if (calib_frame_count_ >= 10) { + // After stable for 5 frames, start binary search + if (calib_frame_count_ >= 5) { + // Phase 3: Binary search to find optimal threshold + int low = 30, high = calib_current_thres_, best_thres = calib_current_thres_; + double best_error = std::numeric_limits::max(); + constexpr int max_iterations = 5; + constexpr double error_tolerance = 100.0; // pixels^2 + + for (int iter = 0; iter < max_iterations; ++iter) { + int mid = (low + high) / 2; + + // Temporarily set binary threshold and run detection + int original_thres = detector_->binary_thres; + detector_->binary_thres = mid; + auto armors = detector_->processROIs(img, gray_img, rois); + detector_->binary_thres = original_thres; + + if (armors.empty()) { + // No armors detected at this threshold, need lower threshold + high = mid - 1; + continue; + } + + // Calculate traditional detection average area + double trad_avg_area = 0; + for (const auto& armor : armors) { + double w = cv::norm(armor.left_light.top - armor.right_light.top); + double h = cv::norm(armor.left_light.top - armor.left_light.bottom); + trad_avg_area += w * h; + } + trad_avg_area /= armors.size(); + + // Calculate error (difference between traditional and YOLO area) + double error = std::abs(trad_avg_area - yolo_avg_area); + + FYT_INFO("armor_yolo_detect", "Calibration binary search iter {}: thres={}, yolo_area={:.1f}, trad_area={:.1f}, error={:.1f}", + iter, mid, yolo_avg_area, trad_avg_area, error); + + if (error < best_error) { + best_error = error; + best_thres = mid; + } + + if (error < error_tolerance) { + FYT_INFO("armor_yolo_detect", "Calibration converged: optimal thres={}", best_thres); + break; + } + + // Adjust search range based on which direction reduces error + // If traditional area > YOLO area, need lower threshold (fewer lights) + if (trad_avg_area > yolo_avg_area) { + high = mid - 1; + } else { + low = mid + 1; + } + } + + // Calibration complete calib_done_ = true; detector_->binary_thres = best_thres;