cyy/yq_2026_duo
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

add auto binarythres fix logic 2

Browse Source
This commit is contained in:
cyy_mac
2026-03-26 06:59:01 +08:00
parent b826235fd7
commit f34cefe597
1 changed files with 74 additions and 56 deletions
Download Patch File Download Diff File Expand all files Collapse all files

130
src/rm_auto_aim/armor_yolo_detect/src/armor_yolo_detector_node.cpp
View File

@@ -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<double>::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<double>::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;