From 46bf3c9b5256505c8c3a8de8cd432e05c4ed2a0c Mon Sep 17 00:00:00 2001 From: MobKBK <15059009+mobkbk@user.noreply.gitee.com> Date: Mon, 23 Mar 2026 07:27:01 +0800 Subject: [PATCH] feat(armor_yolo_detect): hybrid detection with YOLO ROI + traditional vision refinement - Use YOLO to detect armor ROI regions instead of full image - Expand ROI by configurable pixels (default 75) for light detection - Apply traditional binary + light detection within ROI - Keep YOLO's number classification result - Use LightCornerCorrector for corner refinement - Add debug visualization for ROI regions (blue rectangles) - Compatible input/output with armor_detector node Co-Authored-By: Claude Opus 4.6 --- .../armor_yolo_detect/CMakeLists.txt | 2 + .../armor_yolo_detect/armor_yolo_detector.hpp | 66 +++- .../src/armor_yolo_detector.cpp | 326 +++++++++++++++++- .../src/armor_yolo_detector_node.cpp | 106 +++--- 4 files changed, 426 insertions(+), 74 deletions(-) diff --git a/src/rm_auto_aim/armor_yolo_detect/CMakeLists.txt b/src/rm_auto_aim/armor_yolo_detect/CMakeLists.txt index cdad48d..fb30589 100644 --- a/src/rm_auto_aim/armor_yolo_detect/CMakeLists.txt +++ b/src/rm_auto_aim/armor_yolo_detect/CMakeLists.txt @@ -19,6 +19,7 @@ find_package(ament_cmake_auto REQUIRED) find_package(OpenCV REQUIRED) find_package(fmt REQUIRED) find_package(Sophus REQUIRED) +find_package(armor_detector REQUIRED) ament_auto_find_build_dependencies() # TensorRT @@ -76,6 +77,7 @@ target_link_libraries(${PROJECT_NAME} nvinfer_plugin nvonnxparser fmt::fmt + armor_detector ) rclcpp_components_register_node(${PROJECT_NAME} diff --git a/src/rm_auto_aim/armor_yolo_detect/include/armor_yolo_detect/armor_yolo_detector.hpp b/src/rm_auto_aim/armor_yolo_detect/include/armor_yolo_detect/armor_yolo_detector.hpp index 1260031..0f3be87 100644 --- a/src/rm_auto_aim/armor_yolo_detect/include/armor_yolo_detect/armor_yolo_detector.hpp +++ b/src/rm_auto_aim/armor_yolo_detect/include/armor_yolo_detect/armor_yolo_detector.hpp @@ -1,5 +1,8 @@ // Copyright chenyy 2026. Licensed under the MIT License. -// YOLOv5-based armor detector +// YOLOv5-based armor detector with traditional vision refinement +// +// Uses YOLO to provide ROI regions, then applies traditional binary + light detection +// for precise corner localization. Input/output compatible with armor_detector. // // Copyright (C) FYT Vision Group. All rights reserved. // @@ -26,6 +29,8 @@ // project #include "armor_yolo_detect/types.hpp" #include "armor_yolo_detect/yolo_tensorrt.hpp" +#include "armor_detector/types.hpp" +#include "armor_detector/light_corner_corrector.hpp" #include "rm_utils/common.hpp" namespace armor_yolo_detect { @@ -36,24 +41,47 @@ public: float conf_threshold = 0.65f; float nms_threshold = 0.45f; int detect_color = 0; // 0=red, 1=blue + // ROI expansion pixels + int roi_expand_pixel = 75; + }; + + // Light detection params (from armor_detector) + struct LightParams { + double min_ratio = 0.08; + double max_ratio = 0.4; + double max_angle = 40.0; + bool use_fit_line = true; + int color_diff_thresh = 25; + }; + + // Armor matching params (from armor_detector) + struct ArmorParams { + double min_light_ratio = 0.6; + double min_small_center_distance = 0.8; + double max_small_center_distance = 3.2; + double min_large_center_distance = 3.2; + double max_large_center_distance = 5.0; + double max_angle = 35.0; }; Detector() = default; ~Detector() = default; /** - * @brief Initialize detector with TensorRT engine + * @brief Initialize detector with TensorRT engine and traditional vision params * @param engine_path Path to TensorRT engine file * @param params Detection parameters + * @param light_params Light bar detection parameters + * @param armor_params Armor matching parameters */ bool init(const std::string& engine_path, const Params& params); /** - * @brief Detect armors in image - * @param input Input image (RGB) - * @return Vector of detected Armors + * @brief Detect armors using YOLO for ROI + traditional vision for refinement + * @param input Input image (BGR) + * @return Vector of detected Armors (fyt::auto_aim::Armor type for compatibility) */ - std::vector detect(const cv::Mat& input); + std::vector detect(const cv::Mat& input); /** * @brief Detect armors and return raw YOLO objects @@ -64,14 +92,38 @@ public: // Parameters Params params; + LightParams light_params; + ArmorParams armor_params; + int binary_thres = 160; // Debug bool enable_debug = false; cv::Mat debug_img; + // Store ROI rectangles for debug drawing + std::vector debug_rois_; + + // Draw detection results for debug visualization + void drawResults(cv::Mat& img) noexcept; + private: + // Traditional vision methods + std::vector findLightsInROI( + const cv::Mat& rgb_img, const cv::Mat& gray_img, const cv::Mat& binary_img, + const cv::Rect& roi); + bool isLight(const fyt::auto_aim::Light& light); + bool containLight(const int i, const int j, const std::vector& lights); + fyt::auto_aim::ArmorType isArmor(const fyt::auto_aim::Light& light_1, const fyt::auto_aim::Light& light_2); + std::vector matchLightsInROI(const std::vector& lights); + void correctCorners(fyt::auto_aim::Armor& armor, const cv::Mat& gray_img); + + // Expand YOLO bbox to ROI + cv::Rect expandBBox(const cv::Rect& bbox, const cv::Size& img_size, int pixel_expand); + std::unique_ptr tensorrt_; - std::vector armors_; + std::vector yolo_objects_; + std::vector armors_; + std::unique_ptr corner_corrector_; }; } // namespace armor_yolo_detect diff --git a/src/rm_auto_aim/armor_yolo_detect/src/armor_yolo_detector.cpp b/src/rm_auto_aim/armor_yolo_detect/src/armor_yolo_detector.cpp index e301ac1..d857bc1 100644 --- a/src/rm_auto_aim/armor_yolo_detect/src/armor_yolo_detector.cpp +++ b/src/rm_auto_aim/armor_yolo_detect/src/armor_yolo_detector.cpp @@ -1,5 +1,8 @@ // Copyright chenyy 2026. Licensed under the MIT License. -// YOLOv5-based armor detector implementation +// YOLOv5-based armor detector with traditional vision refinement +// +// Uses YOLO to provide ROI regions, then applies traditional binary + light detection +// for precise corner localization. Input/output compatible with armor_detector. // // Copyright (C) FYT Vision Group. All rights reserved. // @@ -33,34 +36,323 @@ bool Detector::init(const std::string& engine_path, const Params& params) { tensorrt_->setPostprocessOptions(params.conf_threshold, params.nms_threshold, 300); + // Initialize corner corrector for precise corner refinement + corner_corrector_ = std::make_unique(); + return true; } -std::vector Detector::detect(const cv::Mat& input) { +std::vector Detector::detect(const cv::Mat& input) { armors_.clear(); + debug_rois_.clear(); - // Run inference - auto yolo_objects = tensorrt_->infer(input, params.detect_color); + // 1. Run YOLO inference to get ROI regions + yolo_objects_ = tensorrt_->infer(input, params.detect_color); - // Convert YOLO objects to Armors - for (const auto& obj : yolo_objects) { - // Calculate center from landmarks - cv::Point2f center( - (obj.landmarks[0] + obj.landmarks[2] + obj.landmarks[4] + obj.landmarks[6]) / 4, - (obj.landmarks[1] + obj.landmarks[3] + obj.landmarks[5] + obj.landmarks[7]) / 4); - - Armor armor(obj, center); - armor.number_class = static_cast(obj.label); - armor.confidence = obj.prob; - - armors_.push_back(armor); + if (yolo_objects_.empty()) { + return armors_; } - return armors_; + // 2. Preprocess image once + cv::Mat gray_img; + cv::cvtColor(input, gray_img, cv::COLOR_BGR2GRAY); + + cv::Mat full_binary_img; + cv::threshold(gray_img, full_binary_img, binary_thres, 255, cv::THRESH_BINARY); + + // 3. For each YOLO detection, create ROI and detect lights + for (const auto& yolo_obj : yolo_objects_) { + // Get bounding box from YOLO + cv::Rect_ bbox = yolo_obj.rect; + + // Expand bbox to create ROI + cv::Rect roi = expandBBox(bbox, input.size(), params.roi_expand_pixel); + + // Clip ROI to image bounds + roi = roi & cv::Rect(0, 0, input.size().width, input.size().height); + + if (roi.area() <= 0) { + continue; + } + + // Store ROI for debug visualization + debug_rois_.push_back(roi); + + // Extract ROI images + cv::Mat roi_gray = gray_img(roi); + cv::Mat roi_binary = full_binary_img(roi); + + // 4. Find lights within this ROI + auto lights = findLightsInROI(input, roi_gray, roi_binary, roi); + + if (lights.empty()) { + continue; + } + + // 5. Match lights to form armors within this ROI + auto roi_armors = matchLightsInROI(lights); + + // 6. Process each armor: assign YOLO classification info + for (auto& armor : roi_armors) { + // Convert ROI coordinates to global coordinates + armor.left_light.top += cv::Point2f(static_cast(roi.x), static_cast(roi.y)); + armor.left_light.bottom += cv::Point2f(static_cast(roi.x), static_cast(roi.y)); + armor.left_light.center += cv::Point2f(static_cast(roi.x), static_cast(roi.y)); + armor.right_light.top += cv::Point2f(static_cast(roi.x), static_cast(roi.y)); + armor.right_light.bottom += cv::Point2f(static_cast(roi.x), static_cast(roi.y)); + armor.right_light.center += cv::Point2f(static_cast(roi.x), static_cast(roi.y)); + armor.center = (armor.left_light.center + armor.right_light.center) / 2; + + // Use YOLO's classification result (color and number class) + armor.classfication_result = numberClassToString(static_cast(yolo_obj.label)); + armor.confidence = yolo_obj.prob; + + // Use armor type from traditional detection (more accurate) + // YOLO info available if needed: yolo_obj.color + + // 7. Correct corners using traditional method + correctCorners(armor, gray_img); + + armors_.push_back(armor); + } + } + + return armors; } std::vector Detector::detectRaw(const cv::Mat& input) { return tensorrt_->infer(input, params.detect_color); } +cv::Rect Detector::expandBBox(const cv::Rect_& bbox, const cv::Size& img_size, int pixel_expand) { + int x = static_cast(bbox.x - pixel_expand); + int y = static_cast(bbox.y - pixel_expand); + int w = static_cast(bbox.width + 2 * pixel_expand); + int h = static_cast(bbox.height + 2 * pixel_expand); + + return cv::Rect(x, y, w, h); +} + +std::vector Detector::findLightsInROI( + const cv::Mat& rgb_img, + const cv::Mat& gray_img, + const cv::Mat& binary_img, + const cv::Rect& roi) { + std::vector lights; + + using std::vector; + vector> contours; + vector hierarchy; + cv::findContours(binary_img, contours, hierarchy, cv::RETR_EXTERNAL, cv::CHAIN_APPROX_SIMPLE); + + lights.reserve(contours.size()); + + for (const auto& contour : contours) { + if (contour.size() < 6) continue; + + fyt::auto_aim::Light light; + std::vector points; + + if (light_params.use_fit_line) { + auto b_rect = cv::boundingRect(contour); + auto r_rect = cv::minAreaRect(contour); + cv::Mat mask = cv::Mat::zeros(b_rect.size(), CV_8UC1); + vector mask_contour; + mask_contour.reserve(contour.size()); + for (const auto& p : contour) { + mask_contour.emplace_back(p - cv::Point(b_rect.x, b_rect.y)); + } + cv::fillPoly(mask, {mask_contour}, 255); + cv::findNonZero(mask, points); + + if (points.empty()) continue; + + cv::Vec4f return_param; + cv::fitLine(points, return_param, cv::DIST_L2, 0, 0.01, 0.01); + cv::Point2f top, bottom; + if (int(return_param[0] * 100) == 100 || int(return_param[1] * 100) == 0) { + top = cv::Point2f(b_rect.x + b_rect.width / 2, b_rect.y); + bottom = cv::Point2f(b_rect.x + b_rect.width / 2, b_rect.y + b_rect.height); + } else { + auto k = return_param[1] / return_param[0]; + auto b = (return_param[3] + b_rect.y) - k * (return_param[2] + b_rect.x); + top = cv::Point2f((b_rect.y - b) / k, b_rect.y); + bottom = cv::Point2f((b_rect.y + b_rect.height - b) / k, b_rect.y + b_rect.height); + } + + double width = std::min(r_rect.size.width, r_rect.size.height); + double length = cv::norm(top - bottom); + light = fyt::auto_aim::Light(r_rect, top, bottom, length, width); + } else { + light = fyt::auto_aim::Light(contour); + } + + if (isLight(light)) { + // Color detection within ROI + int sum_r = 0; + int sum_b = 0; + int sample_count = 0; + + if (light_params.use_fit_line && !points.empty()) { + auto b_rect = cv::boundingRect(contour); + for (const auto& point : points) { + const int x = point.x + b_rect.x; + const int y = point.y + b_rect.y; + const auto& pixel = rgb_img.at(y, x); + sum_r += pixel[0]; + sum_b += pixel[2]; + ++sample_count; + } + } else { + for (const auto& point : contour) { + const auto& pixel = rgb_img.at(point.y, point.x); + sum_r += pixel[0]; + sum_b += pixel[2]; + ++sample_count; + } + } + + if (sample_count > 0 && + std::abs(sum_r - sum_b) / sample_count > light_params.color_diff_thresh) { + light.color = sum_r > sum_b ? EnemyColor::RED : EnemyColor::BLUE; + } + lights.emplace_back(light); + } + } + + std::sort(lights.begin(), lights.end(), [](const fyt::auto_aim::Light& l1, const fyt::auto_aim::Light& l2) { + return l1.center.x < l2.center.x; + }); + + return lights; +} + +bool Detector::isLight(const fyt::auto_aim::Light& light) { + float ratio = light.width / light.length; + bool ratio_ok = light_params.min_ratio < ratio && ratio < light_params.max_ratio; + bool angle_ok = light.tilt_angle < light_params.max_angle; + return ratio_ok && angle_ok; +} + +bool Detector::containLight(const int i, const int j, const std::vector& lights) { + const fyt::auto_aim::Light& light_1 = lights.at(i); + const fyt::auto_aim::Light& light_2 = lights.at(j); + auto points = std::vector{light_1.top, light_1.bottom, light_2.top, light_2.bottom}; + auto bounding_rect = cv::boundingRect(points); + double avg_length = (light_1.length + light_2.length) / 2.0; + double avg_width = (light_1.width + light_2.width) / 2.0; + + for (int k = i + 1; k < j; k++) { + const fyt::auto_aim::Light& test_light = lights.at(k); + + if (test_light.width > 2 * avg_width) { + continue; + } + if (test_light.length < 0.5 * avg_length) { + continue; + } + + if (bounding_rect.contains(test_light.top) || bounding_rect.contains(test_light.bottom) || + bounding_rect.contains(test_light.center)) { + return true; + } + } + return false; +} + +fyt::auto_aim::ArmorType Detector::isArmor( + const fyt::auto_aim::Light& light_1, + const fyt::auto_aim::Light& light_2) { + float light_length_ratio = light_1.length < light_2.length + ? light_1.length / light_2.length + : light_2.length / light_1.length; + bool light_ratio_ok = light_length_ratio > armor_params.min_light_ratio; + + float avg_light_length = (light_1.length + light_2.length) / 2; + float center_distance = cv::norm(light_1.center - light_2.center) / avg_light_length; + bool center_distance_ok = + (armor_params.min_small_center_distance <= center_distance && + center_distance < armor_params.max_small_center_distance) || + (armor_params.min_large_center_distance <= center_distance && + center_distance < armor_params.max_large_center_distance); + + cv::Point2f diff = light_1.center - light_2.center; + float angle = std::abs(std::atan(diff.y / diff.x)) / CV_PI * 180; + bool angle_ok = angle < armor_params.max_angle; + + bool is_armor = light_ratio_ok && center_distance_ok && angle_ok; + + if (is_armor) { + return center_distance > 3.5 ? fyt::auto_aim::ArmorType::LARGE : fyt::auto_aim::ArmorType::SMALL; + } + return fyt::auto_aim::ArmorType::INVALID; +} + +std::vector Detector::matchLightsInROI(const std::vector& lights) { + std::vector armors; + + EnemyColor detect_color = params.detect_color == 0 ? EnemyColor::RED : EnemyColor::BLUE; + + for (auto light_1 = lights.begin(); light_1 != lights.end(); light_1++) { + // Skip lights not matching target color (or accept if color not determined) + if (light_1->color != EnemyColor::WHITE && light_1->color != detect_color) { + continue; + } + + double max_iter_width = light_1->length * armor_params.max_large_center_distance; + + for (auto light_2 = light_1 + 1; light_2 != lights.end(); light_2++) { + if (light_2->color != EnemyColor::WHITE && light_2->color != detect_color) { + continue; + } + if (containLight(light_1 - lights.begin(), light_2 - lights.begin(), lights)) { + continue; + } + if (light_2->center.x - light_1->center.x > max_iter_width) { + break; + } + + auto type = isArmor(*light_1, *light_2); + if (type != fyt::auto_aim::ArmorType::INVALID) { + auto armor = fyt::auto_aim::Armor(*light_1, *light_2); + armor.type = type; + armors.emplace_back(armor); + } + } + } + + return armors; +} + +void Detector::correctCorners(fyt::auto_aim::Armor& armor, const cv::Mat& gray_img) { + if (corner_corrector_ != nullptr) { + corner_corrector_->correctCorners(armor, gray_img); + } +} + +void Detector::drawResults(cv::Mat& img) noexcept { + // Draw ROI rectangles (expanded YOLO bboxes) in blue + for (const auto& roi : debug_rois_) { + cv::rectangle(img, roi, cv::Scalar(255, 0, 0), 2); + } + + // Draw armor outlines + for (const auto& armor : armors_) { + // Draw armor outline + cv::line( + img, armor.left_light.top, armor.left_light.bottom, cv::Scalar(0, 255, 0), 1, cv::LINE_AA); + cv::line( + img, armor.right_light.bottom, armor.right_light.top, cv::Scalar(0, 255, 0), 1, cv::LINE_AA); + cv::line( + img, armor.left_light.top, armor.right_light.top, cv::Scalar(0, 255, 0), 1, cv::LINE_AA); + cv::line( + img, armor.right_light.bottom, armor.left_light.bottom, cv::Scalar(0, 255, 0), 1, cv::LINE_AA); + + // Draw number and confidence + std::string text = fyt::auto_aim::armorTypeToString(armor.type) + " " + armor.classfication_result; + cv::putText( + img, text, armor.left_light.top, cv::FONT_HERSHEY_SIMPLEX, 0.8, cv::Scalar(0, 255, 255), 2); + } +} + } // namespace armor_yolo_detect 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 addec42..9ad0ebb 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 @@ -336,6 +336,7 @@ std::unique_ptr ArmorYoloDetectorNode::initDetector() { params.conf_threshold = this->declare_parameter("conf_threshold", 0.65); params.nms_threshold = this->declare_parameter("nms_threshold", 0.50); params.detect_color = this->declare_parameter("detect_color", -1); + params.roi_expand_pixel = this->declare_parameter("roi_expand_pixel", 75); auto detector = std::make_unique(); if (!detector->init(engine_path.string(), params)) { @@ -343,6 +344,30 @@ std::unique_ptr ArmorYoloDetectorNode::initDetector() { return nullptr; } + // Binary threshold for light detection + detector->binary_thres = this->declare_parameter("binary_thres", 160); + + // Light detection params + detector->light_params.min_ratio = this->declare_parameter("light.min_ratio", 0.08); + detector->light_params.max_ratio = this->declare_parameter("light.max_ratio", 0.4); + detector->light_params.max_angle = this->declare_parameter("light.max_angle", 40.0); + detector->light_params.use_fit_line = this->declare_parameter("light.use_fit_line", true); + detector->light_params.color_diff_thresh = + static_cast(this->declare_parameter("light.color_diff_thresh", 25)); + + // Armor matching params + detector->armor_params.min_light_ratio = + this->declare_parameter("armor.min_light_ratio", 0.6); + detector->armor_params.min_small_center_distance = + this->declare_parameter("armor.min_small_center_distance", 0.8); + detector->armor_params.max_small_center_distance = + this->declare_parameter("armor.max_small_center_distance", 3.2); + detector->armor_params.min_large_center_distance = + this->declare_parameter("armor.min_large_center_distance", 3.2); + detector->armor_params.max_large_center_distance = + this->declare_parameter("armor.max_large_center_distance", 5.0); + detector->armor_params.max_angle = this->declare_parameter("armor.max_angle", 35.0); + FYT_INFO("armor_yolo_detect", "Inference backend: TensorRT ({})", engine_path.string()); // Set dynamic parameter callback @@ -362,70 +387,25 @@ std::vector ArmorYoloDetectorNode::detectArmors( // Convert ROS img to cv::Mat auto img = cv_bridge::toCvShare(img_msg, "bgr8")->image; - // Run detection - auto yolo_armors = detector_->detect(img); + // Run detection: YOLO for ROI + traditional vision for refinement + auto armors = detector_->detect(img); if (detect_ms != nullptr) { *detect_ms = durationMs(std::chrono::steady_clock::now() - stage_start); } - // Convert to auto_aim armor format - std::vector armors; - for (const auto& yolo_armor : yolo_armors) { - armors.push_back(yolo_armor.toAutoAimArmor()); - } - // Debug image publishing stage_start = std::chrono::steady_clock::now(); if (debug_ && debug_result_img_) { detector_->debug_img = img.clone(); cv::putText(detector_->debug_img, - "det=" + std::to_string(yolo_armors.size()), + "det=" + std::to_string(armors.size()), cv::Point(20, 40), cv::FONT_HERSHEY_SIMPLEX, 1.0, cv::Scalar(255, 255, 0), 2); - // Draw detection results - for (const auto& armor : yolo_armors) { - // Draw landmarks - std::vector pts; - for (int i = 0; i < 6; i++) { - pts.emplace_back(static_cast(armor.landmark_points[i].x), - static_cast(armor.landmark_points[i].y)); - cv::circle(detector_->debug_img, - cv::Point(static_cast(armor.landmark_points[i].x), - static_cast(armor.landmark_points[i].y)), - 3, cv::Scalar(255, 0, 0), -1); - } - - // Draw polygon - const cv::Point* pts_ptr = pts.data(); - int npts = static_cast(pts.size()); - cv::polylines(detector_->debug_img, &pts_ptr, &npts, 1, true, cv::Scalar(0, 255, 0), 2); - - float min_x = std::numeric_limits::max(); - float max_x = std::numeric_limits::lowest(); - float min_y = std::numeric_limits::max(); - float max_y = std::numeric_limits::lowest(); - for (const auto& p : armor.landmark_points) { - min_x = std::min(min_x, p.x); - max_x = std::max(max_x, p.x); - min_y = std::min(min_y, p.y); - max_y = std::max(max_y, p.y); - } - const cv::Rect bbox( - cv::Point(static_cast(min_x), static_cast(min_y)), - cv::Point(static_cast(max_x), static_cast(max_y))); - cv::rectangle(detector_->debug_img, bbox, cv::Scalar(0, 128, 255), 2); - - // Draw label - std::ostringstream oss; - oss << numberClassToString(armor.number_class) << " " << std::fixed << std::setprecision(2) - << armor.confidence; - cv::putText(detector_->debug_img, oss.str(), - cv::Point(static_cast(armor.center.x), static_cast(armor.center.y)), - cv::FONT_HERSHEY_SIMPLEX, 0.7, cv::Scalar(0, 255, 255), 2); - } + // Draw detection results using armor_detector's drawResults style + detector_->drawResults(detector_->debug_img); // Publish result image result_img_pub_.publish( @@ -457,6 +437,32 @@ rcl_interfaces::msg::SetParametersResult ArmorYoloDetectorNode::onSetParameters( detector_->params.nms_threshold = param.as_double(); } else if (param.get_name() == "detect_color") { detector_->params.detect_color = param.as_int(); + } else if (param.get_name() == "roi_expand_pixel") { + detector_->params.roi_expand_pixel = param.as_int(); + } else if (param.get_name() == "binary_thres") { + detector_->binary_thres = param.as_int(); + } else if (param.get_name() == "light.min_ratio") { + detector_->light_params.min_ratio = param.as_double(); + } else if (param.get_name() == "light.max_ratio") { + detector_->light_params.max_ratio = param.as_double(); + } else if (param.get_name() == "light.max_angle") { + detector_->light_params.max_angle = param.as_double(); + } else if (param.get_name() == "light.use_fit_line") { + detector_->light_params.use_fit_line = param.as_bool(); + } else if (param.get_name() == "light.color_diff_thresh") { + detector_->light_params.color_diff_thresh = param.as_int(); + } else if (param.get_name() == "armor.min_light_ratio") { + detector_->armor_params.min_light_ratio = param.as_double(); + } else if (param.get_name() == "armor.min_small_center_distance") { + detector_->armor_params.min_small_center_distance = param.as_double(); + } else if (param.get_name() == "armor.max_small_center_distance") { + detector_->armor_params.max_small_center_distance = param.as_double(); + } else if (param.get_name() == "armor.min_large_center_distance") { + detector_->armor_params.min_large_center_distance = param.as_double(); + } else if (param.get_name() == "armor.max_large_center_distance") { + detector_->armor_params.max_large_center_distance = param.as_double(); + } else if (param.get_name() == "armor.max_angle") { + detector_->armor_params.max_angle = param.as_double(); } else if (param.get_name() == "debug.enable_terminal_log") { debug_terminal_log_ = param.as_bool(); } else if (param.get_name() == "debug.enable_markers") {