优化

src/rm_auto_aim/armor_detector/src/armor_detector.cpp

@@ -48,14 +48,18 @@ std::vector<Armor> 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<Light> 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<cv::Vec3b>(y, x);
@@ -150,7 +158,8 @@ std::vector<Light> 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<cv::Vec3b>(point.y, point.x);
           sum_r += pixel[0];
           sum_b += pixel[2];

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<int>(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<rm_interfaces::msg::Armors>("armor_detector/armors",
@@ -400,7 +404,7 @@ std::unique_ptr<Detector> 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<int>(declare_parameter("light.color_diff_thresh", 25))};
 
   Detector::ArmorParams a_params = {
@@ -428,8 +432,8 @@ std::unique_ptr<Detector> ArmorDetectorNode::initDetector() {
   detector->classifier =
     std::make_unique<NumberClassifier>(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<LightCornerCorrector>();
   }

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();

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<cv::Point2f> 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<float>(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<float>(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<cv::Point2f> 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))) {

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<std::pair<double, double>> getTrajectory() const noexcept;
+  const std::vector<Eigen::Vector3d>& 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<double, 3> rpy_;
 
+  // Cached computation results to avoid redundant calculations
+  mutable std::vector<Eigen::Vector3d> cached_armor_positions_;
+  mutable std::vector<std::pair<double, double>> cached_trajectory_;
+  mutable bool trajectory_cache_valid_ = false;
+  mutable double cached_trajectory_pitch_ = 0.;
+
   double prediction_delay_;
   double controller_delay_;
 

src/rm_auto_aim/armor_solver/src/armor_solver.cpp

@@ -66,6 +66,9 @@ Solver::Solver(std::weak_ptr<rclcpp::Node> 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<Eigen::Vector3d> 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<std::pair<double, double>> 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<std::pair<double, double>> 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
   }
 }
 

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<int>(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();