fix(tracker): reject out-of-gate SP matches

[Jiu-xiao]

Summary\n- reject single-armor SP candidates outside max_match_distance / max_match_yaw_diff\n- keep rejected candidate_debug data for preview/audit without updating tracker state\n\n## Validation\n- Ubuntu24 BSP capturefile runtime reached EOF and SYNCED LATEST_IMU\n- capturefile/hik BSP builds passed with this tracker tar\n- branch CI passed: https://github.com/QDU-Robomaster/ArmorTracker/actions/runs/25174223503\n

Summary by Sourcery

Gate single-armor SP tracker updates on position and yaw error thresholds and only update tracker state when a same-target match passes these limits.

Bug Fixes:

• Reject single-armor SP matches whose position or yaw error exceeds configured match thresholds to avoid incorrect tracker updates.

Enhancements:

• Preserve candidate debug data for rejected single-armor SP matches while skipping state updates.
• Add detailed debug logging for accepted and rejected single-armor SP tracker matches.

[sourcery-ai]

Reviewer's Guide

Implements gating for single-armor SP matches so that candidates exceeding configured distance/yaw error thresholds are rejected without updating tracker state, while preserving candidate_debug info and adding detailed logging for accepted vs rejected matches.

Sequence diagram for ArmorTracker single-armor SP gating decision

sequenceDiagram
    actor Detector
    participant ArmorTracker
    participant EKF
    participant Logger

    Detector->>ArmorTracker: Update(armors_msg, imu_msg, time)
    ArmorTracker->>ArmorTracker: find best_armor and best_match
    ArmorTracker->>ArmorTracker: set candidate_debug.count and items[0]
    ArmorTracker->>ArmorTracker: compute accepted = (best_match.xyz_error < max_match_distance && best_match.angle_error < max_match_yaw_diff)
    ArmorTracker->>ArmorTracker: rt_.info_position_diff = best_match.xyz_error
    ArmorTracker->>ArmorTracker: rt_.info_yaw_diff = best_match.angle_error

    alt match rejected (not accepted)
        ArmorTracker->>Logger: XR_LOG_DEBUG("SP tracker reject match ...")
        ArmorTracker-->>Detector: return without updating rt_ or EKF
    else match accepted
        ArmorTracker->>ArmorTracker: matched = true
        ArmorTracker->>ArmorTracker: candidate_debug.matched = 1
        ArmorTracker->>ArmorTracker: candidate_debug.accepted_mode = 1
        ArmorTracker->>ArmorTracker: candidate_debug.selected_index = 0
        ArmorTracker->>ArmorTracker: candidate_debug.same_face_matched = 1
        ArmorTracker->>ArmorTracker: previous_face = rt_.tracked_face_index
        ArmorTracker->>ArmorTracker: SpUpdate(best_armor, best_match, pair_delta_z_mode)
        ArmorTracker->>ArmorTracker: update rt_.tracked_armor, tracked_id, tracked_armors_num, tracked_face_index, last_yaw
        ArmorTracker->>ArmorTracker: rt_.update_count++
        opt face switched
            ArmorTracker->>ArmorTracker: rt_.switch_count++
        end
        ArmorTracker->>ArmorTracker: SyncGeometryRuntimeFromState()
        ArmorTracker->>EKF: ekf_.ekf.SetState(ekf_.state)
        ArmorTracker->>Logger: XR_LOG_DEBUG("SP tracker update ...")
        ArmorTracker-->>Detector: return with updated tracker state
    end

Loading

Class diagram for updated ArmorTracker SP gating logic

classDiagram
    class ArmorTracker {
      +cfg_ : TrackerConfig
      +ekf_ : EkfWrapper
      +rt_ : RuntimeState
      +Update(armors_msg, imu_msg, time)
      +SpUpdate(best_armor, best_match, pair_delta_z_mode)
      +SpArmorCountFor(best_armor) int
      +SyncGeometryRuntimeFromState()
    }

    class TrackerConfig {
      +match : MatchConfig
    }

    class MatchConfig {
      +max_match_distance : float
      +max_match_yaw_diff : float
    }

    class BestMatch {
      +id : int
      +score : float
      +yaw_error : float
      +pitch_error : float
      +distance_error : float
      +angle_error : float
      +xyz_error : float
      +measured_yaw : float
    }

    class RuntimeState {
      +tracked_armor : Armor
      +tracked_id : int
      +tracked_armors_num : ArmorsNum
      +tracked_face_index : int
      +last_yaw : float
      +info_position_diff : float
      +info_yaw_diff : float
      +update_count : int
      +switch_count : int
    }

    class CandidateDebug {
      +matched : int
      +accepted_mode : int
      +selected_index : int
      +same_face_matched : int
      +count : int
      +items : CandidateDebugItem[]
      +best_same_face_score : float
    }

    class CandidateDebugItem {
      +armor_index : uint8_t
      +id : int
      +pred_yaw : float
      +measured_yaw : float
    }

    class EkfWrapper {
      +state : EkfState
      +ekf : EkfCore
    }

    class EkfCore {
      +SetState(state)
    }

    ArmorTracker --> TrackerConfig : uses
    TrackerConfig --> MatchConfig : has
    ArmorTracker --> RuntimeState : updates
    ArmorTracker --> BestMatch : evaluates
    ArmorTracker --> CandidateDebug : fills
    ArmorTracker --> EkfWrapper : synchronizes
    EkfWrapper --> EkfCore : wraps
    CandidateDebug --> CandidateDebugItem : contains

    class Armor {
      +number : int
      +type : int
    }

    class ArmorsNum {
    }

    RuntimeState --> Armor : tracks
    RuntimeState --> ArmorsNum : tracks

Loading

File-Level Changes

Change	Details	Files
Gate single-armor SP matches using max distance and yaw error thresholds, only updating tracker state when the match is accepted and logging both accepted and rejected cases.	Remove unconditional acceptance of single-armor same-target candidates and the associated candidate_debug flags at block entry Introduce an 'accepted' boolean based on xyz_error < cfg_.match.max_match_distance and angle_error < cfg_.match.max_match_yaw_diff, always updating info_position_diff and info_yaw_diff On rejection, skip SpUpdate and all runtime/EKF state mutations and instead log a detailed XR_LOG_DEBUG message describing the rejected match and gating thresholds On acceptance, set matched/candidate_debug fields, perform SpUpdate, update tracked armor/id/face counts and yaw, increment update/switch counters, sync geometry runtime and EKF state, and log a reduced XR_LOG_DEBUG message for the successful update	ArmorTrackerPipeline.hpp

---

Tips and commands

Interacting with Sourcery

• Trigger a new review: Comment @sourcery-ai review on the pull request.
• Continue discussions: Reply directly to Sourcery's review comments.
• Generate a GitHub issue from a review comment: Ask Sourcery to create an
  issue from a review comment by replying to it. You can also reply to a
  review comment with @sourcery-ai issue to create an issue from it.
• Generate a pull request title: Write @sourcery-ai anywhere in the pull
  request title to generate a title at any time. You can also comment
  @sourcery-ai title on the pull request to (re-)generate the title at any time.
• Generate a pull request summary: Write @sourcery-ai summary anywhere in
  the pull request body to generate a PR summary at any time exactly where you
  want it. You can also comment @sourcery-ai summary on the pull request to
  (re-)generate the summary at any time.
• Generate reviewer's guide: Comment @sourcery-ai guide on the pull
  request to (re-)generate the reviewer's guide at any time.
• Resolve all Sourcery comments: Comment @sourcery-ai resolve on the
  pull request to resolve all Sourcery comments. Useful if you've already
  addressed all the comments and don't want to see them anymore.
• Dismiss all Sourcery reviews: Comment @sourcery-ai dismiss on the pull
  request to dismiss all existing Sourcery reviews. Especially useful if you
  want to start fresh with a new review - don't forget to comment
  @sourcery-ai review to trigger a new review!

Customizing Your Experience

Access your dashboard to:

• Enable or disable review features such as the Sourcery-generated pull request
  summary, the reviewer's guide, and others.
• Change the review language.
• Add, remove or edit custom review instructions.
• Adjust other review settings.

Getting Help

• Contact our support team for questions or feedback.
• Visit our documentation for detailed guides and information.
• Keep in touch with the Sourcery team by following us on X/Twitter, LinkedIn or GitHub.

[sourcery-ai]

Hey - I've left some high level feedback:

• The gating condition uses best_match.angle_error but the log message and config name refer to yaw (and max_match_yaw_diff), so it would be good to double‑check whether angle_error is really the intended quantity here or if yaw_error should be used for consistency.
• The new reject path logs every failed SP match at XR_LOG_DEBUG, which could become quite noisy in dense scenes; consider adding a rate limit, a verbosity flag, or promoting only particularly bad rejections (e.g., beyond some margin) to logs.

Prompt for AI Agents

Please address the comments from this code review:

## Overall Comments
- The gating condition uses `best_match.angle_error` but the log message and config name refer to `yaw` (and `max_match_yaw_diff`), so it would be good to double‑check whether `angle_error` is really the intended quantity here or if `yaw_error` should be used for consistency.
- The new reject path logs every failed SP match at `XR_LOG_DEBUG`, which could become quite noisy in dense scenes; consider adding a rate limit, a verbosity flag, or promoting only particularly bad rejections (e.g., beyond some margin) to logs.

---

Sourcery is free for open source - if you like our reviews please consider sharing them ✨

• X
• Mastodon
• LinkedIn
• Facebook

_{Help me be more useful! Please click 👍 or 👎 on each comment and I'll use the feedback to improve your reviews.}