1mod 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
27pub const MAX_TRACKS: usize = 32;
29
30pub const MAX_ASSOC_DETS: usize = 256;
32
33pub const FLAG_TRACK_OVERFLOW: u32 = 1 << 8;
35
36pub const DEFAULT_HIGH_SCORE_U16: u16 = 32_768;
38pub const DEFAULT_LOW_SCORE_U16: u16 = 6_554;
40pub const DEFAULT_MIN_HITS: u32 = 2;
42
43#[repr(u8)]
45#[derive(Debug, Clone, Copy, PartialEq, Eq)]
46pub enum TrackState {
47 Tentative = 0,
49 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 miss_count: u32,
66 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#[derive(Debug, Clone)]
99pub struct BoundedTracker {
100 slots: [TrackSlot; MAX_TRACKS],
101 next_id: u32,
102 pub iou_thresh: f32,
104 pub low_iou_thresh: f32,
106 pub max_miss: u32,
108 pub min_hits: u32,
110 pub high_score_u16: u16,
112 pub low_score_u16: u16,
114 pub class_gated: bool,
116}
117
118impl Default for BoundedTracker {
119 fn default() -> Self {
120 Self::new()
121 }
122}
123
124impl BoundedTracker {
125 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 pub fn active_track_count(&self) -> usize {
142 self.slots.iter().filter(|s| s.active).count()
143 }
144
145 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 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 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 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 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 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 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 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 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 detections[di].track_id = 0;
308 detections[di].frame_index = frame_index;
309 detections[di].flags |= FLAG_TRACK_OVERFLOW;
310 }
311 }
312
313 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 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 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 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 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 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; 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 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 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; 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 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 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}