1use crate::kernels::geometry::{js_ceil, js_round, Rect, Rng};
6
7pub const MOTION_CELL: usize = 24;
10
11#[derive(Clone, Copy, Debug, PartialEq)]
12pub struct Point {
13 pub x: f64,
14 pub y: f64,
15}
16
17#[derive(Clone, Debug, PartialEq)]
18pub struct Motion {
19 pub x: f64,
20 pub y: f64,
21 pub support: u32,
22 pub unique: u32,
23 pub confidence: f64,
24 pub error: f64,
25 pub ambiguous: bool,
26}
27
28#[derive(Clone, Copy)]
29pub struct Gray<'a> {
30 pub width: usize,
31 pub height: usize,
32 pub data: &'a [u8],
33}
34
35#[derive(Clone, Copy, Debug)]
37pub struct MatchPoints {
38 pub ax: f64,
39 pub ay: f64,
40 pub bx: f64,
41 pub by: f64,
42 pub unique: bool,
43}
44
45fn median(values: &mut [f64]) -> f64 {
46 if values.is_empty() {
47 return 0.0;
48 }
49 values.sort_by(|a, b| a.partial_cmp(b).unwrap_or(std::cmp::Ordering::Equal));
50 let m = values.len() >> 1;
51 if values.len() & 1 == 1 {
52 values[m]
53 } else {
54 (values[m - 1] + values[m]) / 2.0
55 }
56}
57
58#[inline]
59fn hypot(x: f64, y: f64) -> f64 {
60 libm::hypot(x, y)
61}
62
63pub fn translation_hypotheses(matches: &[MatchPoints], max: usize) -> Vec<Motion> {
65 let mut bins: Vec<((i32, i32), Vec<usize>)> = Vec::new();
67 for (i, m) in matches.iter().enumerate() {
68 let key = (js_round((m.ax - m.bx) / 3.0), js_round((m.ay - m.by) / 3.0));
69 match bins.iter_mut().find(|(k, _)| *k == key) {
70 Some((_, b)) => b.push(i),
71 None => bins.push((key, vec![i])),
72 }
73 }
74 bins.sort_by(|a, b| b.1.len().cmp(&a.1.len()));
75 bins.truncate(32);
76 let mut out: Vec<Motion> = Vec::new();
77 for (_, seed) in &bins {
78 let mut xs: Vec<f64> = seed
79 .iter()
80 .map(|&i| matches[i].ax - matches[i].bx)
81 .collect();
82 let mut ys: Vec<f64> = seed
83 .iter()
84 .map(|&i| matches[i].ay - matches[i].by)
85 .collect();
86 let (x, y) = (median(&mut xs), median(&mut ys));
87 let support: Vec<&MatchPoints> = matches
88 .iter()
89 .filter(|m| hypot(m.ax - m.bx - x, m.ay - m.by - y) <= 2.5)
90 .collect();
91 if support.len() < 3 {
92 continue;
93 }
94 let mut sx: Vec<f64> = support.iter().map(|m| m.ax - m.bx).collect();
95 let mut sy: Vec<f64> = support.iter().map(|m| m.ay - m.by).collect();
96 let (x, y) = (median(&mut sx), median(&mut sy));
97 if out.iter().any(|p| hypot(p.x - x, p.y - y) < 3.0) {
98 continue;
99 }
100 let unique = support.iter().filter(|m| m.unique).count();
101 let mut cells: Vec<(i32, i32)> = support
102 .iter()
103 .map(|m| ((m.bx as i32) >> 5, (m.by as i32) >> 5))
104 .collect();
105 cells.sort_unstable();
106 cells.dedup();
107 let spread = cells.len() as f64;
108 let n = support.len() as f64;
109 let confidence = ((1.0 - libm::exp(-n / 9.0))
110 * (0.48 + 0.52 * unique as f64 / n)
111 * (spread / 4.0).min(1.0))
112 .clamp(0.0, 0.99);
113 out.push(Motion {
114 x,
115 y,
116 support: support.len() as u32,
117 unique: unique as u32,
118 confidence,
119 error: 0.0,
120 ambiguous: (unique as f64) < (6.0f64).min(n * 0.2),
121 });
122 }
123 out.sort_by(|a, b| (b.support + b.unique).cmp(&(a.support + a.unique)));
124 out.truncate(max);
125 out
126}
127
128pub struct PatchError {
129 pub error: f64,
130 pub texture: f64,
131 pub n: u32,
132}
133
134pub fn patch_error(a: Gray<'_>, b: Gray<'_>, dx: f64, dy: f64, r: Rect, step: usize) -> PatchError {
136 let (w, h, aw, ah) = (
137 b.width as f64,
138 b.height as f64,
139 a.width as f64,
140 a.height as f64,
141 );
142 let x0 = 1f64.max(r.x.ceil()).max((-dx + 1.0).ceil());
143 let x1 = (w - 1.0).min(r.x + r.width).min(aw - dx - 1.0);
144 let y0 = 1f64.max(r.y.ceil()).max((-dy + 1.0).ceil());
145 let y1 = (h - 1.0).min(r.y + r.height).min(ah - dy - 1.0);
146 let (dx, dy) = (js_round(dx), js_round(dy));
147 let (mut total, mut texture, mut n) = (0f64, 0f64, 0u32);
148 let (bw, aw) = (b.width as i64, a.width as i64);
149 let mut y = y0;
150 while y < y1 {
151 let mut x = x0;
152 while x < x1 {
153 let i = (y as i64 * bw + x as i64) as usize;
154 let j = ((y as i64 + dy as i64) * aw + x as i64 + dx as i64) as usize;
155 let bd = b.data;
156 let gradient = (bd[i + 1] as i32 - bd[i - 1] as i32).abs()
157 + (bd[i + b.width] as i32 - bd[i - b.width] as i32).abs();
158 let weight = if gradient > 12 { 2.0 } else { 1.0 };
159 total += (70.0f64).min((a.data[j] as i32 - bd[i] as i32).abs() as f64) * weight;
160 texture += gradient as f64;
161 n += if gradient > 12 { 2 } else { 1 };
162 x += step as f64;
163 }
164 y += step as f64;
165 }
166 PatchError {
167 error: if n > 8 {
168 total / n as f64
169 } else {
170 f64::INFINITY
171 },
172 texture: if n > 0 { texture / n as f64 } else { 0.0 },
173 n,
174 }
175}
176
177pub fn verify_translation(a: Gray<'_>, b: Gray<'_>, dx: f64, dy: f64, roi: Option<Rect>) -> f64 {
179 let r = roi.unwrap_or(Rect {
180 x: 0.0,
181 y: 0.0,
182 width: b.width as f64,
183 height: b.height as f64,
184 });
185 let mut errors: Vec<f64> = Vec::new();
186 let mut y = r.y;
187 while y < r.y + r.height {
188 let mut x = r.x;
189 while x < r.x + r.width {
190 let block = Rect {
191 x,
192 y,
193 width: (48.0f64).min(r.x + r.width - x),
194 height: (48.0f64).min(r.y + r.height - y),
195 };
196 let p = patch_error(a, b, dx, dy, block, 4);
197 if p.n > 12 && p.texture > 3.0 {
198 errors.push(p.error);
199 }
200 x += 48.0;
201 }
202 y += 48.0;
203 }
204 if errors.len() < 2 {
205 return f64::INFINITY;
206 }
207 errors.sort_by(|a, b| a.partial_cmp(b).unwrap_or(std::cmp::Ordering::Equal));
208 let keep = 2usize.max(js_ceil(errors.len() as f64 * 0.75) as usize);
209 errors.iter().take(keep).sum::<f64>() / keep as f64
210}
211
212#[derive(Clone, Copy, Debug, PartialEq)]
213pub struct Audit {
214 pub error: f64,
215 pub mismatch: f64,
216 pub overlap: f64,
217 pub samples: u32,
218 pub blocks: u32,
219 pub agreeing: u32,
220 pub agreement: f64,
221 pub agreeing_error: f64,
222}
223
224pub fn audit_translation(
226 a: Gray<'_>,
227 b: Gray<'_>,
228 dx: f64,
229 dy: f64,
230 roi: Option<Rect>,
231 tolerant: bool,
232) -> Audit {
233 TranslationAudit::new(a, b, tolerant).measure(dx, dy, roi)
234}
235
236pub(crate) struct TranslationAudit<'a> {
240 a: Gray<'a>,
241 b: Gray<'a>,
242 filtered: Option<(Vec<u8>, Vec<u8>)>,
243}
244
245impl<'a> TranslationAudit<'a> {
246 pub(crate) fn new(a: Gray<'a>, b: Gray<'a>, tolerant: bool) -> Self {
247 let filtered = tolerant.then(|| {
250 let mut aa = vec![0; a.data.len()];
251 let mut bb = vec![0; b.data.len()];
252 crate::kernels::features::smooth(a.data, a.width, a.height, &mut aa);
253 crate::kernels::features::smooth(b.data, b.width, b.height, &mut bb);
254 (aa, bb)
255 });
256 Self { a, b, filtered }
257 }
258
259 pub(crate) fn measure(&self, dx: f64, dy: f64, roi: Option<Rect>) -> Audit {
260 let (texture_a, texture_b) = (self.a, self.b);
261 let tolerant = self.filtered.is_some();
262 let (a, b) = match &self.filtered {
263 Some((aa, bb)) => (Gray { data: aa, ..self.a }, Gray { data: bb, ..self.b }),
264 None => (self.a, self.b),
265 };
266 let r = roi.unwrap_or(Rect {
267 x: 0.0,
268 y: 0.0,
269 width: b.width as f64,
270 height: b.height as f64,
271 });
272 let (dx, dy) = (js_round(dx) as f64, js_round(dy) as f64);
273 let x0 = 2f64.max(r.x.ceil()).max((r.x - dx).ceil()).max(1.0 - dx);
276 let y0 = 2f64.max(r.y.ceil()).max((r.y - dy).ceil()).max(1.0 - dy);
277 let x1 = (b.width as f64 - 2.0)
278 .min(a.width as f64 - dx - 2.0)
279 .min(r.x + r.width)
280 .min(r.x + r.width - dx);
281 let y1 = (b.height as f64 - 2.0)
282 .min(a.height as f64 - dy - 2.0)
283 .min(r.y + r.height)
284 .min(r.y + r.height - dy);
285 let overlap = (x1 - x0).max(0.0) * (y1 - y0).max(0.0) / (r.width * r.height).max(1.0);
286 const B: f64 = 32.0;
287 let bw = 1usize.max(js_ceil((x1 - x0) / B).max(0) as usize);
288 let bh = 1usize.max(js_ceil((y1 - y0) / B).max(0) as usize);
289 let mut block_error = vec![0f64; bw * bh];
290 let mut block_bad = vec![0f64; bw * bh];
291 let mut block_n = vec![0f64; bw * bh];
292 let (mut error, mut bad, mut n) = (0f64, 0u32, 0u32);
293 let (bwid, awid) = (b.width as i64, a.width as i64);
294 let (dxi, dyi) = (dx as i64, dy as i64);
295 let mut y = y0;
296 while y < y1 {
297 let mut x = x0;
298 while x < x1 {
299 let i = (y as i64 * bwid + x as i64) as usize;
300 let j = ((y as i64 + dyi) * awid + x as i64 + dxi) as usize;
301 let (ad, bd) = (texture_a.data, texture_b.data);
304 let gb = (bd[i + 1] as i32 - bd[i - 1] as i32).abs()
305 + (bd[i + b.width] as i32 - bd[i - b.width] as i32).abs();
306 let ga = (ad[j + 1] as i32 - ad[j - 1] as i32).abs()
307 + (ad[j + a.width] as i32 - ad[j - a.width] as i32).abs();
308 if ga.max(gb) < 20 {
309 x += 2.0;
310 continue;
311 }
312 let filtered_here = tolerant
314 && x >= r.x + 2.0
315 && x + dx >= r.x + 2.0
316 && y >= r.y + 2.0
317 && y + dy >= r.y + 2.0
318 && x < r.x + r.width - 2.0
319 && x + dx < r.x + r.width - 2.0
320 && y < r.y + r.height - 2.0
321 && y + dy < r.y + r.height - 2.0;
322 let (ad, bd) = if filtered_here {
323 (a.data, b.data)
324 } else {
325 (texture_a.data, texture_b.data)
326 };
327 let mut difference = (ad[j] as i32 - bd[i] as i32).abs();
328 if tolerant {
329 difference = difference
330 .min((ad[j + 1] as i32 - bd[i] as i32).abs())
331 .min((ad[j - 1] as i32 - bd[i] as i32).abs())
332 .min((ad[j + a.width] as i32 - bd[i] as i32).abs())
333 .min((ad[j - a.width] as i32 - bd[i] as i32).abs());
334 }
335 let clipped = difference.min(70) as f64;
336 let q = (((y - y0) / B).floor() as usize) * bw + ((x - x0) / B).floor() as usize;
337 error += clipped;
338 block_error[q] += clipped;
339 block_n[q] += 1.0;
340 if difference > 18 {
341 bad += 1;
342 block_bad[q] += 1.0;
343 }
344 n += 1;
345 x += 2.0;
346 }
347 y += 2.0;
348 }
349 let (mut blocks, mut agreeing, mut agreeing_error) = (0u32, 0u32, 0f64);
350 for q in 0..block_n.len() {
351 if block_n[q] < 12.0 {
352 continue;
353 }
354 blocks += 1;
355 if block_error[q] / block_n[q] < 10.0 && block_bad[q] / block_n[q] < 0.2 {
356 agreeing += 1;
357 agreeing_error += block_error[q] / block_n[q];
358 }
359 }
360 Audit {
361 error: if n >= 24 {
362 error / n as f64
363 } else {
364 f64::INFINITY
365 },
366 mismatch: if n > 0 { bad as f64 / n as f64 } else { 1.0 },
367 overlap,
368 samples: n,
369 blocks,
370 agreeing,
371 agreement: if blocks > 0 {
372 agreeing as f64 / blocks as f64
373 } else {
374 0.0
375 },
376 agreeing_error: if agreeing > 0 {
377 agreeing_error / agreeing as f64
378 } else {
379 f64::INFINITY
380 },
381 }
382 }
383}
384
385pub fn refine_translation(
387 a: Gray<'_>,
388 b: Gray<'_>,
389 p: Point,
390 roi: Option<Rect>,
391 radius: i32,
392) -> (i32, i32) {
393 let r = roi.unwrap_or(Rect {
394 x: 0.0,
395 y: 0.0,
396 width: b.width as f64,
397 height: b.height as f64,
398 });
399 let (px, py) = (js_round(p.x), js_round(p.y));
400 let (mut best, mut error) = ((px, py), f64::INFINITY);
401 for dy in py - radius..=py + radius {
402 for dx in px - radius..=px + radius {
403 let e = patch_error(a, b, dx as f64, dy as f64, r, 7).error;
404 let closer = hypot(dx as f64 - p.x, dy as f64 - p.y)
405 < hypot(best.0 as f64 - p.x, best.1 as f64 - p.y);
406 if e < error - 1e-4 || ((e - error).abs() < 1e-4 && closer) {
407 error = e;
408 best = (dx, dy);
409 }
410 }
411 }
412 best
413}
414
415pub fn detect_scale(matches: &[MatchPoints]) -> f64 {
417 let m: Vec<&MatchPoints> = matches.iter().filter(|m| m.unique).collect();
418 if m.len() < 8 {
419 return 1.0;
420 }
421 let mut rng = Rng::new(7641);
422 let (mut best, mut best_scale) = (0usize, 1.0);
423 for _ in 0..96 {
424 let p = m[(rng.next_f64() * m.len() as f64).floor() as usize];
425 let q = m[(rng.next_f64() * m.len() as f64).floor() as usize];
426 let (bx, by, ax, ay) = (q.bx - p.bx, q.by - p.by, q.ax - p.ax, q.ay - p.ay);
427 let den = bx * bx + by * by;
428 if den < 1000.0 {
429 continue;
430 }
431 let u = (ax * bx + ay * by) / den;
432 let v = (ay * bx - ax * by) / den;
433 let s = hypot(u, v);
434 if !(0.55..=1.8).contains(&s) {
435 continue;
436 }
437 let tx = p.ax - u * p.bx + v * p.by;
438 let ty = p.ay - v * p.bx - u * p.by;
439 let n = m
440 .iter()
441 .filter(|r| {
442 hypot(
443 u * r.bx - v * r.by + tx - r.ax,
444 v * r.bx + u * r.by + ty - r.ay,
445 ) < 2.5
446 })
447 .count();
448 if n > best {
449 best = n;
450 best_scale = s;
451 }
452 }
453 if best as f64 >= (8.0f64).max(m.len() as f64 * 0.5) {
454 best_scale
455 } else {
456 1.0
457 }
458}
459
460#[derive(Clone, Debug, PartialEq)]
461pub struct MotionField {
462 pub motions: Vec<Motion>,
463 pub labels: Vec<u8>,
464 pub confidence: Vec<u8>,
465 pub dynamic: Vec<u8>,
466 pub cols: usize,
467 pub rows: usize,
468 pub cell: usize,
469 pub difference: f64,
470 pub feature_count: u32,
471 pub unknown: bool,
472 pub zoom: f64,
473}
474
475pub fn estimate_motion(
477 a: Gray<'_>,
478 b: Gray<'_>,
479 matches: &[MatchPoints],
480 feature_count: u32,
481) -> MotionField {
482 let cols = b.width.div_ceil(MOTION_CELL);
483 let rows = b.height.div_ceil(MOTION_CELL);
484 let n = cols * rows;
485 let difference = crate::kernels::raster::mean_difference(a.data, b.data);
486 if difference < 0.12 {
487 return MotionField {
488 motions: vec![Motion {
489 x: 0.0,
490 y: 0.0,
491 support: 0,
492 unique: 0,
493 confidence: 0.98,
494 error: difference,
495 ambiguous: false,
496 }],
497 labels: vec![0; n],
498 confidence: vec![245; n],
499 dynamic: vec![0; n],
500 cols,
501 rows,
502 cell: MOTION_CELL,
503 difference,
504 feature_count: 0,
505 unknown: false,
506 zoom: 1.0,
507 };
508 }
509 let mut motions = translation_hypotheses(matches, 6);
510 if !motions.iter().any(|m| hypot(m.x, m.y) < 1.0) {
511 motions.push(Motion {
512 x: 0.0,
513 y: 0.0,
514 support: 0,
515 unique: 0,
516 confidence: 0.5,
517 error: 0.0,
518 ambiguous: false,
519 });
520 }
521 if motions.is_empty() {
522 motions.push(Motion {
523 x: 0.0,
524 y: 0.0,
525 support: 0,
526 unique: 0,
527 confidence: 0.0,
528 error: 255.0,
529 ambiguous: true,
530 });
531 }
532 for m in motions.iter_mut() {
533 let inliers: Vec<&MatchPoints> = matches
534 .iter()
535 .filter(|p| hypot(p.ax - p.bx - m.x, p.ay - p.by - m.y) < 3.0)
536 .take(45)
537 .collect();
538 let (mut best, mut bx, mut by) = (f64::INFINITY, m.x, m.y);
539 let (mx, my) = (js_round(m.x), js_round(m.y));
540 for y in my - 1..=my + 1 {
541 for x in mx - 1..=mx + 1 {
542 let (mut sum, mut k) = (0f64, 0u32);
543 for p in &inliers {
544 let e = patch_error(
545 a,
546 b,
547 x as f64,
548 y as f64,
549 Rect {
550 x: p.bx - 7.0,
551 y: p.by - 7.0,
552 width: 15.0,
553 height: 15.0,
554 },
555 3,
556 )
557 .error;
558 if e.is_finite() {
559 sum += e;
560 k += 1;
561 }
562 }
563 if k > 0 && sum / (k as f64) < best {
564 best = sum / k as f64;
565 bx = x as f64;
566 by = y as f64;
567 }
568 }
569 }
570 m.x = bx;
571 m.y = by;
572 m.error = if best.is_finite() {
573 best
574 } else {
575 verify_translation(a, b, m.x, m.y, None)
576 };
577 if m.support > 0 {
578 m.confidence *= libm::exp(-(m.error.min(80.0)) / 35.0);
579 }
580 }
581 motions.sort_by(|a, b| (b.support + b.unique).cmp(&(a.support + a.unique)));
582 let dominant = motions
583 .iter()
584 .position(|m| hypot(m.x, m.y) > 1.0 && m.support >= 4)
585 .unwrap_or(0);
586 let mut labels = vec![0u8; n];
587 let mut confidence = vec![0u8; n];
588 let mut dynamic = vec![0u8; n];
589 let mut informative = vec![0u8; n];
590 for cy in 0..rows {
591 for cx in 0..cols {
592 let idx = cy * cols + cx;
593 let r = Rect {
594 x: (cx * MOTION_CELL) as f64,
595 y: (cy * MOTION_CELL) as f64,
596 width: MOTION_CELL.min(b.width - cx * MOTION_CELL) as f64,
597 height: MOTION_CELL.min(b.height - cy * MOTION_CELL) as f64,
598 };
599 let (mut best, mut second, mut choice, mut texture, mut dominant_error) =
600 (f64::INFINITY, f64::INFINITY, dominant, 0f64, f64::INFINITY);
601 for (k, m) in motions.iter().enumerate() {
602 let p = patch_error(a, b, m.x, m.y, r, 3);
603 texture = texture.max(p.texture);
604 if k == dominant {
605 dominant_error = p.error;
606 }
607 if p.error < best {
608 second = best;
609 best = p.error;
610 choice = k;
611 } else if p.error < second {
612 second = p.error;
613 }
614 }
615 if texture < 3.0
616 || !best.is_finite()
617 || second - best < 0.6
618 || (!dominant_error.is_finite() && choice != dominant)
619 {
620 labels[idx] = dominant as u8;
621 confidence[idx] = 65;
622 } else {
623 labels[idx] = choice as u8;
624 confidence[idx] = js_round(
625 255.0 * libm::exp(-best / 24.0) * ((second - best) / 8.0).clamp(0.25, 1.0),
626 ) as u8;
627 informative[idx] = (confidence[idx] > 100) as u8;
628 if best > 22.0 {
629 dynamic[idx] = 1;
630 labels[idx] = dominant as u8;
631 confidence[idx] = 45;
632 }
633 }
634 }
635 }
636 for _ in 0..3 {
637 let mut next = labels.clone();
638 for y in 0..rows as i64 {
639 for x in 0..cols as i64 {
640 let i = (y * cols as i64 + x) as usize;
641 if informative[i] != 0 {
642 continue;
643 }
644 let mut votes = vec![0f64; motions.len()];
645 for dy in -2i64..=2 {
646 for dx in -2i64..=2 {
647 let (xx, yy) = (x + dx, y + dy);
648 if xx < 0 || yy < 0 || xx >= cols as i64 || yy >= rows as i64 {
649 continue;
650 }
651 let j = (yy * cols as i64 + xx) as usize;
652 votes[labels[j] as usize] +=
653 confidence[j] as f64 / (1 + dx * dx + dy * dy) as f64;
654 }
655 }
656 let mut k = dominant;
657 for (j, v) in votes.iter().enumerate() {
658 if *v > votes[k] {
659 k = j;
660 }
661 }
662 next[i] = k as u8;
663 }
664 }
665 labels = next;
666 }
667 let strongest =
668 motions.iter().fold(
669 &motions[0],
670 |m, n| if n.confidence > m.confidence { n } else { m },
671 );
672 let unknown = strongest.support < 4 && difference > 5.0;
673 MotionField {
674 motions,
675 labels,
676 confidence,
677 dynamic,
678 cols,
679 rows,
680 cell: MOTION_CELL,
681 difference,
682 feature_count,
683 unknown,
684 zoom: detect_scale(matches),
685 }
686}
687
688pub struct NativeRefinement {
689 pub x: i32,
690 pub y: i32,
691 pub error: f64,
692 pub samples: u32,
693 pub runner_up: f64,
694 pub trimmed: f64,
698}
699
700pub const TRIMMED_KEEP: f64 = 0.75;
704
705#[allow(clippy::too_many_arguments)]
708fn trimmed_error(
709 a: &[u8],
710 b: &[u8],
711 w: i64,
712 h: i64,
713 dx: i64,
714 dy: i64,
715 inside: impl Fn(i64, i64) -> bool,
716 points: &[i64],
717) -> f64 {
718 let mut errors: Vec<f64> = Vec::with_capacity(points.len());
719 for &p in points {
720 let (x, y) = (p % w, p / w);
721 let (xx, yy) = (x + dx, y + dy);
722 if xx < 1 || yy < 1 || xx >= w - 1 || yy >= h - 1 || !inside(xx, yy) {
723 continue;
724 }
725 let i = (p * 4) as usize;
726 let j = ((yy * w + xx) * 4) as usize;
727 let d = (a[j] as i32 - b[i] as i32).abs()
728 + (a[j + 1] as i32 - b[i + 1] as i32).abs()
729 + (a[j + 2] as i32 - b[i + 2] as i32).abs();
730 errors.push((90.0f64).min(d as f64 / 3.0));
731 }
732 if errors.len() < 12 {
733 return f64::INFINITY;
734 }
735 errors.sort_by(|x, y| x.total_cmp(y));
736 let keep = ((errors.len() as f64) * TRIMMED_KEEP).ceil() as usize;
737 errors[..keep].iter().sum::<f64>() / keep as f64
738}
739
740pub fn select_native_points(
763 b: &[u8],
764 width: usize,
765 height: usize,
766 region: Rect,
767 mask: Option<(&[u8], u8)>,
768 guide: Option<(Gray<'_>, usize)>,
769) -> Vec<i64> {
770 let (w, h) = (width as i64, height as i64);
771 let step = 3i64.max(((region.width * region.height / 1600.0).sqrt()).floor() as i64);
772 let inside =
773 |x: i64, y: i64| mask.is_none_or(|(labels, code)| labels[(y * w + x) as usize] == code);
774 let y_limit = ((h - 2) as f64).min(region.y + region.height);
776 let x_limit = ((w - 2) as f64).min(region.x + region.width);
777 let x_start = 2i64.max(region.x.ceil() as i64);
778 let y_start = 2i64.max(region.y.ceil() as i64);
779 if (x_start as f64) >= x_limit || (y_start as f64) >= y_limit {
780 return Vec::new();
781 }
782 if let Some((g, f)) = guide {
783 return select_native_points_guided(
784 b,
785 w,
786 h,
787 x_start,
788 y_start,
789 x_limit,
790 y_limit,
791 step,
792 &inside,
793 g,
794 f.max(1) as i64,
795 );
796 }
797 let (bx0, by0) = ((x_start - 1).max(0), (y_start - 1).max(0));
803 let (bx1, by1) = (
804 (js_ceil(x_limit) as i64 + 1).min(w),
805 (js_ceil(y_limit) as i64 + 1).min(h),
806 );
807 let (bw, bh) = ((bx1 - bx0).max(0) as usize, (by1 - by0).max(0) as usize);
808 let mut luma_plane = vec![0u8; bw * bh];
810 for row in 0..bh {
811 let src = (((by0 + row as i64) * w + bx0) * 4) as usize;
812 let dst = row * bw;
813 for col in 0..bw {
814 let i = src + col * 4;
815 luma_plane[dst + col] =
816 ((b[i] as u32 * 77 + b[i + 1] as u32 * 150 + b[i + 2] as u32 * 29) >> 8) as u8;
817 }
818 }
819 let mut points: Vec<i64> = Vec::new();
820 let mut cy = y_start;
821 while (cy as f64) < y_limit {
822 let mut cx = x_start;
823 while (cx as f64) < x_limit {
824 let mut best: Option<(i32, i64)> = None;
826 let mut dy = 0i64;
827 while dy < step {
828 let y = cy + dy;
829 if (y as f64) >= y_limit {
830 break;
831 }
832 let (row_mid, row_up, row_down) = (
834 (y - by0) as usize * bw,
835 (y - 1 - by0) as usize * bw,
836 (y + 1 - by0) as usize * bw,
837 );
838 let mut dx = 0i64;
839 while dx < step {
840 let x = cx + dx;
841 if (x as f64) >= x_limit {
842 break;
843 }
844 if inside(x, y) {
845 let col = (x - bx0) as usize;
846 let grad = (luma_plane[row_mid + col - 1] as i32
847 - luma_plane[row_mid + col + 1] as i32)
848 .abs()
849 + (luma_plane[row_up + col] as i32 - luma_plane[row_down + col] as i32)
850 .abs();
851 if best.is_none_or(|(bg, _)| grad > bg) {
852 best = Some((grad, y * w + x));
853 }
854 }
855 dx += 1;
856 }
857 dy += 1;
858 }
859 if let Some((grad, p)) = best {
860 if grad > 12 {
861 points.push(p);
862 }
863 }
864 cx += step;
865 }
866 cy += step;
867 }
868 points
869}
870
871#[allow(clippy::too_many_arguments)]
876fn select_native_points_guided(
877 b: &[u8],
878 w: i64,
879 h: i64,
880 x_start: i64,
881 y_start: i64,
882 x_limit: f64,
883 y_limit: f64,
884 step: i64,
885 inside: &impl Fn(i64, i64) -> bool,
886 g: Gray<'_>,
887 f: i64,
888) -> Vec<i64> {
889 let (gw, gh) = (g.width as i64, g.height as i64);
890 let native_luma = |x: i64, y: i64| -> i32 {
893 let i = ((y * w + x) * 4) as usize;
894 ((b[i] as u32 * 77 + b[i + 1] as u32 * 150 + b[i + 2] as u32 * 29) >> 8) as i32
895 };
896 let (window_x_hi, window_y_hi) = (
902 (js_ceil(x_limit) as i64 - 1).min(w - 1),
903 (js_ceil(y_limit) as i64 - 1).min(h - 1),
904 );
905 let mut points: Vec<i64> = Vec::new();
906 let mut cy = y_start;
907 while (cy as f64) < y_limit {
908 let mut cx = x_start;
909 while (cx as f64) < x_limit {
910 let (cell_x_hi, cell_y_hi) = (
911 (cx + step - 1).min(window_x_hi),
912 (cy + step - 1).min(window_y_hi),
913 );
914 let mut chosen: Option<(i64, i64)> = None;
917 let mut best_analysis_grad = i32::MIN;
918 let mut ay = (cy / f).max(1);
919 while ay <= (cell_y_hi / f).min(gh - 2) {
920 let mut ax = (cx / f).max(1);
921 while ax <= (cell_x_hi / f).min(gw - 2) {
922 let (nx0, ny0) = (ax * f, ay * f);
923 let (nx1, ny1) = ((nx0 + f).min(w), (ny0 + f).min(h));
924 let (cx_mid, cy_mid) = (nx0 + (nx1 - nx0) / 2, ny0 + (ny1 - ny0) / 2);
925 if inside(cx_mid, cy_mid) {
926 let gi = (ay * gw + ax) as usize;
927 let grad = (g.data[gi - 1] as i32 - g.data[gi + 1] as i32).abs()
928 + (g.data[gi - gw as usize] as i32 - g.data[gi + gw as usize] as i32)
929 .abs();
930 if grad > best_analysis_grad {
931 best_analysis_grad = grad;
932 chosen = Some((ax, ay));
933 }
934 }
935 ax += 1;
936 }
937 ay += 1;
938 }
939 if let Some((ax, ay)) = chosen {
942 let (nx0, ny0) = (ax * f, ay * f);
943 let (nx1, ny1) = ((nx0 + f).min(w), (ny0 + f).min(h));
944 let mut best: Option<(i32, i64)> = None;
945 let mut y = ny0.max(cy).max(y_start);
946 let y_hi = ny1.min(cy + step).min(h);
947 while y < y_hi && (y as f64) < y_limit {
948 let mut x = nx0.max(cx).max(x_start);
949 let x_hi = nx1.min(cx + step).min(w);
950 while x < x_hi && (x as f64) < x_limit {
951 if inside(x, y) {
952 let grad = (native_luma(x - 1, y) - native_luma(x + 1, y)).abs()
953 + (native_luma(x, y - 1) - native_luma(x, y + 1)).abs();
954 if best.is_none_or(|(bg, _)| grad > bg) {
955 best = Some((grad, y * w + x));
956 }
957 }
958 x += 1;
959 }
960 y += 1;
961 }
962 if let Some((grad, p)) = best {
963 if grad > 12 {
964 points.push(p);
965 }
966 }
967 }
968 cx += step;
969 }
970 cy += step;
971 }
972 points
973}
974
975#[allow(clippy::too_many_arguments)]
979pub fn refine_native_at(
980 a: &[u8],
981 b: &[u8],
982 width: usize,
983 height: usize,
984 guess: Point,
985 mask: Option<(&[u8], u8)>,
986 radius: i32,
987 points: &[i64],
988) -> NativeRefinement {
989 let (w, h) = (width as i64, height as i64);
990 let (gx, gy) = (js_round(guess.x), js_round(guess.y));
991 let inside =
992 |x: i64, y: i64| mask.is_none_or(|(labels, code)| labels[(y * w + x) as usize] == code);
993 if points.len() < 12 {
994 return NativeRefinement {
995 x: gx,
996 y: gy,
997 error: f64::INFINITY,
998 samples: points.len() as u32,
999 runner_up: f64::INFINITY,
1000 trimmed: f64::INFINITY,
1001 };
1002 }
1003 let mut samples = 0u32;
1004 let mut cost = |dx: i64, dy: i64| -> f64 {
1005 let (mut error, mut n) = (0f64, 0u32);
1006 for &p in points {
1007 let (x, y) = (p % w, p / w);
1008 let (xx, yy) = (x + dx, y + dy);
1009 if xx < 1 || yy < 1 || xx >= w - 1 || yy >= h - 1 || !inside(xx, yy) {
1010 continue;
1011 }
1012 let i = (p * 4) as usize;
1013 let j = ((yy * w + xx) * 4) as usize;
1014 let d = (a[j] as i32 - b[i] as i32).abs()
1015 + (a[j + 1] as i32 - b[i + 1] as i32).abs()
1016 + (a[j + 2] as i32 - b[i + 2] as i32).abs();
1017 error += (90.0f64).min(d as f64 / 3.0);
1018 n += 1;
1019 }
1020 samples = samples.max(n);
1021 if n >= 12 {
1022 error / n as f64
1023 } else {
1024 f64::INFINITY
1025 }
1026 };
1027 let (mut best, mut px, mut py) = (f64::INFINITY, gx, gy);
1028 let mut costs: Vec<(i32, i32, f64)> = Vec::new();
1029 for y in gy - radius..=gy + radius {
1030 for x in gx - radius..=gx + radius {
1031 let e = cost(x as i64, y as i64);
1032 costs.push((x, y, e));
1033 let closer = hypot(x as f64 - guess.x, y as f64 - guess.y)
1034 < hypot(px as f64 - guess.x, py as f64 - guess.y);
1035 if e < best - 1e-6 || ((e - best).abs() < 1e-6 && closer) {
1036 best = e;
1037 px = x;
1038 py = y;
1039 }
1040 }
1041 }
1042 let runner_up = costs
1043 .iter()
1044 .filter(|c| (c.0 - px).abs().max((c.1 - py).abs()) > 2)
1045 .map(|c| c.2)
1046 .fold(f64::INFINITY, f64::min);
1047 let trimmed = if best.is_finite() {
1048 trimmed_error(a, b, w, h, px as i64, py as i64, inside, points)
1049 } else {
1050 f64::INFINITY
1051 };
1052 NativeRefinement {
1053 x: px,
1054 y: py,
1055 error: best,
1056 samples,
1057 runner_up,
1058 trimmed,
1059 }
1060}
1061
1062#[allow(clippy::too_many_arguments)]
1070pub fn sampling_insensitive_error(
1071 a: &[u8],
1072 b: &[u8],
1073 width: usize,
1074 height: usize,
1075 offset: (i32, i32),
1076 mask: Option<(&[u8], u8)>,
1077 radius: i32,
1078 points: &[i64],
1079) -> (f64, f64) {
1080 let (w, h) = (width as i64, height as i64);
1081 let inside =
1082 |x: i64, y: i64| mask.is_none_or(|(labels, code)| labels[(y * w + x) as usize] == code);
1083 let stride = width * 4;
1084 let span = |image: &[u8], k: usize| {
1086 let v = image[k] as i32;
1087 [k - 4, k + 4, k - stride, k + stride]
1088 .iter()
1089 .fold((2 * v, 2 * v), |(low, high), &n| {
1090 (low.min(v + image[n] as i32), high.max(v + image[n] as i32))
1091 })
1092 };
1093 let outside = |v: i32, (low, high): (i32, i32)| (v - high).max(low - v).max(0);
1094 let cost = |dx: i64, dy: i64| -> f64 {
1095 let (mut error, mut n) = (0f64, 0u32);
1096 for &p in points {
1097 let (x, y) = (p % w, p / w);
1098 let (xx, yy) = (x + dx, y + dy);
1099 if xx < 1 || yy < 1 || xx >= w - 1 || yy >= h - 1 || !inside(xx, yy) {
1100 continue;
1101 }
1102 let i = (p * 4) as usize;
1103 let j = ((yy * w + xx) * 4) as usize;
1104 let d: i32 = (0..3)
1105 .map(|c| {
1106 outside(2 * b[i + c] as i32, span(a, j + c))
1107 .min(outside(2 * a[j + c] as i32, span(b, i + c)))
1108 })
1109 .sum();
1110 error += (90.0f64).min(d as f64 / 6.0);
1111 n += 1;
1112 }
1113 if n >= 12 {
1114 error / n as f64
1115 } else {
1116 f64::INFINITY
1117 }
1118 };
1119 let (ox, oy) = offset;
1120 let mut runner_up = f64::INFINITY;
1121 for y in oy - radius..=oy + radius {
1122 for x in ox - radius..=ox + radius {
1123 if (x - ox).abs().max((y - oy).abs()) > 2 {
1124 runner_up = runner_up.min(cost(x as i64, y as i64));
1125 }
1126 }
1127 }
1128 (cost(ox as i64, oy as i64), runner_up)
1129}
1130
1131#[allow(clippy::too_many_arguments)]
1141pub fn screen_fixed_split_error(
1142 a: &[u8],
1143 b: &[u8],
1144 width: usize,
1145 height: usize,
1146 offset: (i32, i32),
1147 mask: Option<(&[u8], u8)>,
1148 radius: i32,
1149 points: &[i64],
1150) -> (f64, f64, f64) {
1151 let (w, h) = (width as i64, height as i64);
1152 let inside =
1153 |x: i64, y: i64| mask.is_none_or(|(labels, code)| labels[(y * w + x) as usize] == code);
1154 let stride = width * 4;
1155 let span = |image: &[u8], k: usize| {
1156 let v = image[k] as i32;
1157 [k - 4, k + 4, k - stride, k + stride]
1158 .iter()
1159 .fold((2 * v, 2 * v), |(low, high), &n| {
1160 (low.min(v + image[n] as i32), high.max(v + image[n] as i32))
1161 })
1162 };
1163 let outside = |v: i32, (low, high): (i32, i32)| (v - high).max(low - v).max(0);
1164 let at = |p: i64, dx: i64, dy: i64| -> Option<(usize, usize)> {
1165 let (x, y) = (p % w, p / w);
1166 let (xx, yy) = (x + dx, y + dy);
1167 if xx < 1 || yy < 1 || xx >= w - 1 || yy >= h - 1 || !inside(xx, yy) {
1168 return None;
1169 }
1170 Some(((p * 4) as usize, ((yy * w + xx) * 4) as usize))
1171 };
1172 let still: Vec<Option<f64>> = points
1173 .iter()
1174 .map(|&p| {
1175 at(p, 0, 0).map(|(i, j)| {
1176 let d = (a[j] as i32 - b[i] as i32).abs()
1177 + (a[j + 1] as i32 - b[i + 1] as i32).abs()
1178 + (a[j + 2] as i32 - b[i + 2] as i32).abs();
1179 (90.0f64).min(d as f64 / 3.0)
1180 })
1181 })
1182 .collect();
1183 let cost = |dx: i64, dy: i64| -> (f64, u32, u32) {
1184 let (mut error, mut n, mut moved, mut stayed) = (0f64, 0u32, 0u32, 0u32);
1185 for (&p, zero) in points.iter().zip(&still) {
1186 if let Some((i, j)) = at(p, dx, dy) {
1187 let d: i32 = (0..3)
1188 .map(|c| {
1189 outside(2 * b[i + c] as i32, span(a, j + c))
1190 .min(outside(2 * a[j + c] as i32, span(b, i + c)))
1191 })
1192 .sum();
1193 let e = (90.0f64).min(d as f64 / 6.0);
1194 if let Some(z) = *zero {
1195 moved += (e < z) as u32;
1196 stayed += (z < e) as u32;
1197 }
1198 error += zero.map_or(e, |z| e.min(z));
1199 n += 1;
1200 }
1201 }
1202 let mean = if n >= 12 {
1203 error / n as f64
1204 } else {
1205 f64::INFINITY
1206 };
1207 (mean, moved, stayed)
1208 };
1209 let (ox, oy) = offset;
1210 let mut runner_up = f64::INFINITY;
1211 for y in oy - radius..=oy + radius {
1212 for x in ox - radius..=ox + radius {
1213 if (x - ox).abs().max((y - oy).abs()) > 2 {
1214 runner_up = runner_up.min(cost(x as i64, y as i64).0);
1215 }
1216 }
1217 }
1218 let (error, moved, stayed) = cost(ox as i64, oy as i64);
1219 let share = if moved + stayed > 0 {
1220 moved as f64 / (moved + stayed) as f64
1221 } else {
1222 0.0
1223 };
1224 (error, runner_up, share)
1225}
1226
1227#[allow(clippy::too_many_arguments)]
1232pub fn refine_native(
1233 a: &[u8],
1234 b: &[u8],
1235 width: usize,
1236 height: usize,
1237 guess: Point,
1238 region: Rect,
1239 mask: Option<(&[u8], u8)>,
1240 radius: i32,
1241) -> NativeRefinement {
1242 let points = select_native_points(b, width, height, region, mask, None);
1243 refine_native_at(a, b, width, height, guess, mask, radius, &points)
1244}
1245
1246pub struct Patch<'a> {
1248 pub x: i32,
1249 pub y: i32,
1250 pub size: usize,
1251 pub data: &'a [u8],
1252}
1253
1254pub fn refine_patches(
1256 patches: &[Patch<'_>],
1257 native: Gray<'_>,
1258 region: Rect,
1259 guess: Point,
1260 radius: i32,
1261) -> NativeRefinement {
1262 refine_weighted_patches(patches, native, region, guess, radius, &[])
1263}
1264
1265pub fn refine_spatial_patches(
1269 patches: &[Patch<'_>],
1270 native: Gray<'_>,
1271 region: Rect,
1272 guess: Point,
1273 radius: i32,
1274) -> NativeRefinement {
1275 let mut weights = vec![0.0; patches.len()];
1276 if let Some(first) = patches.first() {
1277 let step = first.size;
1278 for y in (step / 2..js_round(region.height) as usize).step_by(step) {
1279 for x in (step / 2..js_round(region.width) as usize).step_by(step) {
1280 let nearest = patches
1281 .iter()
1282 .enumerate()
1283 .min_by_key(|(_, p)| {
1284 let dx = x as i64 - p.x as i64 - p.size as i64 / 2;
1285 let dy = y as i64 - p.y as i64 - p.size as i64 / 2;
1286 dx * dx + dy * dy
1287 })
1288 .unwrap()
1289 .0;
1290 weights[nearest] += 1.0;
1291 }
1292 }
1293 }
1294 refine_weighted_patches(patches, native, region, guess, radius, &weights)
1295}
1296
1297fn refine_weighted_patches(
1298 patches: &[Patch<'_>],
1299 native: Gray<'_>,
1300 region: Rect,
1301 guess: Point,
1302 radius: i32,
1303 weights: &[f64],
1304) -> NativeRefinement {
1305 let (rx, ry, rw, rh) = (
1306 js_round(region.x),
1307 js_round(region.y),
1308 js_round(region.width),
1309 js_round(region.height),
1310 );
1311 let w = native.width as i64;
1312 let (gx, gy) = (js_round(guess.x), js_round(guess.y));
1313 let region_inside = rx >= 0
1314 && ry >= 0
1315 && rw >= 0
1316 && rh >= 0
1317 && (rx + rw) as usize <= native.width
1318 && (ry + rh) as usize <= native.height;
1319 if patches.is_empty() || !region_inside {
1320 return NativeRefinement {
1321 x: gx,
1322 y: gy,
1323 error: f64::INFINITY,
1324 samples: 0,
1325 runner_up: f64::INFINITY,
1326 trimmed: f64::INFINITY,
1327 };
1328 }
1329 let mut samples = 0u32;
1330 let mut cost = |dx: i32, dy: i32| -> f64 {
1331 let (mut error, mut n, mut weight_sum) = (0f64, 0u32, 0f64);
1332 for (i, p) in patches.iter().enumerate() {
1333 let weight = weights.get(i).copied().unwrap_or(1.0);
1334 let (cx, cy) = (p.x - dx, p.y - dy);
1335 if cx < 0 || cy < 0 || cx + p.size as i32 > rw || cy + p.size as i32 > rh {
1336 continue;
1337 }
1338 let mut py = 0usize;
1339 while py < p.size {
1340 let row_a = ((ry + cy) as i64 + py as i64) * w + (rx + cx) as i64;
1341 let row_p = py * p.size;
1342 let mut px = 0usize;
1343 while px < p.size {
1344 error += weight
1345 * (90.0f64).min(
1346 (native.data[(row_a + px as i64) as usize] as i32
1347 - p.data[row_p + px] as i32)
1348 .abs() as f64,
1349 );
1350 n += 1;
1351 weight_sum += weight;
1352 px += 2;
1353 }
1354 py += 2;
1355 }
1356 }
1357 samples = samples.max(n);
1358 if n >= 64 {
1359 error / weight_sum
1360 } else {
1361 f64::INFINITY
1362 }
1363 };
1364 let (mut best, mut bx, mut by) = (f64::INFINITY, gx, gy);
1365 let mut costs: Vec<(i32, i32, f64)> = Vec::new();
1366 for y in gy - radius..=gy + radius {
1367 for x in gx - radius..=gx + radius {
1368 let e = cost(x, y);
1369 costs.push((x, y, e));
1370 let closer = hypot(x as f64 - guess.x, y as f64 - guess.y)
1371 < hypot(bx as f64 - guess.x, by as f64 - guess.y);
1372 if e < best - 1e-6 || ((e - best).abs() < 1e-6 && closer) {
1373 best = e;
1374 bx = x;
1375 by = y;
1376 }
1377 }
1378 }
1379 let runner_up = costs
1380 .iter()
1381 .filter(|c| (c.0 - bx).abs().max((c.1 - by).abs()) > 2)
1382 .map(|c| c.2)
1383 .fold(f64::INFINITY, f64::min);
1384 NativeRefinement {
1385 x: bx,
1386 y: by,
1387 error: best,
1388 samples,
1389 runner_up,
1390 trimmed: f64::INFINITY,
1391 }
1392}
1393
1394pub struct ExtractedPatch {
1396 pub x: f64,
1397 pub y: f64,
1398 pub size: usize,
1399 pub data: Vec<u8>,
1400}
1401
1402#[allow(clippy::too_many_arguments)]
1408pub fn extract_patches(
1409 native: &[u8],
1410 native_width: usize,
1411 native_height: usize,
1412 region: Rect,
1413 features: &[(f64, f64)],
1414 factor: f64,
1415 count: usize,
1416 size: usize,
1417) -> Vec<ExtractedPatch> {
1418 let mut out: Vec<ExtractedPatch> = Vec::new();
1419 let (rx, ry, rw, rh) = (
1420 js_round(region.x),
1421 js_round(region.y),
1422 js_round(region.width),
1423 js_round(region.height),
1424 );
1425 if (rw as f64) < size as f64 + 2.0 || (rh as f64) < size as f64 + 2.0 {
1426 return out;
1427 }
1428 if rx < 0 || ry < 0 || (rx + rw) as usize > native_width || (ry + rh) as usize > native_height {
1429 return out;
1430 }
1431 let mut taken: Vec<(f64, f64)> = Vec::new();
1432 for &(fx, fy) in features {
1433 if out.len() >= count {
1434 break;
1435 }
1436 let x = (js_round(fx * factor) as f64 - size as f64 / 2.0)
1437 .clamp(0.0, (rw - size as i32) as f64);
1438 let y = (js_round(fy * factor) as f64 - size as f64 / 2.0)
1439 .clamp(0.0, (rh - size as i32) as f64);
1440 if taken
1441 .iter()
1442 .any(|&(tx, ty)| (tx - x).abs() < size as f64 && (ty - y).abs() < size as f64)
1443 {
1444 continue;
1445 }
1446 taken.push((x, y));
1447 let mut data = vec![0u8; size * size];
1448 for row in 0..size {
1449 let src = ((ry + y as i32 + row as i32) as i64 * native_width as i64
1450 + (rx + x as i32) as i64) as usize;
1451 data[row * size..row * size + size].copy_from_slice(&native[src..src + size]);
1452 }
1453 out.push(ExtractedPatch { x, y, size, data });
1454 }
1455 out
1456}
1457
1458pub fn probe_scale(
1463 previous: Gray<'_>,
1464 current: Gray<'_>,
1465 current_features: &[crate::kernels::features::Feature],
1466 roi: Option<Rect>,
1467 scales: &[f64],
1468) -> Option<(f64, f64)> {
1469 let mut best: Option<(f64, f64)> = None;
1470 for &scale in scales {
1471 let (sw, sh, sdata) = resample_gray(previous, scale);
1472 let scaled_features = crate::kernels::features::extract_features(&sdata, sw, sh, 320, None);
1473 let matches =
1474 crate::kernels::features::match_features(&scaled_features, current_features, true);
1475 let match_points: Vec<MatchPoints> = matches
1476 .iter()
1477 .map(|m| MatchPoints {
1478 ax: scaled_features[m.a as usize].x as f64,
1479 ay: scaled_features[m.a as usize].y as f64,
1480 bx: current_features[m.b as usize].x as f64,
1481 by: current_features[m.b as usize].y as f64,
1482 unique: m.unique,
1483 })
1484 .collect();
1485 let scaled_gray = Gray {
1486 width: sw,
1487 height: sh,
1488 data: &sdata,
1489 };
1490 for m in translation_hypotheses(&match_points, 4) {
1491 if m.support < 8 {
1492 continue;
1493 }
1494 let audit = audit_translation(scaled_gray, current, m.x, m.y, roi, true);
1495 if audit.samples < 200
1496 || !audit.error.is_finite()
1497 || audit.agreement < 0.5
1498 || audit.agreeing_error > 10.0
1499 {
1500 continue;
1501 }
1502 if best.is_none_or(|(_, e)| audit.agreeing_error < e) {
1503 best = Some((scale, audit.agreeing_error));
1504 }
1505 }
1506 }
1507 best
1508}
1509
1510pub fn resample_gray(g: Gray<'_>, scale: f64) -> (usize, usize, Vec<u8>) {
1512 let width = 2usize.max(js_round(g.width as f64 * scale) as usize);
1513 let height = 2usize.max(js_round(g.height as f64 * scale) as usize);
1514 let mut data = vec![0u8; width * height];
1515 for y in 0..height {
1516 for x in 0..width {
1517 let sx = (g.width as f64 - 1.001).min(x as f64 / scale).max(0.0);
1518 let sy = (g.height as f64 - 1.001).min(y as f64 / scale).max(0.0);
1519 let (x0, y0) = (sx.floor(), sy.floor());
1520 let (fx, fy) = (sx - x0, sy - y0);
1521 let (x0, y0) = (x0 as usize, y0 as usize);
1522 let (x1, y1) = ((x0 + 1).min(g.width - 1), (y0 + 1).min(g.height - 1));
1525 let d = g.data;
1526 let at = |xx: usize, yy: usize| d[yy * g.width + xx] as f64;
1527 let value = (at(x0, y0) * (1.0 - fx) + at(x1, y0) * fx) * (1.0 - fy)
1528 + (at(x0, y1) * (1.0 - fx) + at(x1, y1) * fx) * fy;
1529 data[y * width + x] = value as u8;
1531 }
1532 }
1533 (width, height, data)
1534}
1535
1536#[cfg(test)]
1537mod tests {
1538 use super::*;
1539 use crate::kernels::features::{extract_features, match_features};
1540
1541 #[test]
1545 fn prepared_audit_reuses_images_but_not_query_state() {
1546 for (width, height) in [(128, 128), (131, 95)] {
1547 let a: Vec<_> = (0..width * height)
1548 .map(|i| if (i % width) % 16 == 8 { 45 } else { 20 })
1549 .collect();
1550 let b: Vec<_> = (0..width * height)
1551 .map(|i| if (i % width + 3) % 16 == 8 { 45 } else { 20 })
1552 .collect();
1553 let ga = Gray {
1554 width,
1555 height,
1556 data: &a,
1557 };
1558 let gb = Gray {
1559 width,
1560 height,
1561 data: &b,
1562 };
1563 for tolerant in [false, true] {
1564 let prepared = TranslationAudit::new(ga, gb, tolerant);
1565 for roi in [
1566 None,
1567 Some(Rect {
1568 x: 16.0,
1569 y: 16.0,
1570 width: 96.0,
1571 height: 64.0,
1572 }),
1573 Some(Rect {
1574 x: 0.0,
1575 y: 0.0,
1576 width: 18.0,
1577 height: 80.0,
1578 }),
1579 ] {
1580 for (dx, dy) in [(3.0, 0.0), (2.5, -0.5), (-4.0, 9.0), (200.0, 200.0)] {
1581 assert_eq!(
1582 prepared.measure(dx, dy, roi),
1583 audit_translation(ga, gb, dx, dy, roi, tolerant)
1584 );
1585 }
1586 }
1587 let correct = prepared.measure(3.0, 0.0, None);
1588 assert!(correct.samples >= 24);
1589 assert_eq!(correct.error, 0.0);
1590 let other_pair = TranslationAudit::new(gb, ga, tolerant);
1591 assert_eq!(
1595 other_pair.measure(-3.0, 0.0, None),
1596 audit_translation(gb, ga, -3.0, 0.0, None, tolerant)
1597 );
1598 assert_eq!(prepared.measure(3.0, 0.0, None), correct);
1599 }
1600 }
1601 }
1602
1603 #[test]
1606 fn tolerant_audit_keeps_faint_narrow_texture_samples() {
1607 let (w, h) = (128, 128);
1608 let pixels = |offset: usize| {
1609 (0..w * h)
1610 .map(|i| if (i % w + offset) % 16 == 8 { 45 } else { 20 })
1611 .collect::<Vec<u8>>()
1612 };
1613 let a = pixels(0);
1614 let b = pixels(3);
1615 let audit = audit_translation(
1616 Gray {
1617 width: w,
1618 height: h,
1619 data: &a,
1620 },
1621 Gray {
1622 width: w,
1623 height: h,
1624 data: &b,
1625 },
1626 3.0,
1627 0.0,
1628 None,
1629 true,
1630 );
1631 assert!(audit.samples >= 24);
1632 assert_eq!(audit.error, 0.0);
1633 assert_eq!(audit.agreement, 1.0);
1634 }
1635
1636 #[test]
1639 fn tolerant_audit_preserves_texture_at_the_pane_boundary() {
1640 let (w, h) = (128, 128);
1641 let mut pixels = vec![20; w * h];
1642 for y in 0..h {
1643 pixels[y * w + 111] = 80;
1644 }
1645 let gray = Gray {
1646 width: w,
1647 height: h,
1648 data: &pixels,
1649 };
1650 let roi = Some(Rect {
1651 x: 16.0,
1652 y: 16.0,
1653 width: 96.0,
1654 height: 96.0,
1655 });
1656 let raw = audit_translation(gray, gray, 0.0, 0.0, roi, false);
1657 let tolerant = audit_translation(gray, gray, 0.0, 0.0, roi, true);
1658 assert!(raw.samples >= 24);
1659 assert_eq!(tolerant.samples, raw.samples);
1660 assert_eq!(tolerant.error, 0.0);
1661 }
1662
1663 fn texture(width: usize, height: usize, seed: u32) -> Vec<u8> {
1664 let mut rng = Rng::new(seed | 1);
1665 let mut data = vec![0u8; width * height];
1666 for y in 0..height {
1667 for x in 0..width {
1668 let block = ((x >> 5) * 7 + (y >> 5) * 13) & 3;
1669 let base = match block {
1670 0 => 30,
1671 1 => 120,
1672 2 => 200,
1673 _ => ((x + y) & 255) as i32,
1674 };
1675 data[y * width + x] =
1676 (base + (rng.next_f64() * 24.0).floor() as i32 - 12).clamp(0, 255) as u8;
1677 }
1678 }
1679 data
1680 }
1681
1682 #[test]
1686 fn the_trimmed_error_drops_a_static_band_but_not_a_wrong_offset() {
1687 let (w, h, dy, band) = (240usize, 240usize, 7i64, 48usize);
1688 let mut rng = Rng::new(11);
1690 let cells: Vec<u8> = (0..(w / 2) * ((h + 64) / 2))
1691 .map(|_| (rng.next_f64() * 256.0) as u8)
1692 .collect();
1693 let world: Vec<u8> = (0..w * (h + 64))
1694 .map(|i| cells[(i / w / 2) * (w / 2) + (i % w) / 2])
1695 .collect();
1696 let frame = |scroll: usize| -> Vec<u8> {
1699 let mut out = vec![255u8; w * h * 4];
1700 for y in 0..h {
1701 for x in 0..w {
1702 let v = if y >= h - band {
1703 if (x / 7 + y / 7) % 2 == 0 {
1704 0
1705 } else {
1706 255
1707 }
1708 } else {
1709 world[(y + scroll) * w + x]
1710 };
1711 out[(y * w + x) * 4..(y * w + x) * 4 + 3].copy_from_slice(&[v, v, v]);
1712 }
1713 }
1714 out
1715 };
1716 let (a, b) = (frame(0), frame(dy as usize));
1717 let region = Rect {
1718 x: 0.0,
1719 y: 0.0,
1720 width: w as f64,
1721 height: h as f64,
1722 };
1723 let points = select_native_points(&b, w, h, region, None, None);
1724 let right = refine_native_at(
1725 &a,
1726 &b,
1727 w,
1728 h,
1729 Point {
1730 x: 0.0,
1731 y: dy as f64,
1732 },
1733 None,
1734 3,
1735 &points,
1736 );
1737 assert_eq!((right.x, right.y), (0, dy as i32));
1738 assert!(
1739 right.error >= 14.0,
1740 "the band must push the plain mean past the limit: {}",
1741 right.error
1742 );
1743 assert!(
1744 right.trimmed < 8.0,
1745 "trimmed {} at the true offset",
1746 right.trimmed
1747 );
1748 let slipped = refine_native_at(
1749 &a,
1750 &b,
1751 w,
1752 h,
1753 Point {
1754 x: 0.0,
1755 y: (dy + 10) as f64,
1756 },
1757 None,
1758 3,
1759 &points,
1760 );
1761 assert!(
1762 slipped.trimmed >= 8.0,
1763 "trimmed {} at a wrong offset",
1764 slipped.trimmed
1765 );
1766 }
1767
1768 fn crop(world: &[u8], ww: usize, x: usize, y: usize, w: usize, h: usize) -> Vec<u8> {
1769 let mut out = vec![0u8; w * h];
1770 for row in 0..h {
1771 out[row * w..(row + 1) * w]
1772 .copy_from_slice(&world[(y + row) * ww + x..(y + row) * ww + x + w]);
1773 }
1774 out
1775 }
1776
1777 #[test]
1778 fn whole_motion_pipeline_runs_on_textured_shifts() {
1779 for (w, h, dx, dy) in [
1780 (320usize, 240usize, 17i64, 23i64),
1781 (640, 360, -41, 8),
1782 (200, 320, 0, 0),
1783 (256, 256, 3, -37),
1784 ] {
1785 let world = texture(w + 200, h + 200, (w * 31 + h) as u32);
1786 let a = crop(&world, w + 200, 100, 100, w, h);
1787 let b = crop(
1788 &world,
1789 w + 200,
1790 (100 + dx) as usize,
1791 (100 + dy) as usize,
1792 w,
1793 h,
1794 );
1795 let fa = extract_features(&a, w, h, 480, None);
1796 let fb = extract_features(&b, w, h, 480, None);
1797 let matches: Vec<MatchPoints> = match_features(&fa, &fb, true)
1798 .iter()
1799 .map(|m| MatchPoints {
1800 ax: fa[m.a as usize].x as f64,
1801 ay: fa[m.a as usize].y as f64,
1802 bx: fb[m.b as usize].x as f64,
1803 by: fb[m.b as usize].y as f64,
1804 unique: m.unique,
1805 })
1806 .collect();
1807 let ga = Gray {
1808 width: w,
1809 height: h,
1810 data: &a,
1811 };
1812 let gb = Gray {
1813 width: w,
1814 height: h,
1815 data: &b,
1816 };
1817 let hyp = translation_hypotheses(&matches, 6);
1818 assert!(!hyp.is_empty() || matches.len() < 3);
1819 let _ = detect_scale(&matches);
1820 for roi in [
1821 None,
1822 Some(Rect {
1823 x: 10.5,
1824 y: 7.0,
1825 width: w as f64 / 2.0,
1826 height: h as f64 / 2.0,
1827 }),
1828 ] {
1829 for (tx, ty) in [
1830 (dx as f64, dy as f64),
1831 (dx as f64 + 1.0, dy as f64 - 2.0),
1832 (0.0, 0.0),
1833 ] {
1834 let _ = verify_translation(ga, gb, tx, ty, roi);
1835 let _ = audit_translation(ga, gb, tx, ty, roi, true);
1836 let _ = refine_translation(
1837 ga,
1838 gb,
1839 Point {
1840 x: tx + 0.4,
1841 y: ty - 0.4,
1842 },
1843 roi,
1844 2,
1845 );
1846 }
1847 }
1848 let field = estimate_motion(ga, gb, &matches, fb.len() as u32);
1849 assert_eq!(field.labels.len(), field.cols * field.rows);
1850 let rgba = |g: &[u8]| -> Vec<u8> {
1851 g.iter()
1852 .flat_map(|&v| [v, ((v as u32 * 3) & 255) as u8, 255 - v, 255])
1853 .collect()
1854 };
1855 let (ra, rb) = (rgba(&a), rgba(&b));
1856 let region = Rect {
1857 x: 4.5,
1858 y: 3.0,
1859 width: (w - 20) as f64,
1860 height: (h - 9) as f64,
1861 };
1862 for guess in [
1863 Point {
1864 x: dx as f64 + 0.3,
1865 y: dy as f64 - 0.6,
1866 },
1867 Point { x: 0.0, y: 40.0 },
1868 ] {
1869 let _ = refine_native(&ra, &rb, w, h, guess, region, None, 3);
1870 }
1871 let _ = resample_gray(ga, 2.0);
1872 let _ = resample_gray(ga, 0.5);
1873 let features: Vec<(f64, f64)> = fb.iter().map(|f| (f.x as f64, f.y as f64)).collect();
1874 let patches = extract_patches(&b, w, h, region, &features, 1.0, 24, 32);
1875 assert!(patches.len() <= 24);
1876 for p in &patches {
1877 assert_eq!(p.data.len(), 32 * 32);
1878 assert!(p.x >= 0.0 && p.y >= 0.0);
1879 }
1880 let _ = refine_patches(
1881 &patches
1882 .iter()
1883 .map(|p| Patch {
1884 x: p.x as i32,
1885 y: p.y as i32,
1886 size: p.size,
1887 data: &p.data,
1888 })
1889 .collect::<Vec<_>>(),
1890 Gray {
1891 width: w,
1892 height: h,
1893 data: &b,
1894 },
1895 region,
1896 Point {
1897 x: -(dx as f64),
1898 y: -(dy as f64),
1899 },
1900 3,
1901 );
1902 assert!(extract_patches(
1904 &b,
1905 w,
1906 h,
1907 Rect {
1908 x: 0.0,
1909 y: 0.0,
1910 width: 10.0,
1911 height: 10.0
1912 },
1913 &features,
1914 1.0,
1915 24,
1916 32
1917 )
1918 .is_empty());
1919 let probe = probe_scale(ga, gb, &fb, None, &[1.0, 1.1, 0.9]);
1920 if let Some((scale, error)) = probe {
1921 assert!(scale > 0.0 && error.is_finite());
1922 }
1923 }
1924 }
1925
1926 fn gray_to_rgba(g: &[u8]) -> Vec<u8> {
1927 g.iter().flat_map(|&v| [v, v, v, 255]).collect()
1928 }
1929
1930 #[test]
1937 fn native_refinement_recovers_a_sparse_shift_the_lattice_would_miss() {
1938 let (w, h) = (280usize, 280usize);
1939 let (dx, dy) = (4i64, -3i64);
1940 let (v0, v_lo, v_mid, v_hi) = (100u8, 20u8, 150u8, 220u8);
1941 let region = Rect {
1942 x: 6.0,
1943 y: 6.0,
1944 width: 270.0,
1945 height: 270.0,
1946 };
1947 let step = 3i64.max(((region.width * region.height / 1600.0).sqrt()).floor() as i64);
1951 assert!(
1952 step >= 6,
1953 "test assumes a wide enough cell, got step={step}"
1954 );
1955 let x_start = 2i64.max(region.x.ceil() as i64);
1956 let y_start = 2i64.max(region.y.ceil() as i64);
1957 let mut a = vec![v0; w * h];
1958 let mut b = vec![v0; w * h];
1959 let mut cy = y_start;
1960 while (cy as f64) < region.y + region.height {
1961 let mut cx = x_start;
1962 while (cx as f64) < region.x + region.width {
1963 let (px, py) = (cx + 3, cy + 3);
1966 b[(py * w as i64 + px - 1) as usize] = v_lo;
1967 b[(py * w as i64 + px) as usize] = v_mid;
1968 b[(py * w as i64 + px + 1) as usize] = v_hi;
1969 let (qx, qy) = (px + dx, py + dy);
1970 a[(qy * w as i64 + qx - 1) as usize] = v_lo;
1971 a[(qy * w as i64 + qx) as usize] = v_mid;
1972 a[(qy * w as i64 + qx + 1) as usize] = v_hi;
1973 cx += step;
1974 }
1975 cy += step;
1976 }
1977 let (ra, rb) = (gray_to_rgba(&a), gray_to_rgba(&b));
1978 let points = select_native_points(&rb, w, h, region, None, None);
1979 assert!(
1980 points.len() >= 12,
1981 "expected many per-cell points, got {}",
1982 points.len()
1983 );
1984 let r = refine_native_at(
1985 &ra,
1986 &rb,
1987 w,
1988 h,
1989 Point {
1990 x: dx as f64,
1991 y: dy as f64,
1992 },
1993 None,
1994 3,
1995 &points,
1996 );
1997 assert_eq!((r.x, r.y), (dx as i32, dy as i32));
1998 assert!(r.error < 1.0, "expected a near-zero error, got {}", r.error);
1999 }
2000
2001 #[test]
2007 fn native_refinement_recovers_a_sparse_shift_via_the_guide() {
2008 let (w, h) = (280usize, 280usize);
2009 let (dx, dy) = (4i64, -3i64);
2010 let (v0, v_lo, v_mid, v_hi) = (100u8, 20u8, 150u8, 220u8);
2011 let region = Rect {
2012 x: 6.0,
2013 y: 6.0,
2014 width: 270.0,
2015 height: 270.0,
2016 };
2017 let step = 3i64.max(((region.width * region.height / 1600.0).sqrt()).floor() as i64);
2018 assert!(
2019 step >= 6,
2020 "test assumes a wide enough cell, got step={step}"
2021 );
2022 let factor = 6usize;
2023 let x_start = 2i64.max(region.x.ceil() as i64);
2024 let y_start = 2i64.max(region.y.ceil() as i64);
2025 let mut a = vec![v0; w * h];
2026 let mut b = vec![v0; w * h];
2027 let mut cy = y_start;
2028 while (cy as f64) < region.y + region.height {
2029 let mut cx = x_start;
2030 while (cx as f64) < region.x + region.width {
2031 let (px, py) = (cx + 3, cy + 3);
2032 b[(py * w as i64 + px - 1) as usize] = v_lo;
2033 b[(py * w as i64 + px) as usize] = v_mid;
2034 b[(py * w as i64 + px + 1) as usize] = v_hi;
2035 let (qx, qy) = (px + dx, py + dy);
2036 a[(qy * w as i64 + qx - 1) as usize] = v_lo;
2037 a[(qy * w as i64 + qx) as usize] = v_mid;
2038 a[(qy * w as i64 + qx + 1) as usize] = v_hi;
2039 cx += step;
2040 }
2041 cy += step;
2042 }
2043 let (ra, rb) = (gray_to_rgba(&a), gray_to_rgba(&b));
2044 let (gw, gh) = crate::kernels::raster::downscaled_size(w, h, factor);
2045 let mut guide_data = vec![0u8; gw * gh];
2046 crate::kernels::raster::downscale_gray(&rb, w, h, factor, &mut guide_data);
2047 let guide = Gray {
2048 width: gw,
2049 height: gh,
2050 data: &guide_data,
2051 };
2052 let points = select_native_points(&rb, w, h, region, None, Some((guide, factor)));
2053 assert!(
2054 points.len() >= 12,
2055 "expected many per-cell points, got {}",
2056 points.len()
2057 );
2058 let r = refine_native_at(
2059 &ra,
2060 &rb,
2061 w,
2062 h,
2063 Point {
2064 x: dx as f64,
2065 y: dy as f64,
2066 },
2067 None,
2068 3,
2069 &points,
2070 );
2071 assert_eq!((r.x, r.y), (dx as i32, dy as i32));
2072 assert!(r.error < 1.0, "expected a near-zero error, got {}", r.error);
2073 }
2074
2075 #[test]
2082 fn native_refinement_guide_at_factor_one_matches_the_unguided_scan() {
2083 let (w, h) = (200usize, 200usize);
2084 let world = texture(w + 200, h + 200, (w * 13 + h) as u32);
2085 let b = crop(&world, w + 200, 100, 100, w, h);
2086 let rb = gray_to_rgba(&b);
2087 let region = Rect {
2088 x: 4.0,
2089 y: 4.0,
2090 width: (w - 20) as f64,
2091 height: (h - 20) as f64,
2092 };
2093 let unguided = select_native_points(&rb, w, h, region, None, None);
2094 let (gw, gh) = crate::kernels::raster::downscaled_size(w, h, 1);
2095 let mut guide_data = vec![0u8; gw * gh];
2096 crate::kernels::raster::downscale_gray(&rb, w, h, 1, &mut guide_data);
2097 let guide = Gray {
2098 width: gw,
2099 height: gh,
2100 data: &guide_data,
2101 };
2102 let guided = select_native_points(&rb, w, h, region, None, Some((guide, 1)));
2103 assert_eq!(unguided, guided);
2104 assert!(
2105 unguided.len() >= 12,
2106 "fixture should be dense enough to compare meaningfully"
2107 );
2108 }
2109
2110 #[test]
2117 fn native_refinement_recovers_a_shift_past_a_screen_fixed_line_on_a_lattice_row() {
2118 let (w, h) = (280usize, 280usize);
2119 let (dx, dy) = (5i64, -4i64);
2120 let (v0, v_lo, v_mid, v_hi, v_bar) = (100u8, 20u8, 150u8, 220u8, 250u8);
2121 let region = Rect {
2122 x: 6.0,
2123 y: 6.0,
2124 width: 270.0,
2125 height: 270.0,
2126 };
2127 let step = 3i64.max(((region.width * region.height / 1600.0).sqrt()).floor() as i64);
2130 assert!(
2131 step >= 6,
2132 "test assumes a wide enough cell, got step={step}"
2133 );
2134 let x_start = 2i64.max(region.x.ceil() as i64);
2135 let y_start = 2i64.max(region.y.ceil() as i64);
2136 let line_y = y_start + 12 * step;
2140 let mut a = vec![v0; w * h];
2141 let mut b = vec![v0; w * h];
2142 let mut cy = y_start;
2143 while cy < line_y {
2144 let mut cx = x_start;
2145 while (cx as f64) < region.x + region.width {
2146 let (px, py) = (cx + 3, cy + 3);
2150 b[(py * w as i64 + px - 1) as usize] = v_lo;
2151 b[(py * w as i64 + px) as usize] = v_mid;
2152 b[(py * w as i64 + px + 1) as usize] = v_hi;
2153 let (qx, qy) = (px + dx, py + dy);
2154 a[(qy * w as i64 + qx - 1) as usize] = v_lo;
2155 a[(qy * w as i64 + qx) as usize] = v_mid;
2156 a[(qy * w as i64 + qx + 1) as usize] = v_hi;
2157 cx += step;
2158 }
2159 cy += step;
2160 }
2161 for y in line_y as usize..h {
2163 for x in 0..w {
2164 let i = y * w + x;
2165 a[i] = v_bar;
2166 b[i] = v_bar;
2167 }
2168 }
2169 let (ra, rb) = (gray_to_rgba(&a), gray_to_rgba(&b));
2170 let points = select_native_points(&rb, w, h, region, None, None);
2171 let r = refine_native_at(
2172 &ra,
2173 &rb,
2174 w,
2175 h,
2176 Point {
2177 x: dx as f64,
2178 y: dy as f64,
2179 },
2180 None,
2181 3,
2182 &points,
2183 );
2184 assert_eq!((r.x, r.y), (dx as i32, dy as i32));
2185 assert!(
2186 r.error < 14.0,
2187 "expected the true shift to clear the odometry gate, got {}",
2188 r.error
2189 );
2190 }
2191}