diff --git a/src/rm_auto_aim/armor_yolo_detect/include/armor_yolo_detect/armor_yolo_detector_node.hpp b/src/rm_auto_aim/armor_yolo_detect/include/armor_yolo_detect/armor_yolo_detector_node.hpp index 29b33ad..cd3a40a 100644 --- a/src/rm_auto_aim/armor_yolo_detect/include/armor_yolo_detect/armor_yolo_detector_node.hpp +++ b/src/rm_auto_aim/armor_yolo_detect/include/armor_yolo_detect/armor_yolo_detector_node.hpp @@ -146,7 +146,9 @@ private: // Binary threshold calibration bool calib_binarythres_ = false; bool calib_done_ = false; + bool calib_searching_ = true; // true=searching for valid thres, false=doing binary search int calib_frame_count_ = 0; + int calib_current_thres_ = 250; std::vector calib_size_errors_; std::string calib_save_yaml_path_; void performBinaryThresCalibration(const sensor_msgs::msg::Image::ConstSharedPtr& img_msg); 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 e16b8ef..93f3586 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 @@ -353,7 +353,9 @@ std::unique_ptr ArmorYoloDetectorNode::initDetector() { // Binary threshold calibration mode calib_binarythres_ = this->declare_parameter("calib_binarythres", false); calib_done_ = false; + calib_searching_ = true; calib_frame_count_ = 0; + calib_current_thres_ = 250; calib_size_errors_.reserve(100); calib_save_yaml_path_ = this->declare_parameter("calib_save_yaml_path", std::string("")); @@ -525,8 +527,10 @@ rcl_interfaces::msg::SetParametersResult ArmorYoloDetectorNode::onSetParameters( calib_binarythres_ = param.as_bool(); if (calib_binarythres_) { calib_done_ = false; + calib_searching_ = true; calib_frame_count_ = 0; - FYT_INFO("armor_yolo_detect", "Binary threshold calibration started"); + calib_current_thres_ = 250; + FYT_INFO("armor_yolo_detect", "Binary threshold calibration started (searching from 250)"); } } else if (param.get_name() == "debug.enable_terminal_log") { debug_terminal_log_ = param.as_bool(); @@ -630,6 +634,8 @@ void ArmorYoloDetectorNode::performBinaryThresCalibration( if (yolo_objects.empty()) { FYT_INFO("armor_yolo_detect", "Calibration: waiting for YOLO detection..."); calib_frame_count_ = 0; // Reset counter when YOLO loses detection + calib_searching_ = true; // Reset to searching mode + calib_current_thres_ = 250; // Reset threshold return; } @@ -652,8 +658,32 @@ void ArmorYoloDetectorNode::performBinaryThresCalibration( } yolo_avg_area /= yolo_objects.size(); - // Binary search for optimal threshold - int low = 50, high = 250, best_thres = detector_->binary_thres; + if (calib_searching_) { + // Phase 1: Decrement threshold from 250 until detection succeeds + // Temporarily set binary threshold and run detection + int original_thres = detector_->binary_thres; + detector_->binary_thres = calib_current_thres_; + auto armors = detector_->processROIs(img, gray_img, rois); + detector_->binary_thres = original_thres; + + if (armors.empty()) { + // No detection at this threshold, decrement and try again next frame + calib_current_thres_--; + if (calib_current_thres_ < 30) { + calib_current_thres_ = 30; // Don't go too low + } + FYT_INFO("armor_yolo_detect", "Calibration searching: thres={}, no detection yet", calib_current_thres_); + return; + } else { + // Detection succeeded! Switch to binary search mode + calib_searching_ = false; + calib_frame_count_ = 0; + FYT_INFO("armor_yolo_detect", "Calibration: detection found at thres={}, starting binary search", calib_current_thres_); + } + } + + // 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 @@ -668,13 +698,8 @@ void ArmorYoloDetectorNode::performBinaryThresCalibration( detector_->binary_thres = original_thres; if (armors.empty()) { - // No armors detected at this threshold, adjust search range - if (iter == 0) { - // First iteration, try higher threshold - low = mid + 1; - } else { - high = mid - 1; - } + // No armors detected at this threshold, need lower threshold + high = mid - 1; continue; } @@ -712,9 +737,12 @@ void ArmorYoloDetectorNode::performBinaryThresCalibration( } } - // Only count frames where we got valid YOLO detection + // Update threshold to best found + calib_current_thres_ = best_thres; + + // Only count frames where we got valid detection calib_frame_count_++; - FYT_INFO("armor_yolo_detect", "Calibration progress: {}/10", calib_frame_count_); + FYT_INFO("armor_yolo_detect", "Calibration progress: {}/10 (thres={})", calib_frame_count_, best_thres); if (calib_frame_count_ >= 10) { calib_done_ = true;