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 cd3a40a..3337100 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,10 +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_; + int calib_best_thres_ = 160; + double calib_best_error_ = std::numeric_limits::max(); std::string calib_save_yaml_path_; void performBinaryThresCalibration(const sensor_msgs::msg::Image::ConstSharedPtr& img_msg); void saveBinaryThresToYaml(int binary_thres); 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 cd88e51..977049b 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,10 +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_best_thres_ = 160; + calib_best_error_ = std::numeric_limits::max(); calib_save_yaml_path_ = this->declare_parameter("calib_save_yaml_path", std::string("")); // Light detection params @@ -527,9 +526,9 @@ rcl_interfaces::msg::SetParametersResult ArmorYoloDetectorNode::onSetParameters( calib_binarythres_ = param.as_bool(); if (calib_binarythres_) { calib_done_ = false; - calib_searching_ = true; - calib_frame_count_ = 0; calib_current_thres_ = 250; + calib_best_thres_ = 160; + calib_best_error_ = std::numeric_limits::max(); FYT_INFO("armor_yolo_detect", "Binary threshold calibration started (searching from 250)"); } } else if (param.get_name() == "debug.enable_terminal_log") { @@ -622,6 +621,25 @@ void ArmorYoloDetectorNode::performBinaryThresCalibration( return; } + // Check if we've reached minimum threshold + if (calib_current_thres_ < 30) { + calib_done_ = true; + detector_->binary_thres = calib_best_thres_; + + // Update parameter so it persists + this->set_parameter(rclcpp::Parameter("binary_thres", calib_best_thres_)); + this->set_parameter(rclcpp::Parameter("calib_binarythres", false)); + + // Save to yaml file if path is configured + if (!calib_save_yaml_path_.empty()) { + saveBinaryThresToYaml(calib_best_thres_); + } + + FYT_INFO("armor_yolo_detect", "=== Binary threshold calibration complete: {} ===", calib_best_thres_); + FYT_INFO("armor_yolo_detect", "Calibration result saved. Set calib_binarythres=false to disable calibration mode."); + return; + } + auto img = cv_bridge::toCvShare(img_msg, "bgr8")->image; cv::Mat gray_img; cv::cvtColor(img, gray_img, cv::COLOR_BGR2GRAY); @@ -629,13 +647,9 @@ void ArmorYoloDetectorNode::performBinaryThresCalibration( // Get YOLO detection for ROIs auto yolo_objects = detector_->detectRaw(img); - // If YOLO doesn't detect anything, stop calibration this frame - // Calibration will resume when YOLO detects again + // If YOLO doesn't detect anything, wait for next frame 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 + FYT_INFO("armor_yolo_detect", "Calibration: waiting for YOLO detection (thres={})...", calib_current_thres_); return; } @@ -658,124 +672,59 @@ void ArmorYoloDetectorNode::performBinaryThresCalibration( } yolo_avg_area /= yolo_objects.size(); - 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; + // Test current threshold + 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 stabilization phase - calib_searching_ = false; - 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 - } + if (armors.empty()) { + // No detection at this threshold, decrement and try again next frame + FYT_INFO("armor_yolo_detect", "Calibration: thres={}, no detection", calib_current_thres_); + calib_current_thres_--; + return; } - // 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 = calib_current_thres_; - auto armors = detector_->processROIs(img, gray_img, rois); - detector_->binary_thres = original_thres; + // Detection succeeded! Calculate error + 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(); - if (armors.empty()) { - // 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; - } + double error = std::abs(trad_avg_area - yolo_avg_area); + + FYT_INFO("armor_yolo_detect", "Calibration: thres={}, yolo_area={:.1f}, trad_area={:.1f}, error={:.1f}", + calib_current_thres_, yolo_avg_area, trad_avg_area, error); + + // Track best threshold (minimum error) + if (error < calib_best_error_) { + calib_best_error_ = error; + calib_best_thres_ = calib_current_thres_; + FYT_INFO("armor_yolo_detect", "Calibration: new best thres={}, error={:.1f}", calib_best_thres_, calib_best_error_); } - // Detection is stable, increment counter - calib_frame_count_++; - FYT_INFO("armor_yolo_detect", "Calibration stability: {}/5 (thres={})", calib_frame_count_, calib_current_thres_); + // Decrement threshold for next frame + calib_current_thres_--; - // 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 + // Check if we should finish (error is small enough) + constexpr double error_tolerance = 100.0; // pixels^2 + if (error <= error_tolerance) { calib_done_ = true; - detector_->binary_thres = best_thres; + detector_->binary_thres = calib_best_thres_; // Update parameter so it persists - this->set_parameter(rclcpp::Parameter("binary_thres", best_thres)); + this->set_parameter(rclcpp::Parameter("binary_thres", calib_best_thres_)); this->set_parameter(rclcpp::Parameter("calib_binarythres", false)); // Save to yaml file if path is configured if (!calib_save_yaml_path_.empty()) { - saveBinaryThresToYaml(best_thres); + saveBinaryThresToYaml(calib_best_thres_); } - FYT_INFO("armor_yolo_detect", "=== Binary threshold calibration complete: {} ===", best_thres); + FYT_INFO("armor_yolo_detect", "=== Binary threshold calibration complete: {} ===", calib_best_thres_); FYT_INFO("armor_yolo_detect", "Calibration result saved. Set calib_binarythres=false to disable calibration mode."); } }