1use crate::kernels::geometry::js_hypot;
10use crate::kernels::pool::SyncPtr;
11
12pub struct FieldMotion {
13 pub x: f64,
14 pub y: f64,
15 pub support: u32,
16 pub confidence: f64,
17}
18
19pub struct Field<'a> {
21 pub motions: &'a [FieldMotion],
22 pub labels: &'a [u8],
23 pub confidence: &'a [u8],
24 pub dynamic: &'a [u8],
25 pub cols: usize,
26 pub rows: usize,
27 pub difference: f64,
28 pub unknown: bool,
29}
30
31#[inline]
32fn norm(m: &FieldMotion) -> f64 {
33 js_hypot(m.x, m.y)
34}
35
36pub const MOVING_COLUMN: f64 = 1.0;
41
42pub fn informative(field: &Field<'_>) -> bool {
43 !field.unknown
44 && field.difference >= 0.2
45 && field
46 .motions
47 .iter()
48 .any(|m| norm(m) > 1.0 && m.confidence > 0.3)
49}
50
51pub struct Learner {
52 pub width: usize,
53 pub height: usize,
54 pub cell: usize,
55 pub cols: usize,
56 pub rows: usize,
57 pub split: Vec<f64>,
58 pub evidence: Vec<f64>,
59 pub activity: Vec<f64>,
60 pub observations: Vec<f64>,
61 pub row_fixed: Vec<f64>,
62 pub row_moving: Vec<f64>,
63 pub informative_frames: u32,
64 pub row_change: Vec<f64>,
65 pub col_change: Vec<f64>,
66 pub col_moving: Vec<f64>,
70 pub col_mean: Vec<f64>,
71 pub col_gain: Vec<f64>,
72 pub horizontal_gain: Vec<f64>,
73 pub native_row_change: Vec<f64>,
75 pub native_col_change: Vec<f64>,
76 pub native_col_mean: Vec<f64>,
77 pub native_frames: u32,
78 pub native_width: usize,
79 pub native_height: usize,
80}
81
82impl Learner {
83 pub fn new(width: usize, height: usize) -> Learner {
84 let cell = 24;
85 let cols = width.div_ceil(cell);
86 let rows = height.div_ceil(cell);
87 let n = cols * rows;
88 Learner {
89 width,
90 height,
91 cell,
92 cols,
93 rows,
94 split: vec![0.0; n * 2],
95 evidence: vec![0.0; n * 2],
96 activity: vec![0.0; n],
97 observations: vec![0.0; n],
98 row_fixed: vec![0.0; height],
99 row_moving: vec![0.0; height],
100 informative_frames: 0,
101 row_change: vec![0.0; height],
102 col_change: vec![0.0; width],
103 col_moving: vec![0.0; width],
104 col_mean: vec![0.0; width],
105 col_gain: vec![0.0; cols],
106 horizontal_gain: vec![0.0; rows],
107 native_row_change: Vec::new(),
108 native_col_change: Vec::new(),
109 native_col_mean: Vec::new(),
110 native_frames: 0,
111 native_width: 0,
112 native_height: 0,
113 }
114 }
115
116 pub fn add(
120 &mut self,
121 field: &Field<'_>,
122 prev: &[u8],
123 current: &[u8],
124 native: Option<(&[u8], &[u8], usize, usize)>,
125 ) -> bool {
126 if !informative(field) {
127 return false;
128 }
129 self.informative_frames += 1;
130 if let Some((a, b, w, h)) = native {
131 self.add_native(a, b, w, h);
132 }
133 let (width, height, cols, rows) = (self.width, self.height, self.cols, self.rows);
134 for y in 0..height {
135 let (mut sum, mut n) = (0.0f64, 0u32);
136 let mut x = 0;
137 while x < width {
138 let i = y * width + x;
139 sum += (prev[i] as i32 - current[i] as i32).abs() as f64;
140 n += 1;
141 x += 3;
142 }
143 self.row_change[y] += sum / (n.max(1) as f64);
144 }
145 let (mut change, mut mean) = (vec![0.0f64; width], vec![0.0f64; width]);
148 let mut n = 0u32;
149 let mut y = 0;
150 while y < height {
151 let (p, c) = (
152 &prev[y * width..(y + 1) * width],
153 ¤t[y * width..(y + 1) * width],
154 );
155 for x in 0..width {
156 change[x] += (p[x] as i32 - c[x] as i32).abs() as f64;
157 mean[x] += c[x] as f64;
158 }
159 n += 1;
160 y += 3;
161 }
162 for x in 0..width {
163 self.col_change[x] += change[x] / n as f64;
164 if change[x] / n as f64 > MOVING_COLUMN {
165 self.col_moving[x] += 1.0;
166 }
167 self.col_mean[x] += mean[x] / n as f64;
168 }
169 let models = field.motions.len();
170 let mut columns = vec![0.0f64; cols * models];
171 let mut lines = vec![0.0f64; rows * models];
172 let informative_frames = self.informative_frames as f64;
173 for y in 0..rows {
174 for x in 0..cols {
175 let i = y * cols + x;
176 let py = (height - 1).min(((y as f64 + 0.5) * self.cell as f64).floor() as usize);
177 let label = field.labels[i] as usize;
180 if field.confidence[i] <= 110
181 || field.dynamic[i] != 0
182 || norm(&field.motions[label]) <= 1.0
183 || self.row_change[py] / informative_frames < 0.9
184 {
185 continue;
186 }
187 let w = field.confidence[i] as f64 / 255.0;
188 columns[x * models + label] += w;
189 lines[y * models + label] += w;
190 }
191 }
192 gain(&columns, models, &mut self.col_gain);
193 gain(&lines, models, &mut self.horizontal_gain);
194 for y in 0..rows {
195 for x in 0..cols {
196 let i = y * cols + x;
197 let m = &field.motions[field.labels[i] as usize];
198 let c = field.confidence[i] as f64 / 255.0;
199 if c > 0.25 && field.dynamic[i] == 0 {
200 self.activity[i] += norm(m) * c;
201 self.observations[i] += c;
202 }
203 let neighbours = [
204 if x + 1 < cols { Some(i + 1) } else { None },
205 if y + 1 < rows { Some(i + cols) } else { None },
206 ];
207 for (k, j) in neighbours.iter().enumerate() {
208 let Some(j) = *j else {
209 continue;
210 };
211 if field.dynamic[i] != 0 || field.dynamic[j] != 0 {
212 continue;
213 }
214 let weight = c.min(field.confidence[j] as f64 / 255.0);
215 if weight < 0.25 {
216 continue;
217 }
218 let other = &field.motions[field.labels[j] as usize];
219 let disagreement = js_hypot(m.x - other.x, m.y - other.y);
220 self.evidence[i * 2 + k] += weight;
221 if disagreement > 1.8 {
222 self.split[i * 2 + k] += weight;
223 }
224 }
225 }
226 }
227 let mut moving: Option<&FieldMotion> = None;
230 for m in field.motions {
231 if norm(m) > 1.0 && m.support >= 4 && moving.is_none_or(|best| m.support > best.support)
232 {
233 moving = Some(m);
234 }
235 }
236 if let Some(moving) = moving {
237 let dx = crate::kernels::geometry::js_round(moving.x) as i64;
238 let dy = crate::kernels::geometry::js_round(moving.y) as i64;
239 let (w, h) = (width as i64, height as i64);
240 for y in 2..(h - 2).max(2) {
241 let mut x = 2i64;
242 while x < w - 2 {
243 if x + dx < 1 || x + dx >= w - 1 || y + dy < 1 || y + dy >= h - 1 {
244 x += 4;
245 continue;
246 }
247 let i = (y * w + x) as usize;
248 let stationary = (prev[i] as i32 - current[i] as i32).abs();
249 let motion =
250 (prev[((y + dy) * w + x + dx) as usize] as i32 - current[i] as i32).abs();
251 if stationary + 6 < motion {
252 self.row_fixed[y as usize] += 1.0;
253 }
254 if motion + 6 < stationary {
255 self.row_moving[y as usize] += 1.0;
256 }
257 x += 4;
258 }
259 }
260 }
261 true
262 }
263
264 fn add_native(&mut self, a: &[u8], b: &[u8], w: usize, h: usize) {
265 if self.native_row_change.is_empty() {
266 self.native_row_change = vec![0.0; h];
267 self.native_col_change = vec![0.0; w];
268 self.native_col_mean = vec![0.0; w];
269 self.native_width = w;
270 self.native_height = h;
271 }
272 if w != self.native_width || h != self.native_height {
273 return;
275 }
276 self.native_frames += 1;
277 let step = (w / 480).max(1);
278 let vstep = (h / 300).max(1);
279 let sample = |i: usize| {
282 ((a[i] as i32 - b[i] as i32).abs()
283 + (a[i + 1] as i32 - b[i + 1] as i32).abs()
284 + (a[i + 2] as i32 - b[i + 2] as i32).abs()) as f64
285 / 3.0
286 };
287 let rows_out = SyncPtr(self.native_row_change.as_mut_ptr());
288 let chunks = crate::kernels::pool::chunks_for(h * w.div_ceil(step), 32 * 1024);
289 crate::kernels::pool::par_for(chunks, |c| {
290 for y in crate::kernels::pool::split(h, chunks, c)
291 ..crate::kernels::pool::split(h, chunks, c + 1)
292 {
293 let (mut sum, mut n) = (0.0f64, 0u32);
294 let mut x = 0;
295 while x < w {
296 sum += sample((y * w + x) * 4);
297 n += 1;
298 x += step;
299 }
300 unsafe { *rows_out.get().add(y) += sum / (n.max(1) as f64) };
302 }
303 });
304 let samples = h.div_ceil(vstep);
305 let (mut change, mut mean) = (vec![0.0f64; w], vec![0.0f64; w]);
306 let (change_out, mean_out) = (SyncPtr(change.as_mut_ptr()), SyncPtr(mean.as_mut_ptr()));
307 let chunks = crate::kernels::pool::chunks_for(w * samples, 32 * 1024);
308 crate::kernels::pool::par_for(chunks, |c| {
309 let (x0, x1) = (
310 crate::kernels::pool::split(w, chunks, c),
311 crate::kernels::pool::split(w, chunks, c + 1),
312 );
313 let (change, mean) = unsafe {
315 (
316 std::slice::from_raw_parts_mut(change_out.get().add(x0), x1 - x0),
317 std::slice::from_raw_parts_mut(mean_out.get().add(x0), x1 - x0),
318 )
319 };
320 let mut y = 0;
321 while y < h {
322 for x in x0..x1 {
323 let i = (y * w + x) * 4;
324 change[x - x0] += sample(i);
325 mean[x - x0] += (b[i] as f64 + b[i + 1] as f64 + b[i + 2] as f64) / 3.0;
326 }
327 y += vstep;
328 }
329 });
330 let n = samples.max(1) as f64;
331 for x in 0..w {
332 self.native_col_change[x] += change[x] / n;
333 self.native_col_mean[x] += mean[x] / n;
334 }
335 }
336}
337
338fn gain(bins: &[f64], models: usize, output: &mut [f64]) {
341 let count = output.len();
342 let mut total = vec![0.0f64; models];
343 let mut left = vec![0.0f64; models];
344 let mut weight = 0.0f64;
345 for k in 0..count {
346 for m in 0..models {
347 total[m] += bins[k * models + m];
348 weight += bins[k * models + m];
349 }
350 }
351 if weight < 10.0 {
352 return;
353 }
354 let all = total.iter().copied().fold(f64::NEG_INFINITY, f64::max);
356 for k in 1..count {
357 let (mut a, mut b) = (0.0f64, 0.0f64);
358 for m in 0..models {
359 left[m] += bins[(k - 1) * models + m];
360 a = a.max(left[m]);
361 b = b.max(total[m] - left[m]);
362 }
363 output[k] += (a + b - all) / weight;
364 }
365}
366
367#[cfg(test)]
368mod tests {
369 use super::*;
370
371 #[test]
372 fn uninformative_fields_leave_every_accumulator_untouched() {
373 let mut learner = Learner::new(48, 24);
374 let motions = [FieldMotion {
375 x: 0.0,
376 y: 0.0,
377 support: 10,
378 confidence: 1.0,
379 }];
380 let cells = learner.cols * learner.rows;
381 let field = Field {
382 motions: &motions,
383 labels: &vec![0u8; cells],
384 confidence: &vec![255u8; cells],
385 dynamic: &vec![0u8; cells],
386 cols: learner.cols,
387 rows: learner.rows,
388 difference: 1.0,
389 unknown: false,
390 };
391 let gray = vec![7u8; 48 * 24];
392 assert!(!learner.add(&field, &gray, &gray, None));
393 assert_eq!(learner.informative_frames, 0);
394 assert!(learner.row_change.iter().all(|&v| v == 0.0));
395 }
396}