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 let (texture_a, texture_b) = (a, b);
236 let filtered = tolerant.then(|| {
237 let mut aa = vec![0; a.data.len()];
238 let mut bb = vec![0; b.data.len()];
239 crate::kernels::features::smooth(a.data, a.width, a.height, &mut aa);
240 crate::kernels::features::smooth(b.data, b.width, b.height, &mut bb);
241 (aa, bb)
242 });
243 let (a, b) = match &filtered {
244 Some((aa, bb)) => (Gray { data: aa, ..a }, Gray { data: bb, ..b }),
245 None => (a, b),
246 };
247 let r = roi.unwrap_or(Rect {
248 x: 0.0,
249 y: 0.0,
250 width: b.width as f64,
251 height: b.height as f64,
252 });
253 let (dx, dy) = (js_round(dx) as f64, js_round(dy) as f64);
254 let x0 = 2f64.max(r.x.ceil()).max((r.x - dx).ceil()).max(1.0 - dx);
257 let y0 = 2f64.max(r.y.ceil()).max((r.y - dy).ceil()).max(1.0 - dy);
258 let x1 = (b.width as f64 - 2.0)
259 .min(a.width as f64 - dx - 2.0)
260 .min(r.x + r.width)
261 .min(r.x + r.width - dx);
262 let y1 = (b.height as f64 - 2.0)
263 .min(a.height as f64 - dy - 2.0)
264 .min(r.y + r.height)
265 .min(r.y + r.height - dy);
266 let overlap = (x1 - x0).max(0.0) * (y1 - y0).max(0.0) / (r.width * r.height).max(1.0);
267 const B: f64 = 32.0;
268 let bw = 1usize.max(js_ceil((x1 - x0) / B).max(0) as usize);
269 let bh = 1usize.max(js_ceil((y1 - y0) / B).max(0) as usize);
270 let mut block_error = vec![0f64; bw * bh];
271 let mut block_bad = vec![0f64; bw * bh];
272 let mut block_n = vec![0f64; bw * bh];
273 let (mut error, mut bad, mut n) = (0f64, 0u32, 0u32);
274 let (bwid, awid) = (b.width as i64, a.width as i64);
275 let (dxi, dyi) = (dx as i64, dy as i64);
276 let mut y = y0;
277 while y < y1 {
278 let mut x = x0;
279 while x < x1 {
280 let i = (y as i64 * bwid + x as i64) as usize;
281 let j = ((y as i64 + dyi) * awid + x as i64 + dxi) as usize;
282 let (ad, bd) = (texture_a.data, texture_b.data);
285 let gb = (bd[i + 1] as i32 - bd[i - 1] as i32).abs()
286 + (bd[i + b.width] as i32 - bd[i - b.width] as i32).abs();
287 let ga = (ad[j + 1] as i32 - ad[j - 1] as i32).abs()
288 + (ad[j + a.width] as i32 - ad[j - a.width] as i32).abs();
289 if ga.max(gb) < 20 {
290 x += 2.0;
291 continue;
292 }
293 let filtered_here = tolerant
295 && x >= r.x + 2.0
296 && x + dx >= r.x + 2.0
297 && y >= r.y + 2.0
298 && y + dy >= r.y + 2.0
299 && x < r.x + r.width - 2.0
300 && x + dx < r.x + r.width - 2.0
301 && y < r.y + r.height - 2.0
302 && y + dy < r.y + r.height - 2.0;
303 let (ad, bd) = if filtered_here {
304 (a.data, b.data)
305 } else {
306 (texture_a.data, texture_b.data)
307 };
308 let mut difference = (ad[j] as i32 - bd[i] as i32).abs();
309 if tolerant {
310 difference = difference
311 .min((ad[j + 1] as i32 - bd[i] as i32).abs())
312 .min((ad[j - 1] as i32 - bd[i] as i32).abs())
313 .min((ad[j + a.width] as i32 - bd[i] as i32).abs())
314 .min((ad[j - a.width] as i32 - bd[i] as i32).abs());
315 }
316 let clipped = difference.min(70) as f64;
317 let q = (((y - y0) / B).floor() as usize) * bw + ((x - x0) / B).floor() as usize;
318 error += clipped;
319 block_error[q] += clipped;
320 block_n[q] += 1.0;
321 if difference > 18 {
322 bad += 1;
323 block_bad[q] += 1.0;
324 }
325 n += 1;
326 x += 2.0;
327 }
328 y += 2.0;
329 }
330 let (mut blocks, mut agreeing, mut agreeing_error) = (0u32, 0u32, 0f64);
331 for q in 0..block_n.len() {
332 if block_n[q] < 12.0 {
333 continue;
334 }
335 blocks += 1;
336 if block_error[q] / block_n[q] < 10.0 && block_bad[q] / block_n[q] < 0.2 {
337 agreeing += 1;
338 agreeing_error += block_error[q] / block_n[q];
339 }
340 }
341 Audit {
342 error: if n >= 24 {
343 error / n as f64
344 } else {
345 f64::INFINITY
346 },
347 mismatch: if n > 0 { bad as f64 / n as f64 } else { 1.0 },
348 overlap,
349 samples: n,
350 blocks,
351 agreeing,
352 agreement: if blocks > 0 {
353 agreeing as f64 / blocks as f64
354 } else {
355 0.0
356 },
357 agreeing_error: if agreeing > 0 {
358 agreeing_error / agreeing as f64
359 } else {
360 f64::INFINITY
361 },
362 }
363}
364
365pub fn refine_translation(
367 a: Gray<'_>,
368 b: Gray<'_>,
369 p: Point,
370 roi: Option<Rect>,
371 radius: i32,
372) -> (i32, i32) {
373 let r = roi.unwrap_or(Rect {
374 x: 0.0,
375 y: 0.0,
376 width: b.width as f64,
377 height: b.height as f64,
378 });
379 let (px, py) = (js_round(p.x), js_round(p.y));
380 let (mut best, mut error) = ((px, py), f64::INFINITY);
381 for dy in py - radius..=py + radius {
382 for dx in px - radius..=px + radius {
383 let e = patch_error(a, b, dx as f64, dy as f64, r, 7).error;
384 let closer = hypot(dx as f64 - p.x, dy as f64 - p.y)
385 < hypot(best.0 as f64 - p.x, best.1 as f64 - p.y);
386 if e < error - 1e-4 || ((e - error).abs() < 1e-4 && closer) {
387 error = e;
388 best = (dx, dy);
389 }
390 }
391 }
392 best
393}
394
395pub fn detect_scale(matches: &[MatchPoints]) -> f64 {
397 let m: Vec<&MatchPoints> = matches.iter().filter(|m| m.unique).collect();
398 if m.len() < 8 {
399 return 1.0;
400 }
401 let mut rng = Rng::new(7641);
402 let (mut best, mut best_scale) = (0usize, 1.0);
403 for _ in 0..96 {
404 let p = m[(rng.next_f64() * m.len() as f64).floor() as usize];
405 let q = m[(rng.next_f64() * m.len() as f64).floor() as usize];
406 let (bx, by, ax, ay) = (q.bx - p.bx, q.by - p.by, q.ax - p.ax, q.ay - p.ay);
407 let den = bx * bx + by * by;
408 if den < 1000.0 {
409 continue;
410 }
411 let u = (ax * bx + ay * by) / den;
412 let v = (ay * bx - ax * by) / den;
413 let s = hypot(u, v);
414 if !(0.55..=1.8).contains(&s) {
415 continue;
416 }
417 let tx = p.ax - u * p.bx + v * p.by;
418 let ty = p.ay - v * p.bx - u * p.by;
419 let n = m
420 .iter()
421 .filter(|r| {
422 hypot(
423 u * r.bx - v * r.by + tx - r.ax,
424 v * r.bx + u * r.by + ty - r.ay,
425 ) < 2.5
426 })
427 .count();
428 if n > best {
429 best = n;
430 best_scale = s;
431 }
432 }
433 if best as f64 >= (8.0f64).max(m.len() as f64 * 0.5) {
434 best_scale
435 } else {
436 1.0
437 }
438}
439
440#[derive(Clone, Debug, PartialEq)]
441pub struct MotionField {
442 pub motions: Vec<Motion>,
443 pub labels: Vec<u8>,
444 pub confidence: Vec<u8>,
445 pub dynamic: Vec<u8>,
446 pub cols: usize,
447 pub rows: usize,
448 pub cell: usize,
449 pub difference: f64,
450 pub feature_count: u32,
451 pub unknown: bool,
452 pub zoom: f64,
453}
454
455pub fn estimate_motion(
457 a: Gray<'_>,
458 b: Gray<'_>,
459 matches: &[MatchPoints],
460 feature_count: u32,
461) -> MotionField {
462 let cols = b.width.div_ceil(MOTION_CELL);
463 let rows = b.height.div_ceil(MOTION_CELL);
464 let n = cols * rows;
465 let difference = crate::kernels::raster::mean_difference(a.data, b.data);
466 if difference < 0.12 {
467 return MotionField {
468 motions: vec![Motion {
469 x: 0.0,
470 y: 0.0,
471 support: 0,
472 unique: 0,
473 confidence: 0.98,
474 error: difference,
475 ambiguous: false,
476 }],
477 labels: vec![0; n],
478 confidence: vec![245; n],
479 dynamic: vec![0; n],
480 cols,
481 rows,
482 cell: MOTION_CELL,
483 difference,
484 feature_count: 0,
485 unknown: false,
486 zoom: 1.0,
487 };
488 }
489 let mut motions = translation_hypotheses(matches, 6);
490 if !motions.iter().any(|m| hypot(m.x, m.y) < 1.0) {
491 motions.push(Motion {
492 x: 0.0,
493 y: 0.0,
494 support: 0,
495 unique: 0,
496 confidence: 0.5,
497 error: 0.0,
498 ambiguous: false,
499 });
500 }
501 if motions.is_empty() {
502 motions.push(Motion {
503 x: 0.0,
504 y: 0.0,
505 support: 0,
506 unique: 0,
507 confidence: 0.0,
508 error: 255.0,
509 ambiguous: true,
510 });
511 }
512 for m in motions.iter_mut() {
513 let inliers: Vec<&MatchPoints> = matches
514 .iter()
515 .filter(|p| hypot(p.ax - p.bx - m.x, p.ay - p.by - m.y) < 3.0)
516 .take(45)
517 .collect();
518 let (mut best, mut bx, mut by) = (f64::INFINITY, m.x, m.y);
519 let (mx, my) = (js_round(m.x), js_round(m.y));
520 for y in my - 1..=my + 1 {
521 for x in mx - 1..=mx + 1 {
522 let (mut sum, mut k) = (0f64, 0u32);
523 for p in &inliers {
524 let e = patch_error(
525 a,
526 b,
527 x as f64,
528 y as f64,
529 Rect {
530 x: p.bx - 7.0,
531 y: p.by - 7.0,
532 width: 15.0,
533 height: 15.0,
534 },
535 3,
536 )
537 .error;
538 if e.is_finite() {
539 sum += e;
540 k += 1;
541 }
542 }
543 if k > 0 && sum / (k as f64) < best {
544 best = sum / k as f64;
545 bx = x as f64;
546 by = y as f64;
547 }
548 }
549 }
550 m.x = bx;
551 m.y = by;
552 m.error = if best.is_finite() {
553 best
554 } else {
555 verify_translation(a, b, m.x, m.y, None)
556 };
557 if m.support > 0 {
558 m.confidence *= libm::exp(-(m.error.min(80.0)) / 35.0);
559 }
560 }
561 motions.sort_by(|a, b| (b.support + b.unique).cmp(&(a.support + a.unique)));
562 let dominant = motions
563 .iter()
564 .position(|m| hypot(m.x, m.y) > 1.0 && m.support >= 4)
565 .unwrap_or(0);
566 let mut labels = vec![0u8; n];
567 let mut confidence = vec![0u8; n];
568 let mut dynamic = vec![0u8; n];
569 let mut informative = vec![0u8; n];
570 for cy in 0..rows {
571 for cx in 0..cols {
572 let idx = cy * cols + cx;
573 let r = Rect {
574 x: (cx * MOTION_CELL) as f64,
575 y: (cy * MOTION_CELL) as f64,
576 width: MOTION_CELL.min(b.width - cx * MOTION_CELL) as f64,
577 height: MOTION_CELL.min(b.height - cy * MOTION_CELL) as f64,
578 };
579 let (mut best, mut second, mut choice, mut texture, mut dominant_error) =
580 (f64::INFINITY, f64::INFINITY, dominant, 0f64, f64::INFINITY);
581 for (k, m) in motions.iter().enumerate() {
582 let p = patch_error(a, b, m.x, m.y, r, 3);
583 texture = texture.max(p.texture);
584 if k == dominant {
585 dominant_error = p.error;
586 }
587 if p.error < best {
588 second = best;
589 best = p.error;
590 choice = k;
591 } else if p.error < second {
592 second = p.error;
593 }
594 }
595 if texture < 3.0
596 || !best.is_finite()
597 || second - best < 0.6
598 || (!dominant_error.is_finite() && choice != dominant)
599 {
600 labels[idx] = dominant as u8;
601 confidence[idx] = 65;
602 } else {
603 labels[idx] = choice as u8;
604 confidence[idx] = js_round(
605 255.0 * libm::exp(-best / 24.0) * ((second - best) / 8.0).clamp(0.25, 1.0),
606 ) as u8;
607 informative[idx] = (confidence[idx] > 100) as u8;
608 if best > 22.0 {
609 dynamic[idx] = 1;
610 labels[idx] = dominant as u8;
611 confidence[idx] = 45;
612 }
613 }
614 }
615 }
616 for _ in 0..3 {
617 let mut next = labels.clone();
618 for y in 0..rows as i64 {
619 for x in 0..cols as i64 {
620 let i = (y * cols as i64 + x) as usize;
621 if informative[i] != 0 {
622 continue;
623 }
624 let mut votes = vec![0f64; motions.len()];
625 for dy in -2i64..=2 {
626 for dx in -2i64..=2 {
627 let (xx, yy) = (x + dx, y + dy);
628 if xx < 0 || yy < 0 || xx >= cols as i64 || yy >= rows as i64 {
629 continue;
630 }
631 let j = (yy * cols as i64 + xx) as usize;
632 votes[labels[j] as usize] +=
633 confidence[j] as f64 / (1 + dx * dx + dy * dy) as f64;
634 }
635 }
636 let mut k = dominant;
637 for (j, v) in votes.iter().enumerate() {
638 if *v > votes[k] {
639 k = j;
640 }
641 }
642 next[i] = k as u8;
643 }
644 }
645 labels = next;
646 }
647 let strongest =
648 motions.iter().fold(
649 &motions[0],
650 |m, n| if n.confidence > m.confidence { n } else { m },
651 );
652 let unknown = strongest.support < 4 && difference > 5.0;
653 MotionField {
654 motions,
655 labels,
656 confidence,
657 dynamic,
658 cols,
659 rows,
660 cell: MOTION_CELL,
661 difference,
662 feature_count,
663 unknown,
664 zoom: detect_scale(matches),
665 }
666}
667
668pub struct NativeRefinement {
669 pub x: i32,
670 pub y: i32,
671 pub error: f64,
672 pub samples: u32,
673 pub runner_up: f64,
674 pub trimmed: f64,
678}
679
680pub const TRIMMED_KEEP: f64 = 0.75;
684
685#[allow(clippy::too_many_arguments)]
688fn trimmed_error(
689 a: &[u8],
690 b: &[u8],
691 w: i64,
692 h: i64,
693 dx: i64,
694 dy: i64,
695 inside: impl Fn(i64, i64) -> bool,
696 points: &[i64],
697) -> f64 {
698 let mut errors: Vec<f64> = Vec::with_capacity(points.len());
699 for &p in points {
700 let (x, y) = (p % w, p / w);
701 let (xx, yy) = (x + dx, y + dy);
702 if xx < 1 || yy < 1 || xx >= w - 1 || yy >= h - 1 || !inside(xx, yy) {
703 continue;
704 }
705 let i = (p * 4) as usize;
706 let j = ((yy * w + xx) * 4) as usize;
707 let d = (a[j] as i32 - b[i] as i32).abs()
708 + (a[j + 1] as i32 - b[i + 1] as i32).abs()
709 + (a[j + 2] as i32 - b[i + 2] as i32).abs();
710 errors.push((90.0f64).min(d as f64 / 3.0));
711 }
712 if errors.len() < 12 {
713 return f64::INFINITY;
714 }
715 errors.sort_by(|x, y| x.total_cmp(y));
716 let keep = ((errors.len() as f64) * TRIMMED_KEEP).ceil() as usize;
717 errors[..keep].iter().sum::<f64>() / keep as f64
718}
719
720pub fn select_native_points(
743 b: &[u8],
744 width: usize,
745 height: usize,
746 region: Rect,
747 mask: Option<(&[u8], u8)>,
748 guide: Option<(Gray<'_>, usize)>,
749) -> Vec<i64> {
750 let (w, h) = (width as i64, height as i64);
751 let step = 3i64.max(((region.width * region.height / 1600.0).sqrt()).floor() as i64);
752 let inside =
753 |x: i64, y: i64| mask.is_none_or(|(labels, code)| labels[(y * w + x) as usize] == code);
754 let y_limit = ((h - 2) as f64).min(region.y + region.height);
756 let x_limit = ((w - 2) as f64).min(region.x + region.width);
757 let x_start = 2i64.max(region.x.ceil() as i64);
758 let y_start = 2i64.max(region.y.ceil() as i64);
759 if (x_start as f64) >= x_limit || (y_start as f64) >= y_limit {
760 return Vec::new();
761 }
762 if let Some((g, f)) = guide {
763 return select_native_points_guided(
764 b,
765 w,
766 h,
767 x_start,
768 y_start,
769 x_limit,
770 y_limit,
771 step,
772 &inside,
773 g,
774 f.max(1) as i64,
775 );
776 }
777 let (bx0, by0) = ((x_start - 1).max(0), (y_start - 1).max(0));
783 let (bx1, by1) = (
784 (js_ceil(x_limit) as i64 + 1).min(w),
785 (js_ceil(y_limit) as i64 + 1).min(h),
786 );
787 let (bw, bh) = ((bx1 - bx0).max(0) as usize, (by1 - by0).max(0) as usize);
788 let mut luma_plane = vec![0u8; bw * bh];
790 for row in 0..bh {
791 let src = (((by0 + row as i64) * w + bx0) * 4) as usize;
792 let dst = row * bw;
793 for col in 0..bw {
794 let i = src + col * 4;
795 luma_plane[dst + col] =
796 ((b[i] as u32 * 77 + b[i + 1] as u32 * 150 + b[i + 2] as u32 * 29) >> 8) as u8;
797 }
798 }
799 let mut points: Vec<i64> = Vec::new();
800 let mut cy = y_start;
801 while (cy as f64) < y_limit {
802 let mut cx = x_start;
803 while (cx as f64) < x_limit {
804 let mut best: Option<(i32, i64)> = None;
806 let mut dy = 0i64;
807 while dy < step {
808 let y = cy + dy;
809 if (y as f64) >= y_limit {
810 break;
811 }
812 let (row_mid, row_up, row_down) = (
814 (y - by0) as usize * bw,
815 (y - 1 - by0) as usize * bw,
816 (y + 1 - by0) as usize * bw,
817 );
818 let mut dx = 0i64;
819 while dx < step {
820 let x = cx + dx;
821 if (x as f64) >= x_limit {
822 break;
823 }
824 if inside(x, y) {
825 let col = (x - bx0) as usize;
826 let grad = (luma_plane[row_mid + col - 1] as i32
827 - luma_plane[row_mid + col + 1] as i32)
828 .abs()
829 + (luma_plane[row_up + col] as i32 - luma_plane[row_down + col] as i32)
830 .abs();
831 if best.is_none_or(|(bg, _)| grad > bg) {
832 best = Some((grad, y * w + x));
833 }
834 }
835 dx += 1;
836 }
837 dy += 1;
838 }
839 if let Some((grad, p)) = best {
840 if grad > 12 {
841 points.push(p);
842 }
843 }
844 cx += step;
845 }
846 cy += step;
847 }
848 points
849}
850
851#[allow(clippy::too_many_arguments)]
856fn select_native_points_guided(
857 b: &[u8],
858 w: i64,
859 h: i64,
860 x_start: i64,
861 y_start: i64,
862 x_limit: f64,
863 y_limit: f64,
864 step: i64,
865 inside: &impl Fn(i64, i64) -> bool,
866 g: Gray<'_>,
867 f: i64,
868) -> Vec<i64> {
869 let (gw, gh) = (g.width as i64, g.height as i64);
870 let native_luma = |x: i64, y: i64| -> i32 {
873 let i = ((y * w + x) * 4) as usize;
874 ((b[i] as u32 * 77 + b[i + 1] as u32 * 150 + b[i + 2] as u32 * 29) >> 8) as i32
875 };
876 let (window_x_hi, window_y_hi) = (
882 (js_ceil(x_limit) as i64 - 1).min(w - 1),
883 (js_ceil(y_limit) as i64 - 1).min(h - 1),
884 );
885 let mut points: Vec<i64> = Vec::new();
886 let mut cy = y_start;
887 while (cy as f64) < y_limit {
888 let mut cx = x_start;
889 while (cx as f64) < x_limit {
890 let (cell_x_hi, cell_y_hi) = (
891 (cx + step - 1).min(window_x_hi),
892 (cy + step - 1).min(window_y_hi),
893 );
894 let mut chosen: Option<(i64, i64)> = None;
897 let mut best_analysis_grad = i32::MIN;
898 let mut ay = (cy / f).max(1);
899 while ay <= (cell_y_hi / f).min(gh - 2) {
900 let mut ax = (cx / f).max(1);
901 while ax <= (cell_x_hi / f).min(gw - 2) {
902 let (nx0, ny0) = (ax * f, ay * f);
903 let (nx1, ny1) = ((nx0 + f).min(w), (ny0 + f).min(h));
904 let (cx_mid, cy_mid) = (nx0 + (nx1 - nx0) / 2, ny0 + (ny1 - ny0) / 2);
905 if inside(cx_mid, cy_mid) {
906 let gi = (ay * gw + ax) as usize;
907 let grad = (g.data[gi - 1] as i32 - g.data[gi + 1] as i32).abs()
908 + (g.data[gi - gw as usize] as i32 - g.data[gi + gw as usize] as i32)
909 .abs();
910 if grad > best_analysis_grad {
911 best_analysis_grad = grad;
912 chosen = Some((ax, ay));
913 }
914 }
915 ax += 1;
916 }
917 ay += 1;
918 }
919 if let Some((ax, ay)) = chosen {
922 let (nx0, ny0) = (ax * f, ay * f);
923 let (nx1, ny1) = ((nx0 + f).min(w), (ny0 + f).min(h));
924 let mut best: Option<(i32, i64)> = None;
925 let mut y = ny0.max(cy).max(y_start);
926 let y_hi = ny1.min(cy + step).min(h);
927 while y < y_hi && (y as f64) < y_limit {
928 let mut x = nx0.max(cx).max(x_start);
929 let x_hi = nx1.min(cx + step).min(w);
930 while x < x_hi && (x as f64) < x_limit {
931 if inside(x, y) {
932 let grad = (native_luma(x - 1, y) - native_luma(x + 1, y)).abs()
933 + (native_luma(x, y - 1) - native_luma(x, y + 1)).abs();
934 if best.is_none_or(|(bg, _)| grad > bg) {
935 best = Some((grad, y * w + x));
936 }
937 }
938 x += 1;
939 }
940 y += 1;
941 }
942 if let Some((grad, p)) = best {
943 if grad > 12 {
944 points.push(p);
945 }
946 }
947 }
948 cx += step;
949 }
950 cy += step;
951 }
952 points
953}
954
955#[allow(clippy::too_many_arguments)]
959pub fn refine_native_at(
960 a: &[u8],
961 b: &[u8],
962 width: usize,
963 height: usize,
964 guess: Point,
965 mask: Option<(&[u8], u8)>,
966 radius: i32,
967 points: &[i64],
968) -> NativeRefinement {
969 let (w, h) = (width as i64, height as i64);
970 let (gx, gy) = (js_round(guess.x), js_round(guess.y));
971 let inside =
972 |x: i64, y: i64| mask.is_none_or(|(labels, code)| labels[(y * w + x) as usize] == code);
973 if points.len() < 12 {
974 return NativeRefinement {
975 x: gx,
976 y: gy,
977 error: f64::INFINITY,
978 samples: points.len() as u32,
979 runner_up: f64::INFINITY,
980 trimmed: f64::INFINITY,
981 };
982 }
983 let mut samples = 0u32;
984 let mut cost = |dx: i64, dy: i64| -> f64 {
985 let (mut error, mut n) = (0f64, 0u32);
986 for &p in points {
987 let (x, y) = (p % w, p / w);
988 let (xx, yy) = (x + dx, y + dy);
989 if xx < 1 || yy < 1 || xx >= w - 1 || yy >= h - 1 || !inside(xx, yy) {
990 continue;
991 }
992 let i = (p * 4) as usize;
993 let j = ((yy * w + xx) * 4) as usize;
994 let d = (a[j] as i32 - b[i] as i32).abs()
995 + (a[j + 1] as i32 - b[i + 1] as i32).abs()
996 + (a[j + 2] as i32 - b[i + 2] as i32).abs();
997 error += (90.0f64).min(d as f64 / 3.0);
998 n += 1;
999 }
1000 samples = samples.max(n);
1001 if n >= 12 {
1002 error / n as f64
1003 } else {
1004 f64::INFINITY
1005 }
1006 };
1007 let (mut best, mut px, mut py) = (f64::INFINITY, gx, gy);
1008 let mut costs: Vec<(i32, i32, f64)> = Vec::new();
1009 for y in gy - radius..=gy + radius {
1010 for x in gx - radius..=gx + radius {
1011 let e = cost(x as i64, y as i64);
1012 costs.push((x, y, e));
1013 let closer = hypot(x as f64 - guess.x, y as f64 - guess.y)
1014 < hypot(px as f64 - guess.x, py as f64 - guess.y);
1015 if e < best - 1e-6 || ((e - best).abs() < 1e-6 && closer) {
1016 best = e;
1017 px = x;
1018 py = y;
1019 }
1020 }
1021 }
1022 let runner_up = costs
1023 .iter()
1024 .filter(|c| (c.0 - px).abs().max((c.1 - py).abs()) > 2)
1025 .map(|c| c.2)
1026 .fold(f64::INFINITY, f64::min);
1027 let trimmed = if best.is_finite() {
1028 trimmed_error(a, b, w, h, px as i64, py as i64, inside, points)
1029 } else {
1030 f64::INFINITY
1031 };
1032 NativeRefinement {
1033 x: px,
1034 y: py,
1035 error: best,
1036 samples,
1037 runner_up,
1038 trimmed,
1039 }
1040}
1041
1042#[allow(clippy::too_many_arguments)]
1050pub fn sampling_insensitive_error(
1051 a: &[u8],
1052 b: &[u8],
1053 width: usize,
1054 height: usize,
1055 offset: (i32, i32),
1056 mask: Option<(&[u8], u8)>,
1057 radius: i32,
1058 points: &[i64],
1059) -> (f64, f64) {
1060 let (w, h) = (width as i64, height as i64);
1061 let inside =
1062 |x: i64, y: i64| mask.is_none_or(|(labels, code)| labels[(y * w + x) as usize] == code);
1063 let stride = width * 4;
1064 let span = |image: &[u8], k: usize| {
1066 let v = image[k] as i32;
1067 [k - 4, k + 4, k - stride, k + stride]
1068 .iter()
1069 .fold((2 * v, 2 * v), |(low, high), &n| {
1070 (low.min(v + image[n] as i32), high.max(v + image[n] as i32))
1071 })
1072 };
1073 let outside = |v: i32, (low, high): (i32, i32)| (v - high).max(low - v).max(0);
1074 let cost = |dx: i64, dy: i64| -> f64 {
1075 let (mut error, mut n) = (0f64, 0u32);
1076 for &p in points {
1077 let (x, y) = (p % w, p / w);
1078 let (xx, yy) = (x + dx, y + dy);
1079 if xx < 1 || yy < 1 || xx >= w - 1 || yy >= h - 1 || !inside(xx, yy) {
1080 continue;
1081 }
1082 let i = (p * 4) as usize;
1083 let j = ((yy * w + xx) * 4) as usize;
1084 let d: i32 = (0..3)
1085 .map(|c| {
1086 outside(2 * b[i + c] as i32, span(a, j + c))
1087 .min(outside(2 * a[j + c] as i32, span(b, i + c)))
1088 })
1089 .sum();
1090 error += (90.0f64).min(d as f64 / 6.0);
1091 n += 1;
1092 }
1093 if n >= 12 {
1094 error / n as f64
1095 } else {
1096 f64::INFINITY
1097 }
1098 };
1099 let (ox, oy) = offset;
1100 let mut runner_up = f64::INFINITY;
1101 for y in oy - radius..=oy + radius {
1102 for x in ox - radius..=ox + radius {
1103 if (x - ox).abs().max((y - oy).abs()) > 2 {
1104 runner_up = runner_up.min(cost(x as i64, y as i64));
1105 }
1106 }
1107 }
1108 (cost(ox as i64, oy as i64), runner_up)
1109}
1110
1111#[allow(clippy::too_many_arguments)]
1116pub fn refine_native(
1117 a: &[u8],
1118 b: &[u8],
1119 width: usize,
1120 height: usize,
1121 guess: Point,
1122 region: Rect,
1123 mask: Option<(&[u8], u8)>,
1124 radius: i32,
1125) -> NativeRefinement {
1126 let points = select_native_points(b, width, height, region, mask, None);
1127 refine_native_at(a, b, width, height, guess, mask, radius, &points)
1128}
1129
1130pub struct Patch<'a> {
1132 pub x: i32,
1133 pub y: i32,
1134 pub size: usize,
1135 pub data: &'a [u8],
1136}
1137
1138pub fn refine_patches(
1140 patches: &[Patch<'_>],
1141 native: Gray<'_>,
1142 region: Rect,
1143 guess: Point,
1144 radius: i32,
1145) -> NativeRefinement {
1146 refine_weighted_patches(patches, native, region, guess, radius, &[])
1147}
1148
1149pub fn refine_spatial_patches(
1153 patches: &[Patch<'_>],
1154 native: Gray<'_>,
1155 region: Rect,
1156 guess: Point,
1157 radius: i32,
1158) -> NativeRefinement {
1159 let mut weights = vec![0.0; patches.len()];
1160 if let Some(first) = patches.first() {
1161 let step = first.size;
1162 for y in (step / 2..js_round(region.height) as usize).step_by(step) {
1163 for x in (step / 2..js_round(region.width) as usize).step_by(step) {
1164 let nearest = patches
1165 .iter()
1166 .enumerate()
1167 .min_by_key(|(_, p)| {
1168 let dx = x as i64 - p.x as i64 - p.size as i64 / 2;
1169 let dy = y as i64 - p.y as i64 - p.size as i64 / 2;
1170 dx * dx + dy * dy
1171 })
1172 .unwrap()
1173 .0;
1174 weights[nearest] += 1.0;
1175 }
1176 }
1177 }
1178 refine_weighted_patches(patches, native, region, guess, radius, &weights)
1179}
1180
1181fn refine_weighted_patches(
1182 patches: &[Patch<'_>],
1183 native: Gray<'_>,
1184 region: Rect,
1185 guess: Point,
1186 radius: i32,
1187 weights: &[f64],
1188) -> NativeRefinement {
1189 let (rx, ry, rw, rh) = (
1190 js_round(region.x),
1191 js_round(region.y),
1192 js_round(region.width),
1193 js_round(region.height),
1194 );
1195 let w = native.width as i64;
1196 let (gx, gy) = (js_round(guess.x), js_round(guess.y));
1197 let region_inside = rx >= 0
1198 && ry >= 0
1199 && rw >= 0
1200 && rh >= 0
1201 && (rx + rw) as usize <= native.width
1202 && (ry + rh) as usize <= native.height;
1203 if patches.is_empty() || !region_inside {
1204 return NativeRefinement {
1205 x: gx,
1206 y: gy,
1207 error: f64::INFINITY,
1208 samples: 0,
1209 runner_up: f64::INFINITY,
1210 trimmed: f64::INFINITY,
1211 };
1212 }
1213 let mut samples = 0u32;
1214 let mut cost = |dx: i32, dy: i32| -> f64 {
1215 let (mut error, mut n, mut weight_sum) = (0f64, 0u32, 0f64);
1216 for (i, p) in patches.iter().enumerate() {
1217 let weight = weights.get(i).copied().unwrap_or(1.0);
1218 let (cx, cy) = (p.x - dx, p.y - dy);
1219 if cx < 0 || cy < 0 || cx + p.size as i32 > rw || cy + p.size as i32 > rh {
1220 continue;
1221 }
1222 let mut py = 0usize;
1223 while py < p.size {
1224 let row_a = ((ry + cy) as i64 + py as i64) * w + (rx + cx) as i64;
1225 let row_p = py * p.size;
1226 let mut px = 0usize;
1227 while px < p.size {
1228 error += weight
1229 * (90.0f64).min(
1230 (native.data[(row_a + px as i64) as usize] as i32
1231 - p.data[row_p + px] as i32)
1232 .abs() as f64,
1233 );
1234 n += 1;
1235 weight_sum += weight;
1236 px += 2;
1237 }
1238 py += 2;
1239 }
1240 }
1241 samples = samples.max(n);
1242 if n >= 64 {
1243 error / weight_sum
1244 } else {
1245 f64::INFINITY
1246 }
1247 };
1248 let (mut best, mut bx, mut by) = (f64::INFINITY, gx, gy);
1249 let mut costs: Vec<(i32, i32, f64)> = Vec::new();
1250 for y in gy - radius..=gy + radius {
1251 for x in gx - radius..=gx + radius {
1252 let e = cost(x, y);
1253 costs.push((x, y, e));
1254 let closer = hypot(x as f64 - guess.x, y as f64 - guess.y)
1255 < hypot(bx as f64 - guess.x, by as f64 - guess.y);
1256 if e < best - 1e-6 || ((e - best).abs() < 1e-6 && closer) {
1257 best = e;
1258 bx = x;
1259 by = y;
1260 }
1261 }
1262 }
1263 let runner_up = costs
1264 .iter()
1265 .filter(|c| (c.0 - bx).abs().max((c.1 - by).abs()) > 2)
1266 .map(|c| c.2)
1267 .fold(f64::INFINITY, f64::min);
1268 NativeRefinement {
1269 x: bx,
1270 y: by,
1271 error: best,
1272 samples,
1273 runner_up,
1274 trimmed: f64::INFINITY,
1275 }
1276}
1277
1278pub struct ExtractedPatch {
1280 pub x: f64,
1281 pub y: f64,
1282 pub size: usize,
1283 pub data: Vec<u8>,
1284}
1285
1286#[allow(clippy::too_many_arguments)]
1292pub fn extract_patches(
1293 native: &[u8],
1294 native_width: usize,
1295 native_height: usize,
1296 region: Rect,
1297 features: &[(f64, f64)],
1298 factor: f64,
1299 count: usize,
1300 size: usize,
1301) -> Vec<ExtractedPatch> {
1302 let mut out: Vec<ExtractedPatch> = Vec::new();
1303 let (rx, ry, rw, rh) = (
1304 js_round(region.x),
1305 js_round(region.y),
1306 js_round(region.width),
1307 js_round(region.height),
1308 );
1309 if (rw as f64) < size as f64 + 2.0 || (rh as f64) < size as f64 + 2.0 {
1310 return out;
1311 }
1312 if rx < 0 || ry < 0 || (rx + rw) as usize > native_width || (ry + rh) as usize > native_height {
1313 return out;
1314 }
1315 let mut taken: Vec<(f64, f64)> = Vec::new();
1316 for &(fx, fy) in features {
1317 if out.len() >= count {
1318 break;
1319 }
1320 let x = (js_round(fx * factor) as f64 - size as f64 / 2.0)
1321 .clamp(0.0, (rw - size as i32) as f64);
1322 let y = (js_round(fy * factor) as f64 - size as f64 / 2.0)
1323 .clamp(0.0, (rh - size as i32) as f64);
1324 if taken
1325 .iter()
1326 .any(|&(tx, ty)| (tx - x).abs() < size as f64 && (ty - y).abs() < size as f64)
1327 {
1328 continue;
1329 }
1330 taken.push((x, y));
1331 let mut data = vec![0u8; size * size];
1332 for row in 0..size {
1333 let src = ((ry + y as i32 + row as i32) as i64 * native_width as i64
1334 + (rx + x as i32) as i64) as usize;
1335 data[row * size..row * size + size].copy_from_slice(&native[src..src + size]);
1336 }
1337 out.push(ExtractedPatch { x, y, size, data });
1338 }
1339 out
1340}
1341
1342pub fn probe_scale(
1347 previous: Gray<'_>,
1348 current: Gray<'_>,
1349 current_features: &[crate::kernels::features::Feature],
1350 roi: Option<Rect>,
1351 scales: &[f64],
1352) -> Option<(f64, f64)> {
1353 let mut best: Option<(f64, f64)> = None;
1354 for &scale in scales {
1355 let (sw, sh, sdata) = resample_gray(previous, scale);
1356 let scaled_features = crate::kernels::features::extract_features(&sdata, sw, sh, 320, None);
1357 let matches =
1358 crate::kernels::features::match_features(&scaled_features, current_features, true);
1359 let match_points: Vec<MatchPoints> = matches
1360 .iter()
1361 .map(|m| MatchPoints {
1362 ax: scaled_features[m.a as usize].x as f64,
1363 ay: scaled_features[m.a as usize].y as f64,
1364 bx: current_features[m.b as usize].x as f64,
1365 by: current_features[m.b as usize].y as f64,
1366 unique: m.unique,
1367 })
1368 .collect();
1369 let scaled_gray = Gray {
1370 width: sw,
1371 height: sh,
1372 data: &sdata,
1373 };
1374 for m in translation_hypotheses(&match_points, 4) {
1375 if m.support < 8 {
1376 continue;
1377 }
1378 let audit = audit_translation(scaled_gray, current, m.x, m.y, roi, true);
1379 if audit.samples < 200
1380 || !audit.error.is_finite()
1381 || audit.agreement < 0.5
1382 || audit.agreeing_error > 10.0
1383 {
1384 continue;
1385 }
1386 if best.is_none_or(|(_, e)| audit.agreeing_error < e) {
1387 best = Some((scale, audit.agreeing_error));
1388 }
1389 }
1390 }
1391 best
1392}
1393
1394pub fn resample_gray(g: Gray<'_>, scale: f64) -> (usize, usize, Vec<u8>) {
1396 let width = 2usize.max(js_round(g.width as f64 * scale) as usize);
1397 let height = 2usize.max(js_round(g.height as f64 * scale) as usize);
1398 let mut data = vec![0u8; width * height];
1399 for y in 0..height {
1400 for x in 0..width {
1401 let sx = (g.width as f64 - 1.001).min(x as f64 / scale).max(0.0);
1402 let sy = (g.height as f64 - 1.001).min(y as f64 / scale).max(0.0);
1403 let (x0, y0) = (sx.floor(), sy.floor());
1404 let (fx, fy) = (sx - x0, sy - y0);
1405 let (x0, y0) = (x0 as usize, y0 as usize);
1406 let (x1, y1) = ((x0 + 1).min(g.width - 1), (y0 + 1).min(g.height - 1));
1409 let d = g.data;
1410 let at = |xx: usize, yy: usize| d[yy * g.width + xx] as f64;
1411 let value = (at(x0, y0) * (1.0 - fx) + at(x1, y0) * fx) * (1.0 - fy)
1412 + (at(x0, y1) * (1.0 - fx) + at(x1, y1) * fx) * fy;
1413 data[y * width + x] = value as u8;
1415 }
1416 }
1417 (width, height, data)
1418}
1419
1420#[cfg(test)]
1421mod tests {
1422 use super::*;
1423 use crate::kernels::features::{extract_features, match_features};
1424
1425 #[test]
1428 fn tolerant_audit_keeps_faint_narrow_texture_samples() {
1429 let (w, h) = (128, 128);
1430 let pixels = |offset: usize| {
1431 (0..w * h)
1432 .map(|i| if (i % w + offset) % 16 == 8 { 45 } else { 20 })
1433 .collect::<Vec<u8>>()
1434 };
1435 let a = pixels(0);
1436 let b = pixels(3);
1437 let audit = audit_translation(
1438 Gray {
1439 width: w,
1440 height: h,
1441 data: &a,
1442 },
1443 Gray {
1444 width: w,
1445 height: h,
1446 data: &b,
1447 },
1448 3.0,
1449 0.0,
1450 None,
1451 true,
1452 );
1453 assert!(audit.samples >= 24);
1454 assert_eq!(audit.error, 0.0);
1455 assert_eq!(audit.agreement, 1.0);
1456 }
1457
1458 #[test]
1461 fn tolerant_audit_preserves_texture_at_the_pane_boundary() {
1462 let (w, h) = (128, 128);
1463 let mut pixels = vec![20; w * h];
1464 for y in 0..h {
1465 pixels[y * w + 111] = 80;
1466 }
1467 let gray = Gray {
1468 width: w,
1469 height: h,
1470 data: &pixels,
1471 };
1472 let roi = Some(Rect {
1473 x: 16.0,
1474 y: 16.0,
1475 width: 96.0,
1476 height: 96.0,
1477 });
1478 let raw = audit_translation(gray, gray, 0.0, 0.0, roi, false);
1479 let tolerant = audit_translation(gray, gray, 0.0, 0.0, roi, true);
1480 assert!(raw.samples >= 24);
1481 assert_eq!(tolerant.samples, raw.samples);
1482 assert_eq!(tolerant.error, 0.0);
1483 }
1484
1485 fn texture(width: usize, height: usize, seed: u32) -> Vec<u8> {
1486 let mut rng = Rng::new(seed | 1);
1487 let mut data = vec![0u8; width * height];
1488 for y in 0..height {
1489 for x in 0..width {
1490 let block = ((x >> 5) * 7 + (y >> 5) * 13) & 3;
1491 let base = match block {
1492 0 => 30,
1493 1 => 120,
1494 2 => 200,
1495 _ => ((x + y) & 255) as i32,
1496 };
1497 data[y * width + x] =
1498 (base + (rng.next_f64() * 24.0).floor() as i32 - 12).clamp(0, 255) as u8;
1499 }
1500 }
1501 data
1502 }
1503
1504 #[test]
1508 fn the_trimmed_error_drops_a_static_band_but_not_a_wrong_offset() {
1509 let (w, h, dy, band) = (240usize, 240usize, 7i64, 48usize);
1510 let mut rng = Rng::new(11);
1512 let cells: Vec<u8> = (0..(w / 2) * ((h + 64) / 2))
1513 .map(|_| (rng.next_f64() * 256.0) as u8)
1514 .collect();
1515 let world: Vec<u8> = (0..w * (h + 64))
1516 .map(|i| cells[(i / w / 2) * (w / 2) + (i % w) / 2])
1517 .collect();
1518 let frame = |scroll: usize| -> Vec<u8> {
1521 let mut out = vec![255u8; w * h * 4];
1522 for y in 0..h {
1523 for x in 0..w {
1524 let v = if y >= h - band {
1525 if (x / 7 + y / 7) % 2 == 0 {
1526 0
1527 } else {
1528 255
1529 }
1530 } else {
1531 world[(y + scroll) * w + x]
1532 };
1533 out[(y * w + x) * 4..(y * w + x) * 4 + 3].copy_from_slice(&[v, v, v]);
1534 }
1535 }
1536 out
1537 };
1538 let (a, b) = (frame(0), frame(dy as usize));
1539 let region = Rect {
1540 x: 0.0,
1541 y: 0.0,
1542 width: w as f64,
1543 height: h as f64,
1544 };
1545 let points = select_native_points(&b, w, h, region, None, None);
1546 let right = refine_native_at(
1547 &a,
1548 &b,
1549 w,
1550 h,
1551 Point {
1552 x: 0.0,
1553 y: dy as f64,
1554 },
1555 None,
1556 3,
1557 &points,
1558 );
1559 assert_eq!((right.x, right.y), (0, dy as i32));
1560 assert!(
1561 right.error >= 14.0,
1562 "the band must push the plain mean past the limit: {}",
1563 right.error
1564 );
1565 assert!(
1566 right.trimmed < 8.0,
1567 "trimmed {} at the true offset",
1568 right.trimmed
1569 );
1570 let slipped = refine_native_at(
1571 &a,
1572 &b,
1573 w,
1574 h,
1575 Point {
1576 x: 0.0,
1577 y: (dy + 10) as f64,
1578 },
1579 None,
1580 3,
1581 &points,
1582 );
1583 assert!(
1584 slipped.trimmed >= 8.0,
1585 "trimmed {} at a wrong offset",
1586 slipped.trimmed
1587 );
1588 }
1589
1590 fn crop(world: &[u8], ww: usize, x: usize, y: usize, w: usize, h: usize) -> Vec<u8> {
1591 let mut out = vec![0u8; w * h];
1592 for row in 0..h {
1593 out[row * w..(row + 1) * w]
1594 .copy_from_slice(&world[(y + row) * ww + x..(y + row) * ww + x + w]);
1595 }
1596 out
1597 }
1598
1599 #[test]
1600 fn whole_motion_pipeline_runs_on_textured_shifts() {
1601 for (w, h, dx, dy) in [
1602 (320usize, 240usize, 17i64, 23i64),
1603 (640, 360, -41, 8),
1604 (200, 320, 0, 0),
1605 (256, 256, 3, -37),
1606 ] {
1607 let world = texture(w + 200, h + 200, (w * 31 + h) as u32);
1608 let a = crop(&world, w + 200, 100, 100, w, h);
1609 let b = crop(
1610 &world,
1611 w + 200,
1612 (100 + dx) as usize,
1613 (100 + dy) as usize,
1614 w,
1615 h,
1616 );
1617 let fa = extract_features(&a, w, h, 480, None);
1618 let fb = extract_features(&b, w, h, 480, None);
1619 let matches: Vec<MatchPoints> = match_features(&fa, &fb, true)
1620 .iter()
1621 .map(|m| MatchPoints {
1622 ax: fa[m.a as usize].x as f64,
1623 ay: fa[m.a as usize].y as f64,
1624 bx: fb[m.b as usize].x as f64,
1625 by: fb[m.b as usize].y as f64,
1626 unique: m.unique,
1627 })
1628 .collect();
1629 let ga = Gray {
1630 width: w,
1631 height: h,
1632 data: &a,
1633 };
1634 let gb = Gray {
1635 width: w,
1636 height: h,
1637 data: &b,
1638 };
1639 let hyp = translation_hypotheses(&matches, 6);
1640 assert!(!hyp.is_empty() || matches.len() < 3);
1641 let _ = detect_scale(&matches);
1642 for roi in [
1643 None,
1644 Some(Rect {
1645 x: 10.5,
1646 y: 7.0,
1647 width: w as f64 / 2.0,
1648 height: h as f64 / 2.0,
1649 }),
1650 ] {
1651 for (tx, ty) in [
1652 (dx as f64, dy as f64),
1653 (dx as f64 + 1.0, dy as f64 - 2.0),
1654 (0.0, 0.0),
1655 ] {
1656 let _ = verify_translation(ga, gb, tx, ty, roi);
1657 let _ = audit_translation(ga, gb, tx, ty, roi, true);
1658 let _ = refine_translation(
1659 ga,
1660 gb,
1661 Point {
1662 x: tx + 0.4,
1663 y: ty - 0.4,
1664 },
1665 roi,
1666 2,
1667 );
1668 }
1669 }
1670 let field = estimate_motion(ga, gb, &matches, fb.len() as u32);
1671 assert_eq!(field.labels.len(), field.cols * field.rows);
1672 let rgba = |g: &[u8]| -> Vec<u8> {
1673 g.iter()
1674 .flat_map(|&v| [v, ((v as u32 * 3) & 255) as u8, 255 - v, 255])
1675 .collect()
1676 };
1677 let (ra, rb) = (rgba(&a), rgba(&b));
1678 let region = Rect {
1679 x: 4.5,
1680 y: 3.0,
1681 width: (w - 20) as f64,
1682 height: (h - 9) as f64,
1683 };
1684 for guess in [
1685 Point {
1686 x: dx as f64 + 0.3,
1687 y: dy as f64 - 0.6,
1688 },
1689 Point { x: 0.0, y: 40.0 },
1690 ] {
1691 let _ = refine_native(&ra, &rb, w, h, guess, region, None, 3);
1692 }
1693 let _ = resample_gray(ga, 2.0);
1694 let _ = resample_gray(ga, 0.5);
1695 let features: Vec<(f64, f64)> = fb.iter().map(|f| (f.x as f64, f.y as f64)).collect();
1696 let patches = extract_patches(&b, w, h, region, &features, 1.0, 24, 32);
1697 assert!(patches.len() <= 24);
1698 for p in &patches {
1699 assert_eq!(p.data.len(), 32 * 32);
1700 assert!(p.x >= 0.0 && p.y >= 0.0);
1701 }
1702 let _ = refine_patches(
1703 &patches
1704 .iter()
1705 .map(|p| Patch {
1706 x: p.x as i32,
1707 y: p.y as i32,
1708 size: p.size,
1709 data: &p.data,
1710 })
1711 .collect::<Vec<_>>(),
1712 Gray {
1713 width: w,
1714 height: h,
1715 data: &b,
1716 },
1717 region,
1718 Point {
1719 x: -(dx as f64),
1720 y: -(dy as f64),
1721 },
1722 3,
1723 );
1724 assert!(extract_patches(
1726 &b,
1727 w,
1728 h,
1729 Rect {
1730 x: 0.0,
1731 y: 0.0,
1732 width: 10.0,
1733 height: 10.0
1734 },
1735 &features,
1736 1.0,
1737 24,
1738 32
1739 )
1740 .is_empty());
1741 let probe = probe_scale(ga, gb, &fb, None, &[1.0, 1.1, 0.9]);
1742 if let Some((scale, error)) = probe {
1743 assert!(scale > 0.0 && error.is_finite());
1744 }
1745 }
1746 }
1747
1748 fn gray_to_rgba(g: &[u8]) -> Vec<u8> {
1749 g.iter().flat_map(|&v| [v, v, v, 255]).collect()
1750 }
1751
1752 #[test]
1759 fn native_refinement_recovers_a_sparse_shift_the_lattice_would_miss() {
1760 let (w, h) = (280usize, 280usize);
1761 let (dx, dy) = (4i64, -3i64);
1762 let (v0, v_lo, v_mid, v_hi) = (100u8, 20u8, 150u8, 220u8);
1763 let region = Rect {
1764 x: 6.0,
1765 y: 6.0,
1766 width: 270.0,
1767 height: 270.0,
1768 };
1769 let step = 3i64.max(((region.width * region.height / 1600.0).sqrt()).floor() as i64);
1773 assert!(
1774 step >= 6,
1775 "test assumes a wide enough cell, got step={step}"
1776 );
1777 let x_start = 2i64.max(region.x.ceil() as i64);
1778 let y_start = 2i64.max(region.y.ceil() as i64);
1779 let mut a = vec![v0; w * h];
1780 let mut b = vec![v0; w * h];
1781 let mut cy = y_start;
1782 while (cy as f64) < region.y + region.height {
1783 let mut cx = x_start;
1784 while (cx as f64) < region.x + region.width {
1785 let (px, py) = (cx + 3, cy + 3);
1788 b[(py * w as i64 + px - 1) as usize] = v_lo;
1789 b[(py * w as i64 + px) as usize] = v_mid;
1790 b[(py * w as i64 + px + 1) as usize] = v_hi;
1791 let (qx, qy) = (px + dx, py + dy);
1792 a[(qy * w as i64 + qx - 1) as usize] = v_lo;
1793 a[(qy * w as i64 + qx) as usize] = v_mid;
1794 a[(qy * w as i64 + qx + 1) as usize] = v_hi;
1795 cx += step;
1796 }
1797 cy += step;
1798 }
1799 let (ra, rb) = (gray_to_rgba(&a), gray_to_rgba(&b));
1800 let points = select_native_points(&rb, w, h, region, None, None);
1801 assert!(
1802 points.len() >= 12,
1803 "expected many per-cell points, got {}",
1804 points.len()
1805 );
1806 let r = refine_native_at(
1807 &ra,
1808 &rb,
1809 w,
1810 h,
1811 Point {
1812 x: dx as f64,
1813 y: dy as f64,
1814 },
1815 None,
1816 3,
1817 &points,
1818 );
1819 assert_eq!((r.x, r.y), (dx as i32, dy as i32));
1820 assert!(r.error < 1.0, "expected a near-zero error, got {}", r.error);
1821 }
1822
1823 #[test]
1829 fn native_refinement_recovers_a_sparse_shift_via_the_guide() {
1830 let (w, h) = (280usize, 280usize);
1831 let (dx, dy) = (4i64, -3i64);
1832 let (v0, v_lo, v_mid, v_hi) = (100u8, 20u8, 150u8, 220u8);
1833 let region = Rect {
1834 x: 6.0,
1835 y: 6.0,
1836 width: 270.0,
1837 height: 270.0,
1838 };
1839 let step = 3i64.max(((region.width * region.height / 1600.0).sqrt()).floor() as i64);
1840 assert!(
1841 step >= 6,
1842 "test assumes a wide enough cell, got step={step}"
1843 );
1844 let factor = 6usize;
1845 let x_start = 2i64.max(region.x.ceil() as i64);
1846 let y_start = 2i64.max(region.y.ceil() as i64);
1847 let mut a = vec![v0; w * h];
1848 let mut b = vec![v0; w * h];
1849 let mut cy = y_start;
1850 while (cy as f64) < region.y + region.height {
1851 let mut cx = x_start;
1852 while (cx as f64) < region.x + region.width {
1853 let (px, py) = (cx + 3, cy + 3);
1854 b[(py * w as i64 + px - 1) as usize] = v_lo;
1855 b[(py * w as i64 + px) as usize] = v_mid;
1856 b[(py * w as i64 + px + 1) as usize] = v_hi;
1857 let (qx, qy) = (px + dx, py + dy);
1858 a[(qy * w as i64 + qx - 1) as usize] = v_lo;
1859 a[(qy * w as i64 + qx) as usize] = v_mid;
1860 a[(qy * w as i64 + qx + 1) as usize] = v_hi;
1861 cx += step;
1862 }
1863 cy += step;
1864 }
1865 let (ra, rb) = (gray_to_rgba(&a), gray_to_rgba(&b));
1866 let (gw, gh) = crate::kernels::raster::downscaled_size(w, h, factor);
1867 let mut guide_data = vec![0u8; gw * gh];
1868 crate::kernels::raster::downscale_gray(&rb, w, h, factor, &mut guide_data);
1869 let guide = Gray {
1870 width: gw,
1871 height: gh,
1872 data: &guide_data,
1873 };
1874 let points = select_native_points(&rb, w, h, region, None, Some((guide, factor)));
1875 assert!(
1876 points.len() >= 12,
1877 "expected many per-cell points, got {}",
1878 points.len()
1879 );
1880 let r = refine_native_at(
1881 &ra,
1882 &rb,
1883 w,
1884 h,
1885 Point {
1886 x: dx as f64,
1887 y: dy as f64,
1888 },
1889 None,
1890 3,
1891 &points,
1892 );
1893 assert_eq!((r.x, r.y), (dx as i32, dy as i32));
1894 assert!(r.error < 1.0, "expected a near-zero error, got {}", r.error);
1895 }
1896
1897 #[test]
1904 fn native_refinement_guide_at_factor_one_matches_the_unguided_scan() {
1905 let (w, h) = (200usize, 200usize);
1906 let world = texture(w + 200, h + 200, (w * 13 + h) as u32);
1907 let b = crop(&world, w + 200, 100, 100, w, h);
1908 let rb = gray_to_rgba(&b);
1909 let region = Rect {
1910 x: 4.0,
1911 y: 4.0,
1912 width: (w - 20) as f64,
1913 height: (h - 20) as f64,
1914 };
1915 let unguided = select_native_points(&rb, w, h, region, None, None);
1916 let (gw, gh) = crate::kernels::raster::downscaled_size(w, h, 1);
1917 let mut guide_data = vec![0u8; gw * gh];
1918 crate::kernels::raster::downscale_gray(&rb, w, h, 1, &mut guide_data);
1919 let guide = Gray {
1920 width: gw,
1921 height: gh,
1922 data: &guide_data,
1923 };
1924 let guided = select_native_points(&rb, w, h, region, None, Some((guide, 1)));
1925 assert_eq!(unguided, guided);
1926 assert!(
1927 unguided.len() >= 12,
1928 "fixture should be dense enough to compare meaningfully"
1929 );
1930 }
1931
1932 #[test]
1939 fn native_refinement_recovers_a_shift_past_a_screen_fixed_line_on_a_lattice_row() {
1940 let (w, h) = (280usize, 280usize);
1941 let (dx, dy) = (5i64, -4i64);
1942 let (v0, v_lo, v_mid, v_hi, v_bar) = (100u8, 20u8, 150u8, 220u8, 250u8);
1943 let region = Rect {
1944 x: 6.0,
1945 y: 6.0,
1946 width: 270.0,
1947 height: 270.0,
1948 };
1949 let step = 3i64.max(((region.width * region.height / 1600.0).sqrt()).floor() as i64);
1952 assert!(
1953 step >= 6,
1954 "test assumes a wide enough cell, got step={step}"
1955 );
1956 let x_start = 2i64.max(region.x.ceil() as i64);
1957 let y_start = 2i64.max(region.y.ceil() as i64);
1958 let line_y = y_start + 12 * step;
1962 let mut a = vec![v0; w * h];
1963 let mut b = vec![v0; w * h];
1964 let mut cy = y_start;
1965 while cy < line_y {
1966 let mut cx = x_start;
1967 while (cx as f64) < region.x + region.width {
1968 let (px, py) = (cx + 3, cy + 3);
1972 b[(py * w as i64 + px - 1) as usize] = v_lo;
1973 b[(py * w as i64 + px) as usize] = v_mid;
1974 b[(py * w as i64 + px + 1) as usize] = v_hi;
1975 let (qx, qy) = (px + dx, py + dy);
1976 a[(qy * w as i64 + qx - 1) as usize] = v_lo;
1977 a[(qy * w as i64 + qx) as usize] = v_mid;
1978 a[(qy * w as i64 + qx + 1) as usize] = v_hi;
1979 cx += step;
1980 }
1981 cy += step;
1982 }
1983 for y in line_y as usize..h {
1985 for x in 0..w {
1986 let i = y * w + x;
1987 a[i] = v_bar;
1988 b[i] = v_bar;
1989 }
1990 }
1991 let (ra, rb) = (gray_to_rgba(&a), gray_to_rgba(&b));
1992 let points = select_native_points(&rb, w, h, region, None, None);
1993 let r = refine_native_at(
1994 &ra,
1995 &rb,
1996 w,
1997 h,
1998 Point {
1999 x: dx as f64,
2000 y: dy as f64,
2001 },
2002 None,
2003 3,
2004 &points,
2005 );
2006 assert_eq!((r.x, r.y), (dx as i32, dy as i32));
2007 assert!(
2008 r.error < 14.0,
2009 "expected the true shift to clear the odometry gate, got {}",
2010 r.error
2011 );
2012 }
2013}