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

add auto binarythres fix logic strong

Browse Source
This commit is contained in:
cyy_mac
2026-03-26 07:09:47 +08:00
parent f34cefe597
commit f387f4ecc3
2 changed files with 64 additions and 116 deletions
Download Patch File Download Diff File Expand all files Collapse all files

5
src/rm_auto_aim/armor_yolo_detect/include/armor_yolo_detect/armor_yolo_detector_node.hpp
View File

@@ -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<double> calib_size_errors_;
int calib_best_thres_ = 160;
double calib_best_error_ = std::numeric_limits<double>::max();
std::string calib_save_yaml_path_;
void performBinaryThresCalibration(const sensor_msgs::msg::Image::ConstSharedPtr& img_msg);
void saveBinaryThresToYaml(int binary_thres);

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

@@ -353,10 +353,9 @@ std::unique_ptr<Detector> 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<double>::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<double>::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<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
// 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.");
}
}