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

add auto binarythres fix logic strong 11

Browse Source
This commit is contained in:
cyy_mac
2026-03-26 07:14:14 +08:00
parent f387f4ecc3
commit b56999f3f3
2 changed files with 66 additions and 43 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -149,6 +149,8 @@ private:
int calib_current_thres_ = 250;
int calib_best_thres_ = 160;
double calib_best_error_ = std::numeric_limits<double>::max();
int calib_frame_skip_ = 0; // Frame counter to slow down decrement
int calib_frame_skip_interval_ = 10; // Decrement every 10 frames
std::string calib_save_yaml_path_;
void performBinaryThresCalibration(const sensor_msgs::msg::Image::ConstSharedPtr& img_msg);
void saveBinaryThresToYaml(int binary_thres);

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

@@ -31,6 +31,7 @@
#include <numeric>
#include <sstream>
#include <string>
#include <thread>
#include <vector>
#include <cv_bridge/cv_bridge.h>
@@ -356,6 +357,8 @@ std::unique_ptr<Detector> ArmorYoloDetectorNode::initDetector() {
calib_current_thres_ = 250;
calib_best_thres_ = 160;
calib_best_error_ = std::numeric_limits<double>::max();
calib_frame_skip_ = 0;
calib_frame_skip_interval_ = 10;
calib_save_yaml_path_ = this->declare_parameter("calib_save_yaml_path", std::string(""));
// Light detection params
@@ -627,8 +630,14 @@ void ArmorYoloDetectorNode::performBinaryThresCalibration(
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));
auto result1 = this->set_parameter(rclcpp::Parameter("binary_thres", calib_best_thres_));
FYT_INFO("armor_yolo_detect", "Set binary_thres={}, success={}", calib_best_thres_, result1.successful);
// Small delay before disabling calibration
std::this_thread::sleep_for(std::chrono::milliseconds(100));
auto result2 = this->set_parameter(rclcpp::Parameter("calib_binarythres", false));
FYT_INFO("armor_yolo_detect", "Set calib_binarythres=false, success={}", result2.successful);
// Save to yaml file if path is configured
if (!calib_save_yaml_path_.empty()) {
@@ -679,53 +688,65 @@ void ArmorYoloDetectorNode::performBinaryThresCalibration(
detector_->binary_thres = original_thres;
if (armors.empty()) {
// No detection at this threshold, decrement and try again next frame
// No detection at this threshold
FYT_INFO("armor_yolo_detect", "Calibration: thres={}, no detection", calib_current_thres_);
calib_current_thres_--;
return;
}
} else {
// 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();
// 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();
double error = std::abs(trad_avg_area - yolo_avg_area);
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);
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_);
}
// Decrement threshold for next frame
calib_current_thres_--;
// 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 = 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_);
// 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_);
}
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.");
// 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 = calib_best_thres_;
// Update parameter so it persists
auto result1 = this->set_parameter(rclcpp::Parameter("binary_thres", calib_best_thres_));
FYT_INFO("armor_yolo_detect", "Set binary_thres={}, success={}", calib_best_thres_, result1.successful);
// Small delay before disabling calibration
std::this_thread::sleep_for(std::chrono::milliseconds(100));
auto result2 = this->set_parameter(rclcpp::Parameter("calib_binarythres", false));
FYT_INFO("armor_yolo_detect", "Set calib_binarythres=false, success={}", result2.successful);
// 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;
}
}
// Only decrement every few frames to slow down
calib_frame_skip_++;
if (calib_frame_skip_ >= calib_frame_skip_interval_) {
calib_frame_skip_ = 0;
calib_current_thres_--;
FYT_INFO("armor_yolo_detect", "Calibration: decrementing to thres={}", calib_current_thres_);
} else {
FYT_INFO("armor_yolo_detect", "Calibration: skipping (frame {}/{})", calib_frame_skip_, calib_frame_skip_interval_);
}
}