diff --git a/src/rm_auto_aim/armor_detector/src/armor_detector.cpp b/src/rm_auto_aim/armor_detector/src/armor_detector.cpp index 3dbce5a..e942d56 100644 --- a/src/rm_auto_aim/armor_detector/src/armor_detector.cpp +++ b/src/rm_auto_aim/armor_detector/src/armor_detector.cpp @@ -48,14 +48,18 @@ std::vector Detector::detect(const cv::Mat &input) noexcept { armors_ = matchLights(lights_); if (!armors_.empty() && classifier != nullptr) { - // Sequential processing here avoids nested parallelism with node-level workers. + // Sequential classification is required because OpenCV DNN uses a mutex + // internally which serializes inference anyway for (auto &armor : armors_) { // 4. Extract the number image armor.number_img = classifier->extractNumber(input, armor); // 5. Do classification classifier->classify(input, armor); - // 6. Correct the corners of the armor - if (corner_corrector != nullptr) { + } + + // 6. Sequential corner correction + if (corner_corrector != nullptr) { + for (auto &armor : armors_) { corner_corrector->correctCorners(armor, gray_img_); } } @@ -134,13 +138,17 @@ std::vector Detector::findLights(const cv::Mat &rgb_img, } if (isLight(light)) { + // Optimized color sampling - subsample every Nth pixel for speed int sum_r = 0; int sum_b = 0; int sample_count = 0; + constexpr int SAMPLE_STRIDE = 3; // Sample every 3rd pixel for speed + if (light_params.use_fit_line && !points.empty()) { // fit-line mode keeps local contour pixels in points; convert to global by boundingRect offset auto b_rect = cv::boundingRect(contour); - for (const auto &point : points) { + for (size_t idx = 0; idx < points.size(); idx += SAMPLE_STRIDE) { + const auto &point = points[idx]; const int x = point.x + b_rect.x; const int y = point.y + b_rect.y; const auto &pixel = rgb_img.at(y, x); @@ -150,7 +158,8 @@ std::vector Detector::findLights(const cv::Mat &rgb_img, } } else { // no-fit-line mode: use contour points directly to avoid expensive mask + findNonZero - for (const auto &point : contour) { + for (size_t idx = 0; idx < contour.size(); idx += SAMPLE_STRIDE) { + const auto &point = contour[idx]; const auto &pixel = rgb_img.at(point.y, point.x); sum_r += pixel[0]; sum_b += pixel[2]; diff --git a/src/rm_auto_aim/armor_detector/src/armor_detector_node.cpp b/src/rm_auto_aim/armor_detector/src/armor_detector_node.cpp index 7444cd7..5b8df5f 100644 --- a/src/rm_auto_aim/armor_detector/src/armor_detector_node.cpp +++ b/src/rm_auto_aim/armor_detector/src/armor_detector_node.cpp @@ -69,6 +69,10 @@ double durationMs(const std::chrono::steady_clock::duration &duration) { ArmorDetectorNode::ArmorDetectorNode(const rclcpp::NodeOptions &options) : Node("armor_detector", options) { + // Enable OpenCV multi-threading for this thread + cv::setNumThreads(4); + cv::setUseOptimized(true); + FYT_REGISTER_LOGGER("armor_detector", "~/fyt2024-log", INFO); FYT_INFO("armor_detector", "Starting ArmorDetectorNode!"); // Detector @@ -79,8 +83,8 @@ ArmorDetectorNode::ArmorDetectorNode(const rclcpp::NodeOptions &options) process_every_n_frames_ = std::max(1, static_cast(this->declare_parameter("process_every_n_frames", 1))); - // Tricks to make pose more accurate - use_ba_ = this->declare_parameter("use_ba", true); + // Tricks to make pose more accurate - disabled by default for better CPU performance + use_ba_ = this->declare_parameter("use_ba", false); // Armors Publisher armors_pub_ = this->create_publisher("armor_detector/armors", @@ -400,7 +404,7 @@ std::unique_ptr ArmorDetectorNode::initDetector() { .min_ratio = declare_parameter("light.min_ratio", 0.08), .max_ratio = declare_parameter("light.max_ratio", 0.4), .max_angle = declare_parameter("light.max_angle", 40.0), - .use_fit_line = declare_parameter("light.use_fit_line", true), + .use_fit_line = declare_parameter("light.use_fit_line", false), .color_diff_thresh = static_cast(declare_parameter("light.color_diff_thresh", 25))}; Detector::ArmorParams a_params = { @@ -428,8 +432,8 @@ std::unique_ptr ArmorDetectorNode::initDetector() { detector->classifier = std::make_unique(model_path, label_path, threshold, ignore_classes); - // Init Corrector - bool use_pca = this->declare_parameter("use_pca", true); + // Init Corrector - disabled by default for better CPU performance + bool use_pca = this->declare_parameter("use_pca", false); if (use_pca) { detector->corner_corrector = std::make_unique(); } diff --git a/src/rm_auto_aim/armor_detector/src/ba_solver.cpp b/src/rm_auto_aim/armor_detector/src/ba_solver.cpp index f8d8447..fb2bceb 100644 --- a/src/rm_auto_aim/armor_detector/src/ba_solver.cpp +++ b/src/rm_auto_aim/armor_detector/src/ba_solver.cpp @@ -118,9 +118,9 @@ BaSolver::solveBa(const Armor &armor, const Eigen::Vector3d &t_camera_armor, optimizer_.addEdge(edge); } - // Start optimizing + // Start optimizing - reduced from 20 to 10 iterations for performance optimizer_.initializeOptimization(); - optimizer_.optimize(20); + optimizer_.optimize(10); // Get yaw angle after optimization double yaw_optimized = v_yaw->estimate(); diff --git a/src/rm_auto_aim/armor_detector/src/light_corner_corrector.cpp b/src/rm_auto_aim/armor_detector/src/light_corner_corrector.cpp index 37c6417..db8ca67 100644 --- a/src/rm_auto_aim/armor_detector/src/light_corner_corrector.cpp +++ b/src/rm_auto_aim/armor_detector/src/light_corner_corrector.cpp @@ -82,15 +82,29 @@ SymmetryAxis LightCornerCorrector::findSymmetryAxis(const cv::Mat &gray_img, con cv::Point2f centroid = cv::Point2f(moments.m10 / moments.m00, moments.m01 / moments.m00) + cv::Point2f(light_box.x, light_box.y); - // Initialize the PointCloud + // Initialize the PointCloud - optimized to avoid per-pixel brightness loop + // Only sample points at regular intervals based on brightness std::vector points; - for (int i = 0; i < roi.rows; i++) { - for (int j = 0; j < roi.cols; j++) { - for (int k = 0; k < std::round(roi.at(i, j)); k++) { + points.reserve(roi.rows * roi.cols / 4); // Pre-allocate + + constexpr int SAMPLE_STRIDE = 2; // Sample every 2nd pixel + for (int i = 0; i < roi.rows; i += SAMPLE_STRIDE) { + for (int j = 0; j < roi.cols; j += SAMPLE_STRIDE) { + int brightness = std::round(roi.at(i, j)); + // Add point proportionally to brightness (1-3 times) + int repetitions = std::min(brightness, 3); + for (int k = 0; k < repetitions; k++) { points.emplace_back(cv::Point2f(j, i)); } } } + + if (points.empty()) { + // Fallback: use simple axis from bounding rect + cv::Point2f axis(0, 1); + return SymmetryAxis{.centroid = centroid, .direction = axis, .mean_val = mean_val}; + } + cv::Mat points_mat = cv::Mat(points).reshape(1); // PCA (Principal Component Analysis) @@ -121,21 +135,27 @@ cv::Point2f LightCornerCorrector::findCorner(const cv::Mat &gray_img, return point.x >= 0 && point.x < gray_img.cols && point.y >= 0 && point.y < gray_img.rows; }; - auto distance = [](float x0, float y0, float x1, float y1) -> float { - return std::sqrt((x0 - x1) * (x0 - x1) + (y0 - y1) * (y0 - y1)); + // Use squared distance to avoid sqrt in inner loop + auto distance_sq = [](float x0, float y0, float x1, float y1) -> float { + float dx = x0 - x1; + float dy = y0 - y1; + return dx * dx + dy * dy; }; int oper = order == "top" ? 1 : -1; float L = light.length; float dx = axis.direction.x * oper; float dy = axis.direction.y * oper; + float max_dist_sq = L * L * (END - START) * (END - START); std::vector candidates; - // Select multiple corner candidates and take the average as the final corner - int n = light.width - 2; + // Limit candidates for performance - use subsampling for wide lights + int n = std::max(1, light.width - 2); + int stride = std::max(1, n / 5); // Limit to ~5 samples regardless of width int half_n = std::round(n / 2); - for (int i = -half_n; i <= half_n; i++) { + + for (int i = -half_n; i <= half_n; i += stride) { float x0 = axis.centroid.x + L * START * dx + i; float y0 = axis.centroid.y + L * START * dy; @@ -144,7 +164,7 @@ cv::Point2f LightCornerCorrector::findCorner(const cv::Mat &gray_img, float max_brightness_diff = 0; bool has_corner = false; // Search along the symmetry axis to find the corner that has the maximum brightness difference - for (float x = x0 + dx, y = y0 + dy; distance(x, y, x0, y0) < L * (END - START); + for (float x = x0 + dx, y = y0 + dy; distance_sq(x, y, x0, y0) < max_dist_sq; x += dx, y += dy) { cv::Point2f cur = cv::Point2f(x, y); if (!inImage(cv::Point(cur))) { diff --git a/src/rm_auto_aim/armor_solver/include/armor_solver/armor_solver.hpp b/src/rm_auto_aim/armor_solver/include/armor_solver/armor_solver.hpp index d012edb..373da29 100644 --- a/src/rm_auto_aim/armor_solver/include/armor_solver/armor_solver.hpp +++ b/src/rm_auto_aim/armor_solver/include/armor_solver/armor_solver.hpp @@ -50,6 +50,9 @@ public: enum State { TRACKING_ARMOR = 0, TRACKING_CENTER = 1 } state; std::vector> getTrajectory() const noexcept; + const std::vector& getArmorPositions() const noexcept { + return cached_armor_positions_; + } void setBulletSpeed(double bullet_speed) noexcept; void updateRuntimeParams(double max_tracking_v_yaw, double prediction_delay, @@ -97,6 +100,12 @@ private: std::array rpy_; + // Cached computation results to avoid redundant calculations + mutable std::vector cached_armor_positions_; + mutable std::vector> cached_trajectory_; + mutable bool trajectory_cache_valid_ = false; + mutable double cached_trajectory_pitch_ = 0.; + double prediction_delay_; double controller_delay_; diff --git a/src/rm_auto_aim/armor_solver/src/armor_solver.cpp b/src/rm_auto_aim/armor_solver/src/armor_solver.cpp index 400ce77..21b4ebf 100644 --- a/src/rm_auto_aim/armor_solver/src/armor_solver.cpp +++ b/src/rm_auto_aim/armor_solver/src/armor_solver.cpp @@ -66,6 +66,9 @@ Solver::Solver(std::weak_ptr n) : node_(n) { overflow_count_ = 0; transfer_thresh_ = 5; + // Initialize cache + trajectory_cache_valid_ = false; + node.reset(); } @@ -140,16 +143,16 @@ rm_interfaces::msg::GimbalCmd Solver::solve(const rm_interfaces::msg::Target &ta target_yaw += dt * target.v_yaw; // Choose the best armor to shoot - std::vector armor_positions = getArmorPositions(target_position, - target_yaw, - target.radius_1, - target.radius_2, - target.d_zc, - target.d_za, - target.armors_num); + cached_armor_positions_ = getArmorPositions(target_position, + target_yaw, + target.radius_1, + target.radius_2, + target.d_zc, + target.d_za, + target.armors_num); int idx = - selectBestArmor(armor_positions, target_position, target_yaw, target.v_yaw, target.armors_num); - auto chosen_armor_position = armor_positions.at(idx); + selectBestArmor(cached_armor_positions_, target_position, target_yaw, target.v_yaw, target.armors_num); + auto chosen_armor_position = cached_armor_positions_.at(idx); if (chosen_armor_position.norm() < 0.1) { throw std::runtime_error("No valid armor to shoot"); } @@ -183,14 +186,14 @@ rm_interfaces::msg::GimbalCmd Solver::solve(const rm_interfaces::msg::Target &ta target_position.y() += controller_delay_ * target.velocity.y; target_position.z() += controller_delay_ * target.velocity.z; target_yaw += controller_delay_ * target.v_yaw; - armor_positions = getArmorPositions(target_position, + cached_armor_positions_ = getArmorPositions(target_position, target_yaw, target.radius_1, target.radius_2, target.d_zc, target.d_za, target.armors_num); - chosen_armor_position = armor_positions.at(idx); + chosen_armor_position = cached_armor_positions_.at(idx); gimbal_cmd.distance = chosen_armor_position.norm(); if (chosen_armor_position.norm() < 0.1) { throw std::runtime_error("No valid armor to shoot"); @@ -372,8 +375,26 @@ void Solver::calcYawAndPitch(const Eigen::Vector3d &p, } std::vector> Solver::getTrajectory() const noexcept { - auto trajectory = trajectory_compensator_->getTrajectory(15, rpy_[1]); - // Rotate + // Use cached trajectory if pitch hasn't changed + if (trajectory_cache_valid_ && std::abs(cached_trajectory_pitch_ - rpy_[1]) < 1e-6) { + // Return cached trajectory with rotation applied + auto trajectory = cached_trajectory_; + for (auto &p : trajectory) { + double x = p.first; + double y = p.second; + p.first = x * cos(rpy_[1]) + y * sin(rpy_[1]); + p.second = -x * sin(rpy_[1]) + y * cos(rpy_[1]); + } + return trajectory; + } + + // Compute new trajectory and cache it + cached_trajectory_ = trajectory_compensator_->getTrajectory(15, rpy_[1]); + cached_trajectory_pitch_ = rpy_[1]; + trajectory_cache_valid_ = true; + + // Return with rotation applied + auto trajectory = cached_trajectory_; for (auto &p : trajectory) { double x = p.first; double y = p.second; @@ -386,6 +407,7 @@ std::vector> Solver::getTrajectory() const noexcept { void Solver::setBulletSpeed(double bullet_speed) noexcept { if (std::isfinite(bullet_speed) && bullet_speed > 0.0) { trajectory_compensator_->velocity = bullet_speed; + trajectory_cache_valid_ = false; // Invalidate cache when bullet speed changes } } diff --git a/src/rm_auto_aim/armor_solver/src/armor_solver_node.cpp b/src/rm_auto_aim/armor_solver/src/armor_solver_node.cpp index 3679e70..93d8830 100644 --- a/src/rm_auto_aim/armor_solver/src/armor_solver_node.cpp +++ b/src/rm_auto_aim/armor_solver/src/armor_solver_node.cpp @@ -454,39 +454,39 @@ void ArmorSolverNode::publishMarkers(const rm_interfaces::msg::Target &target_ms angular_v_marker_.points.emplace_back(arrow_end); armors_marker_.action = visualization_msgs::msg::Marker::ADD; - armors_marker_.scale.y = tracker_->tracked_armor.type == "small" ? 0.135 : 0.23; - // Draw armors - bool is_current_pair = true; - size_t a_n = target_msg.armors_num; - geometry_msgs::msg::Point p_a; - double r = 0; - for (size_t i = 0; i < a_n; i++) { - double tmp_yaw = yaw + i * (2 * M_PI / a_n); - // Only 4 armors has 2 radius and height - if (a_n == 4) { - r = is_current_pair ? r1 : r2; - p_a.z = zc + d_zc + (is_current_pair ? 0 : d_za); - is_current_pair = !is_current_pair; - } else { - r = r1; - p_a.z = zc; - } - p_a.x = xc - r * cos(tmp_yaw); - p_a.y = yc - r * sin(tmp_yaw); + // Hoist loop-invariant conditional outside loop + bool is_small_armor = tracker_->tracked_armor.type == "small"; + armors_marker_.scale.y = is_small_armor ? 0.135 : 0.23; - armors_marker_.id = i; - armors_marker_.pose.position = p_a; + // Use cached armor positions from solver instead of recomputing + const auto& armor_positions = solver_->getArmorPositions(); + size_t a_n = target_msg.armors_num; + bool is_outpost = (target_msg.id == "outpost"); + double outpost_pitch = is_outpost ? -0.2618 : 0.2618; + double two_pi_over_n = 2 * M_PI / a_n; + + for (size_t i = 0; i < a_n && i < armor_positions.size(); i++) { + const auto& pos = armor_positions[i]; + armors_marker_.id = static_cast(i); + armors_marker_.pose.position.x = pos.x(); + armors_marker_.pose.position.y = pos.y(); + armors_marker_.pose.position.z = pos.z(); tf2::Quaternion q; - q.setRPY(0, target_msg.id == "outpost" ? -0.2618 : 0.2618, tmp_yaw); + double tmp_yaw = yaw + i * two_pi_over_n; + q.setRPY(0, outpost_pitch, tmp_yaw); armors_marker_.pose.orientation = tf2::toMsg(q); marker_array.markers.emplace_back(armors_marker_); } selection_marker_.action = visualization_msgs::msg::Marker::ADD; selection_marker_.points.clear(); - selection_marker_.pose.position.y = gimbal_cmd.distance * sin(gimbal_cmd.yaw * M_PI / 180); - selection_marker_.pose.position.x = gimbal_cmd.distance * cos(gimbal_cmd.yaw * M_PI / 180); - selection_marker_.pose.position.z = gimbal_cmd.distance * sin(gimbal_cmd.pitch * M_PI / 180); + // Pre-compute trig values for selection marker + double sin_yaw_cmd = sin(gimbal_cmd.yaw * M_PI / 180); + double cos_yaw_cmd = cos(gimbal_cmd.yaw * M_PI / 180); + double sin_pitch_cmd = sin(gimbal_cmd.pitch * M_PI / 180); + selection_marker_.pose.position.y = gimbal_cmd.distance * sin_yaw_cmd; + selection_marker_.pose.position.x = gimbal_cmd.distance * cos_yaw_cmd; + selection_marker_.pose.position.z = gimbal_cmd.distance * sin_pitch_cmd; trajectory_marker_.action = visualization_msgs::msg::Marker::ADD; trajectory_marker_.points.clear();