qualia_vision/tracker/
associate_greedy_iou.rs1use crate::preprocess::iou_u16;
8use crate::types::Detection;
9
10use super::MAX_TRACKS;
11
12pub const NO_TRACK: u16 = u16::MAX;
14
15pub fn associate_greedy_iou(
25 detections: &[Detection],
26 det_order: &[u16],
27 n_order: usize,
28 det_eligible: &[bool],
29 track_boxes: &[Detection; MAX_TRACKS],
30 track_class: &[u64; MAX_TRACKS],
31 track_active: &[bool; MAX_TRACKS],
32 track_used: &mut [bool; MAX_TRACKS],
33 class_gated: bool,
34 iou_thresh: f32,
35 det_matched: &mut [bool],
36 det_to_track: &mut [u16],
37) -> usize {
38 let n_order = n_order.min(det_order.len());
39 let mut pairs = 0usize;
40
41 for oi in 0..n_order {
42 let di = det_order[oi] as usize;
43 if di >= detections.len() || di >= det_eligible.len() || di >= det_matched.len() {
44 continue;
45 }
46 if !det_eligible[di] || det_matched[di] {
47 continue;
48 }
49 let det = detections[di];
50 if det.class_hash == 0 && det.score_u16 == 0 {
51 continue;
52 }
53
54 let mut best_ti: Option<usize> = None;
55 let mut best_iou = iou_thresh;
56 for ti in 0..MAX_TRACKS {
57 if !track_active[ti] || track_used[ti] {
58 continue;
59 }
60 if class_gated && track_class[ti] != det.class_hash {
61 continue;
62 }
63 let iou = iou_u16(&det, &track_boxes[ti]);
64 if iou > best_iou {
66 best_iou = iou;
67 best_ti = Some(ti);
68 }
69 }
70
71 if let Some(ti) = best_ti {
72 track_used[ti] = true;
73 det_matched[di] = true;
74 if di < det_to_track.len() {
75 det_to_track[di] = ti as u16;
76 }
77 pairs = pairs.saturating_add(1);
78 }
79 }
80 pairs
81}
82
83#[cfg(test)]
84mod tests {
85 use super::*;
86 use crate::types::Detection;
87
88 fn box_at(class: u64, x0: u16, y0: u16, x1: u16, y1: u16, score: u16) -> Detection {
89 let mut d = Detection::empty();
90 d.class_hash = class;
91 d.score_u16 = score;
92 d.x_min_u16 = x0;
93 d.y_min_u16 = y0;
94 d.x_max_u16 = x1;
95 d.y_max_u16 = y1;
96 d
97 }
98
99 #[test]
100 fn matches_overlapping_same_class() {
101 let dets = [
102 box_at(0xAA, 1000, 1000, 5000, 5000, 50_000),
103 box_at(0xBB, 40_000, 40_000, 50_000, 50_000, 40_000),
104 ];
105 let mut track_boxes = [Detection::empty(); MAX_TRACKS];
106 let mut track_class = [0u64; MAX_TRACKS];
107 let mut track_active = [false; MAX_TRACKS];
108 track_boxes[0] = box_at(0xAA, 1100, 1100, 5100, 5100, 0);
109 track_class[0] = 0xAA;
110 track_active[0] = true;
111 track_boxes[1] = box_at(0xBB, 41_000, 41_000, 51_000, 51_000, 0);
112 track_class[1] = 0xBB;
113 track_active[1] = true;
114
115 let det_order = [0u16, 1];
116 let det_eligible = [true, true];
117 let mut track_used = [false; MAX_TRACKS];
118 let mut det_matched = [false; 2];
119 let mut det_to_track = [NO_TRACK; 2];
120
121 let n = associate_greedy_iou(
122 &dets,
123 &det_order,
124 2,
125 &det_eligible,
126 &track_boxes,
127 &track_class,
128 &track_active,
129 &mut track_used,
130 true,
131 0.3,
132 &mut det_matched,
133 &mut det_to_track,
134 );
135 assert_eq!(n, 2);
136 assert_eq!(det_to_track[0], 0);
137 assert_eq!(det_to_track[1], 1);
138 }
139
140 #[test]
141 fn skips_ineligible_detections() {
142 let dets = [box_at(0xAA, 1000, 1000, 5000, 5000, 50_000)];
143 let mut track_boxes = [Detection::empty(); MAX_TRACKS];
144 let mut track_class = [0u64; MAX_TRACKS];
145 let mut track_active = [false; MAX_TRACKS];
146 track_boxes[0] = box_at(0xAA, 1100, 1100, 5100, 5100, 0);
147 track_class[0] = 0xAA;
148 track_active[0] = true;
149
150 let det_order = [0u16];
151 let det_eligible = [false];
152 let mut track_used = [false; MAX_TRACKS];
153 let mut det_matched = [false; 1];
154 let mut det_to_track = [NO_TRACK; 1];
155
156 let n = associate_greedy_iou(
157 &dets,
158 &det_order,
159 1,
160 &det_eligible,
161 &track_boxes,
162 &track_class,
163 &track_active,
164 &mut track_used,
165 true,
166 0.3,
167 &mut det_matched,
168 &mut det_to_track,
169 );
170 assert_eq!(n, 0);
171 assert_eq!(det_to_track[0], NO_TRACK);
172 }
173}