Skip to main content

qualia_vision/tracker/
mod.rs

1//! Phase 5 / V5 — bounded multi-object tracker (ByteTrack-class association).
2//!
3//! Pure Rust, no training, no Python. Fixed slot table — no heap on the hot path.
4//!
5//! # Association (ByteTrack-style two-stage IoU)
6//! 1. **High-score** detections associate first (`iou_thresh`).
7//! 2. **Low-score** detections recover remaining unmatched tracks (`low_iou_thresh`);
8//!    they never spawn new tracks.
9//! 3. Unmatched high-score detections birth **Tentative** tracks.
10//! 4. After `min_hits` successful associations a track becomes **Confirmed**.
11//! 5. Unmatched tracks age; Tentative dies faster; Confirmed expires after `max_miss`.
12//!
13//! Overflow: when all slots full, new detections keep `track_id = 0` and set
14//! `FLAG_TRACK_OVERFLOW` (deterministic; no eviction of live tracks).
15
16mod associate_greedy_iou;
17mod score_tier_of;
18
19pub use associate_greedy_iou::{associate_greedy_iou, NO_TRACK};
20pub use score_tier_of::{score_tier_of, ScoreTier};
21
22use crate::types::Detection;
23
24use associate_greedy_iou::associate_greedy_iou as assoc_iou;
25use score_tier_of::score_tier_of as tier_of;
26
27/// Maximum simultaneous tracks (caller-visible bound).
28pub const MAX_TRACKS: usize = 32;
29
30/// Max detections considered per frame (fixed association scratch).
31pub const MAX_ASSOC_DETS: usize = 256;
32
33/// Detection flag: track table full; this box was not assigned a track id.
34pub const FLAG_TRACK_OVERFLOW: u32 = 1 << 8;
35
36/// Default high-score floor (~0.5 in score_u16 units).
37pub const DEFAULT_HIGH_SCORE_U16: u16 = 32_768;
38/// Default low-score floor (~0.1) — band for second-stage recovery only.
39pub const DEFAULT_LOW_SCORE_U16: u16 = 6_554;
40/// Default hits required before Tentative → Confirmed.
41pub const DEFAULT_MIN_HITS: u32 = 2;
42
43/// Lifecycle of a resident track slot.
44#[repr(u8)]
45#[derive(Debug, Clone, Copy, PartialEq, Eq)]
46pub enum TrackState {
47    /// Newborn; may be dropped on first miss before confirmation.
48    Tentative = 0,
49    /// Survived `min_hits` associations; ages with `max_miss`.
50    Confirmed = 1,
51}
52
53#[derive(Debug, Clone, Copy)]
54struct TrackSlot {
55    active: bool,
56    state: TrackState,
57    id: u32,
58    class_hash: u64,
59    x_min_u16: u16,
60    y_min_u16: u16,
61    x_max_u16: u16,
62    y_max_u16: u16,
63    last_frame: u32,
64    /// Frames since last match (0 = matched this update).
65    miss_count: u32,
66    /// Successful associations including birth frame.
67    hit_count: u32,
68}
69
70impl TrackSlot {
71    const EMPTY: Self = Self {
72        active: false,
73        state: TrackState::Tentative,
74        id: 0,
75        class_hash: 0,
76        x_min_u16: 0,
77        y_min_u16: 0,
78        x_max_u16: 0,
79        y_max_u16: 0,
80        last_frame: 0,
81        miss_count: 0,
82        hit_count: 0,
83    };
84
85    fn as_box(&self) -> Detection {
86        let mut d = Detection::empty();
87        d.class_hash = self.class_hash;
88        d.x_min_u16 = self.x_min_u16;
89        d.y_min_u16 = self.y_min_u16;
90        d.x_max_u16 = self.x_max_u16;
91        d.y_max_u16 = self.y_max_u16;
92        d.track_id = self.id;
93        d
94    }
95}
96
97/// Bounded ByteTrack-class IoU tracker over a frame sequence.
98#[derive(Debug, Clone)]
99pub struct BoundedTracker {
100    slots: [TrackSlot; MAX_TRACKS],
101    next_id: u32,
102    /// Minimum IoU for high-score (first) association.
103    pub iou_thresh: f32,
104    /// Minimum IoU for low-score (second / recovery) association.
105    pub low_iou_thresh: f32,
106    /// Drop a **Confirmed** track after this many consecutive unmatched frames.
107    pub max_miss: u32,
108    /// Hits needed to promote Tentative → Confirmed.
109    pub min_hits: u32,
110    /// Score ≥ this → High tier (first match + birth).
111    pub high_score_u16: u16,
112    /// Low ≤ score < high → Low tier (recovery only).
113    pub low_score_u16: u16,
114    /// If true, only match same class_hash.
115    pub class_gated: bool,
116}
117
118impl Default for BoundedTracker {
119    fn default() -> Self {
120        Self::new()
121    }
122}
123
124impl BoundedTracker {
125    /// Creates a new instance.
126    pub fn new() -> Self {
127        Self {
128            slots: [TrackSlot::EMPTY; MAX_TRACKS],
129            next_id: 1,
130            iou_thresh: 0.3,
131            low_iou_thresh: 0.25,
132            max_miss: 5,
133            min_hits: DEFAULT_MIN_HITS,
134            high_score_u16: DEFAULT_HIGH_SCORE_U16,
135            low_score_u16: DEFAULT_LOW_SCORE_U16,
136            class_gated: true,
137        }
138    }
139
140    /// `active-track` count.
141    pub fn active_track_count(&self) -> usize {
142        self.slots.iter().filter(|s| s.active).count()
143    }
144
145    /// `confirmed-track` count.
146    pub fn confirmed_track_count(&self) -> usize {
147        self.slots
148            .iter()
149            .filter(|s| s.active && s.state == TrackState::Confirmed)
150            .count()
151    }
152
153    /// Assign `track_id` on each of the first `n` detections in place.
154    ///
155    /// Unmatched / overflow / reject-tier detections get `track_id = 0`
156    /// (and `FLAG_TRACK_OVERFLOW` when the table is full for a high-score birth).
157    pub fn update(&mut self, frame_index: u32, detections: &mut [Detection], n: usize) {
158        let n = n.min(detections.len()).min(MAX_ASSOC_DETS);
159
160        // Score-descending order (selection sort — fixed buffer, deterministic).
161        let mut order = [0u16; MAX_ASSOC_DETS];
162        for i in 0..n {
163            order[i] = i as u16;
164        }
165        for i in 0..n {
166            let mut best = i;
167            for j in (i + 1)..n {
168                if detections[order[j] as usize].score_u16
169                    > detections[order[best] as usize].score_u16
170                {
171                    best = j;
172                }
173            }
174            order.swap(i, best);
175        }
176
177        // Snapshot track geometry for pure association helpers.
178        let mut track_boxes = [Detection::empty(); MAX_TRACKS];
179        let mut track_class = [0u64; MAX_TRACKS];
180        let mut track_active = [false; MAX_TRACKS];
181        for ti in 0..MAX_TRACKS {
182            let slot = &self.slots[ti];
183            if slot.active {
184                track_active[ti] = true;
185                track_class[ti] = slot.class_hash;
186                track_boxes[ti] = slot.as_box();
187            }
188        }
189
190        let mut track_used = [false; MAX_TRACKS];
191        let mut det_matched = [false; MAX_ASSOC_DETS];
192        let mut det_to_track = [NO_TRACK; MAX_ASSOC_DETS];
193        let mut high_eligible = [false; MAX_ASSOC_DETS];
194        let mut low_eligible = [false; MAX_ASSOC_DETS];
195
196        for di in 0..n {
197            let det = detections[di];
198            if det.class_hash == 0 && det.score_u16 == 0 {
199                continue;
200            }
201            match tier_of(det.score_u16, self.high_score_u16, self.low_score_u16) {
202                ScoreTier::High => high_eligible[di] = true,
203                ScoreTier::Low => low_eligible[di] = true,
204                ScoreTier::Reject => {}
205            }
206        }
207
208        // Stage 1 — high-score IoU match.
209        let _ = assoc_iou(
210            detections,
211            &order,
212            n,
213            &high_eligible,
214            &track_boxes,
215            &track_class,
216            &track_active,
217            &mut track_used,
218            self.class_gated,
219            self.iou_thresh,
220            &mut det_matched,
221            &mut det_to_track,
222        );
223
224        // Stage 2 — low-score recovery of remaining tracks (no birth later).
225        let _ = assoc_iou(
226            detections,
227            &order,
228            n,
229            &low_eligible,
230            &track_boxes,
231            &track_class,
232            &track_active,
233            &mut track_used,
234            self.class_gated,
235            self.low_iou_thresh,
236            &mut det_matched,
237            &mut det_to_track,
238        );
239
240        // Apply matches: update geometry, hits, lifecycle.
241        for di in 0..n {
242            if !det_matched[di] {
243                continue;
244            }
245            let ti = det_to_track[di] as usize;
246            if ti >= MAX_TRACKS || !self.slots[ti].active {
247                continue;
248            }
249            let det = detections[di];
250            let slot = &mut self.slots[ti];
251            slot.x_min_u16 = det.x_min_u16;
252            slot.y_min_u16 = det.y_min_u16;
253            slot.x_max_u16 = det.x_max_u16;
254            slot.y_max_u16 = det.y_max_u16;
255            slot.last_frame = frame_index;
256            slot.miss_count = 0;
257            slot.hit_count = slot.hit_count.saturating_add(1);
258            if slot.state == TrackState::Tentative && slot.hit_count >= self.min_hits {
259                slot.state = TrackState::Confirmed;
260            }
261            detections[di].track_id = slot.id;
262            detections[di].frame_index = frame_index;
263        }
264
265        // Birth: unmatched high-score detections only (score order).
266        for oi in 0..n {
267            let di = order[oi] as usize;
268            if det_matched[di] {
269                continue;
270            }
271            let det = detections[di];
272            if det.class_hash == 0 && det.score_u16 == 0 {
273                detections[di].track_id = 0;
274                detections[di].frame_index = frame_index;
275                continue;
276            }
277            // Low / Reject: no new track.
278            if !high_eligible[di] {
279                detections[di].track_id = 0;
280                detections[di].frame_index = frame_index;
281                continue;
282            }
283            if let Some(ti) = self.slots.iter().position(|s| !s.active) {
284                let id = self.next_id;
285                self.next_id = self.next_id.wrapping_add(1).max(1);
286                self.slots[ti] = TrackSlot {
287                    active: true,
288                    state: if self.min_hits <= 1 {
289                        TrackState::Confirmed
290                    } else {
291                        TrackState::Tentative
292                    },
293                    id,
294                    class_hash: det.class_hash,
295                    x_min_u16: det.x_min_u16,
296                    y_min_u16: det.y_min_u16,
297                    x_max_u16: det.x_max_u16,
298                    y_max_u16: det.y_max_u16,
299                    last_frame: frame_index,
300                    miss_count: 0,
301                    hit_count: 1,
302                };
303                detections[di].track_id = id;
304                detections[di].frame_index = frame_index;
305            } else {
306                // Overflow: leave track_id 0, flag, do not evict.
307                detections[di].track_id = 0;
308                detections[di].frame_index = frame_index;
309                detections[di].flags |= FLAG_TRACK_OVERFLOW;
310            }
311        }
312
313        // Age unmatched tracks (skip matched and same-frame births).
314        for (ti, slot) in self.slots.iter_mut().enumerate() {
315            if !slot.active || track_used[ti] || slot.last_frame == frame_index {
316                continue;
317            }
318            slot.miss_count = slot.miss_count.saturating_add(1);
319            let expire = match slot.state {
320                // Tentative dies on first miss (strict newborn gate).
321                TrackState::Tentative => true,
322                TrackState::Confirmed => slot.miss_count > self.max_miss,
323            };
324            if expire {
325                *slot = TrackSlot::EMPTY;
326            }
327        }
328    }
329
330    /// Run tracker over a sequence of per-frame detection slices.
331    /// `frames[f]` is (detections buffer, count). Mutates each buffer's track_id.
332    pub fn track_sequence(&mut self, frames: &mut [(&mut [Detection], usize)]) {
333        for (fi, (dets, n)) in frames.iter_mut().enumerate() {
334            self.update(fi as u32, dets, *n);
335        }
336    }
337}
338
339#[cfg(test)]
340mod tests {
341    use super::*;
342
343    fn box_at(class: u64, x0: u16, y0: u16, x1: u16, y1: u16, score: u16) -> Detection {
344        let mut d = Detection::empty();
345        d.class_hash = class;
346        d.instance_hash = class ^ (x0 as u64);
347        d.score_u16 = score;
348        d.x_min_u16 = x0;
349        d.y_min_u16 = y0;
350        d.x_max_u16 = x1;
351        d.y_max_u16 = y1;
352        d
353    }
354
355    #[test]
356    fn same_object_keeps_track_id() {
357        let mut t = BoundedTracker::new();
358        let mut f0 = [box_at(0xAA, 1000, 1000, 5000, 5000, 50_000)];
359        t.update(0, &mut f0, 1);
360        let id0 = f0[0].track_id;
361        assert_ne!(id0, 0);
362
363        // Slightly moved same class — should match.
364        let mut f1 = [box_at(0xAA, 1200, 1100, 5200, 5100, 48_000)];
365        t.update(1, &mut f1, 1);
366        assert_eq!(f1[0].track_id, id0);
367        assert_eq!(t.active_track_count(), 1);
368        // Second hit promotes Tentative → Confirmed when min_hits=2.
369        assert_eq!(t.confirmed_track_count(), 1);
370    }
371
372    #[test]
373    fn two_objects_two_tracks() {
374        let mut t = BoundedTracker::new();
375        let mut f0 = [
376            box_at(0x11, 0, 0, 10_000, 10_000, 50_000),
377            box_at(0x22, 40_000, 40_000, 55_000, 55_000, 49_000),
378        ];
379        t.update(0, &mut f0, 2);
380        assert_ne!(f0[0].track_id, 0);
381        assert_ne!(f0[1].track_id, 0);
382        assert_ne!(f0[0].track_id, f0[1].track_id);
383        assert_eq!(t.active_track_count(), 2);
384    }
385
386    #[test]
387    fn two_objects_keep_ids_across_frames() {
388        let mut t = BoundedTracker::new();
389        let mut f0 = [
390            box_at(0x11, 0, 0, 10_000, 10_000, 50_000),
391            box_at(0x22, 40_000, 40_000, 55_000, 55_000, 49_000),
392        ];
393        t.update(0, &mut f0, 2);
394        let id_a = f0[0].track_id;
395        let id_b = f0[1].track_id;
396        assert_ne!(id_a, 0);
397        assert_ne!(id_b, 0);
398        assert_ne!(id_a, id_b);
399
400        // Move each slightly; identities must stick.
401        let mut f1 = [
402            box_at(0x11, 200, 100, 10_200, 10_100, 48_000),
403            box_at(0x22, 40_200, 40_100, 55_200, 55_100, 47_000),
404        ];
405        t.update(1, &mut f1, 2);
406        assert_eq!(f1[0].track_id, id_a);
407        assert_eq!(f1[1].track_id, id_b);
408        assert_eq!(t.active_track_count(), 2);
409        assert_eq!(t.confirmed_track_count(), 2);
410    }
411
412    #[test]
413    fn overflow_is_deterministic() {
414        let mut t = BoundedTracker::new();
415        t.max_miss = 100;
416        t.min_hits = 1; // keep all slots Confirmed so empty frames do not purge tentative
417                        // Fill all tracks with non-overlapping boxes (distinct classes, high score).
418        for i in 0..MAX_TRACKS {
419            let x = (i as u16).wrapping_mul(2000);
420            let mut f = [box_at(
421                0x100 + i as u64,
422                x,
423                0,
424                x.saturating_add(500),
425                500,
426                40_000,
427            )];
428            t.update(i as u32, &mut f, 1);
429            assert_ne!(f[0].track_id, 0, "slot {i}");
430        }
431        assert_eq!(t.active_track_count(), MAX_TRACKS);
432        // One more high-score det must attempt birth → overflow (low-score never births).
433        let mut extra = [box_at(0xDEAD, 60_000, 60_000, 65_000, 65_000, 45_000)];
434        t.update(MAX_TRACKS as u32, &mut extra, 1);
435        assert_eq!(extra[0].track_id, 0);
436        assert_ne!(extra[0].flags & FLAG_TRACK_OVERFLOW, 0);
437        // Second overflow still deterministic.
438        let mut extra2 = [box_at(0xBEEF, 58_000, 58_000, 63_000, 63_000, 46_000)];
439        t.update(MAX_TRACKS as u32 + 1, &mut extra2, 1);
440        assert_eq!(extra2[0].track_id, 0);
441        assert_ne!(extra2[0].flags & FLAG_TRACK_OVERFLOW, 0);
442        assert_eq!(t.active_track_count(), MAX_TRACKS);
443    }
444
445    #[test]
446    fn miss_expires_track() {
447        let mut t = BoundedTracker::new();
448        t.max_miss = 2;
449        t.min_hits = 1; // confirmed immediately so we test confirmed aging
450        let mut f0 = [box_at(0xBB, 1000, 1000, 4000, 4000, 50_000)];
451        t.update(0, &mut f0, 1);
452        assert_eq!(t.active_track_count(), 1);
453        assert_eq!(t.confirmed_track_count(), 1);
454        // Empty frames.
455        let mut empty = [];
456        t.update(1, &mut empty, 0);
457        t.update(2, &mut empty, 0);
458        t.update(3, &mut empty, 0);
459        assert_eq!(t.active_track_count(), 0);
460    }
461
462    #[test]
463    fn tentative_expires_on_first_miss() {
464        let mut t = BoundedTracker::new();
465        t.min_hits = 3;
466        t.max_miss = 10;
467        let mut f0 = [box_at(0xCC, 1000, 1000, 4000, 4000, 50_000)];
468        t.update(0, &mut f0, 1);
469        assert_eq!(t.active_track_count(), 1);
470        assert_eq!(t.confirmed_track_count(), 0);
471        let mut empty = [];
472        t.update(1, &mut empty, 0);
473        assert_eq!(
474            t.active_track_count(),
475            0,
476            "tentative must die on first miss"
477        );
478    }
479
480    #[test]
481    fn low_score_recovers_unmatched_track() {
482        let mut t = BoundedTracker::new();
483        t.min_hits = 1;
484        t.high_score_u16 = 40_000;
485        t.low_score_u16 = 5_000;
486        t.low_iou_thresh = 0.2;
487
488        let mut f0 = [box_at(0xDD, 1000, 1000, 5000, 5000, 50_000)];
489        t.update(0, &mut f0, 1);
490        let id = f0[0].track_id;
491        assert_ne!(id, 0);
492
493        // High-score miss; low-score overlapping det should recover the track.
494        let mut f1 = [box_at(0xDD, 1200, 1100, 5200, 5100, 10_000)];
495        t.update(1, &mut f1, 1);
496        assert_eq!(f1[0].track_id, id);
497        assert_eq!(t.active_track_count(), 1);
498    }
499
500    #[test]
501    fn low_score_does_not_spawn() {
502        let mut t = BoundedTracker::new();
503        t.high_score_u16 = 40_000;
504        t.low_score_u16 = 5_000;
505        let mut f0 = [box_at(0xEE, 1000, 1000, 5000, 5000, 10_000)];
506        t.update(0, &mut f0, 1);
507        assert_eq!(f0[0].track_id, 0);
508        assert_eq!(t.active_track_count(), 0);
509    }
510}