use crate::kernels::geometry::{js_ceil, js_round, Rect, Rng};
pub const MOTION_CELL: usize = 24;
#[derive(Clone, Copy, Debug, PartialEq)]
pub struct Point {
pub x: f64,
pub y: f64,
}
#[derive(Clone, Debug, PartialEq)]
pub struct Motion {
pub x: f64,
pub y: f64,
pub support: u32,
pub unique: u32,
pub confidence: f64,
pub error: f64,
pub ambiguous: bool,
}
#[derive(Clone, Copy)]
pub struct Gray<'a> {
pub width: usize,
pub height: usize,
pub data: &'a [u8],
}
#[derive(Clone, Copy, Debug)]
pub struct MatchPoints {
pub ax: f64,
pub ay: f64,
pub bx: f64,
pub by: f64,
pub unique: bool,
}
fn median(values: &mut [f64]) -> f64 {
if values.is_empty() {
return 0.0;
}
values.sort_by(|a, b| a.partial_cmp(b).unwrap_or(std::cmp::Ordering::Equal));
let m = values.len() >> 1;
if values.len() & 1 == 1 {
values[m]
} else {
(values[m - 1] + values[m]) / 2.0
}
}
#[inline]
fn hypot(x: f64, y: f64) -> f64 {
libm::hypot(x, y)
}
pub fn translation_hypotheses(matches: &[MatchPoints], max: usize) -> Vec<Motion> {
let mut bins: Vec<((i32, i32), Vec<usize>)> = Vec::new();
for (i, m) in matches.iter().enumerate() {
let key = (js_round((m.ax - m.bx) / 3.0), js_round((m.ay - m.by) / 3.0));
match bins.iter_mut().find(|(k, _)| *k == key) {
Some((_, b)) => b.push(i),
None => bins.push((key, vec![i])),
}
}
bins.sort_by(|a, b| b.1.len().cmp(&a.1.len()));
bins.truncate(32);
let mut out: Vec<Motion> = Vec::new();
for (_, seed) in &bins {
let mut xs: Vec<f64> = seed
.iter()
.map(|&i| matches[i].ax - matches[i].bx)
.collect();
let mut ys: Vec<f64> = seed
.iter()
.map(|&i| matches[i].ay - matches[i].by)
.collect();
let (x, y) = (median(&mut xs), median(&mut ys));
let support: Vec<&MatchPoints> = matches
.iter()
.filter(|m| hypot(m.ax - m.bx - x, m.ay - m.by - y) <= 2.5)
.collect();
if support.len() < 3 {
continue;
}
let mut sx: Vec<f64> = support.iter().map(|m| m.ax - m.bx).collect();
let mut sy: Vec<f64> = support.iter().map(|m| m.ay - m.by).collect();
let (x, y) = (median(&mut sx), median(&mut sy));
if out.iter().any(|p| hypot(p.x - x, p.y - y) < 3.0) {
continue;
}
let unique = support.iter().filter(|m| m.unique).count();
let mut cells: Vec<(i32, i32)> = support
.iter()
.map(|m| ((m.bx as i32) >> 5, (m.by as i32) >> 5))
.collect();
cells.sort_unstable();
cells.dedup();
let spread = cells.len() as f64;
let n = support.len() as f64;
let confidence = ((1.0 - libm::exp(-n / 9.0))
* (0.48 + 0.52 * unique as f64 / n)
* (spread / 4.0).min(1.0))
.clamp(0.0, 0.99);
out.push(Motion {
x,
y,
support: support.len() as u32,
unique: unique as u32,
confidence,
error: 0.0,
ambiguous: (unique as f64) < (6.0f64).min(n * 0.2),
});
}
out.sort_by(|a, b| (b.support + b.unique).cmp(&(a.support + a.unique)));
out.truncate(max);
out
}
pub struct PatchError {
pub error: f64,
pub texture: f64,
pub n: u32,
}
pub fn patch_error(a: Gray<'_>, b: Gray<'_>, dx: f64, dy: f64, r: Rect, step: usize) -> PatchError {
let (w, h, aw, ah) = (
b.width as f64,
b.height as f64,
a.width as f64,
a.height as f64,
);
let x0 = 1f64.max(r.x.ceil()).max((-dx + 1.0).ceil());
let x1 = (w - 1.0).min(r.x + r.width).min(aw - dx - 1.0);
let y0 = 1f64.max(r.y.ceil()).max((-dy + 1.0).ceil());
let y1 = (h - 1.0).min(r.y + r.height).min(ah - dy - 1.0);
let (dx, dy) = (js_round(dx), js_round(dy));
let (mut total, mut texture, mut n) = (0f64, 0f64, 0u32);
let (bw, aw) = (b.width as i64, a.width as i64);
let mut y = y0;
while y < y1 {
let mut x = x0;
while x < x1 {
let i = (y as i64 * bw + x as i64) as usize;
let j = ((y as i64 + dy as i64) * aw + x as i64 + dx as i64) as usize;
let bd = b.data;
let gradient = (bd[i + 1] as i32 - bd[i - 1] as i32).abs()
+ (bd[i + b.width] as i32 - bd[i - b.width] as i32).abs();
let weight = if gradient > 12 { 2.0 } else { 1.0 };
total += (70.0f64).min((a.data[j] as i32 - bd[i] as i32).abs() as f64) * weight;
texture += gradient as f64;
n += if gradient > 12 { 2 } else { 1 };
x += step as f64;
}
y += step as f64;
}
PatchError {
error: if n > 8 {
total / n as f64
} else {
f64::INFINITY
},
texture: if n > 0 { texture / n as f64 } else { 0.0 },
n,
}
}
pub fn verify_translation(a: Gray<'_>, b: Gray<'_>, dx: f64, dy: f64, roi: Option<Rect>) -> f64 {
let r = roi.unwrap_or(Rect {
x: 0.0,
y: 0.0,
width: b.width as f64,
height: b.height as f64,
});
let mut errors: Vec<f64> = Vec::new();
let mut y = r.y;
while y < r.y + r.height {
let mut x = r.x;
while x < r.x + r.width {
let block = Rect {
x,
y,
width: (48.0f64).min(r.x + r.width - x),
height: (48.0f64).min(r.y + r.height - y),
};
let p = patch_error(a, b, dx, dy, block, 4);
if p.n > 12 && p.texture > 3.0 {
errors.push(p.error);
}
x += 48.0;
}
y += 48.0;
}
if errors.len() < 2 {
return f64::INFINITY;
}
errors.sort_by(|a, b| a.partial_cmp(b).unwrap_or(std::cmp::Ordering::Equal));
let keep = 2usize.max(js_ceil(errors.len() as f64 * 0.75) as usize);
errors.iter().take(keep).sum::<f64>() / keep as f64
}
#[derive(Clone, Copy, Debug, PartialEq)]
pub struct Audit {
pub error: f64,
pub mismatch: f64,
pub overlap: f64,
pub samples: u32,
pub blocks: u32,
pub agreeing: u32,
pub agreement: f64,
pub agreeing_error: f64,
}
pub fn audit_translation(
a: Gray<'_>,
b: Gray<'_>,
dx: f64,
dy: f64,
roi: Option<Rect>,
tolerant: bool,
) -> Audit {
let (texture_a, texture_b) = (a, b);
let filtered = tolerant.then(|| {
let mut aa = vec![0; a.data.len()];
let mut bb = vec![0; b.data.len()];
crate::kernels::features::smooth(a.data, a.width, a.height, &mut aa);
crate::kernels::features::smooth(b.data, b.width, b.height, &mut bb);
(aa, bb)
});
let (a, b) = match &filtered {
Some((aa, bb)) => (Gray { data: aa, ..a }, Gray { data: bb, ..b }),
None => (a, b),
};
let r = roi.unwrap_or(Rect {
x: 0.0,
y: 0.0,
width: b.width as f64,
height: b.height as f64,
});
let (dx, dy) = (js_round(dx) as f64, js_round(dy) as f64);
let x0 = 2f64.max(r.x.ceil()).max((r.x - dx).ceil()).max(1.0 - dx);
let y0 = 2f64.max(r.y.ceil()).max((r.y - dy).ceil()).max(1.0 - dy);
let x1 = (b.width as f64 - 2.0)
.min(a.width as f64 - dx - 2.0)
.min(r.x + r.width)
.min(r.x + r.width - dx);
let y1 = (b.height as f64 - 2.0)
.min(a.height as f64 - dy - 2.0)
.min(r.y + r.height)
.min(r.y + r.height - dy);
let overlap = (x1 - x0).max(0.0) * (y1 - y0).max(0.0) / (r.width * r.height).max(1.0);
const B: f64 = 32.0;
let bw = 1usize.max(js_ceil((x1 - x0) / B).max(0) as usize);
let bh = 1usize.max(js_ceil((y1 - y0) / B).max(0) as usize);
let mut block_error = vec![0f64; bw * bh];
let mut block_bad = vec![0f64; bw * bh];
let mut block_n = vec![0f64; bw * bh];
let (mut error, mut bad, mut n) = (0f64, 0u32, 0u32);
let (bwid, awid) = (b.width as i64, a.width as i64);
let (dxi, dyi) = (dx as i64, dy as i64);
let mut y = y0;
while y < y1 {
let mut x = x0;
while x < x1 {
let i = (y as i64 * bwid + x as i64) as usize;
let j = ((y as i64 + dyi) * awid + x as i64 + dxi) as usize;
let (ad, bd) = (texture_a.data, texture_b.data);
let gb = (bd[i + 1] as i32 - bd[i - 1] as i32).abs()
+ (bd[i + b.width] as i32 - bd[i - b.width] as i32).abs();
let ga = (ad[j + 1] as i32 - ad[j - 1] as i32).abs()
+ (ad[j + a.width] as i32 - ad[j - a.width] as i32).abs();
if ga.max(gb) < 20 {
x += 2.0;
continue;
}
let filtered_here = tolerant
&& x >= r.x + 2.0
&& x + dx >= r.x + 2.0
&& y >= r.y + 2.0
&& y + dy >= r.y + 2.0
&& x < r.x + r.width - 2.0
&& x + dx < r.x + r.width - 2.0
&& y < r.y + r.height - 2.0
&& y + dy < r.y + r.height - 2.0;
let (ad, bd) = if filtered_here {
(a.data, b.data)
} else {
(texture_a.data, texture_b.data)
};
let mut difference = (ad[j] as i32 - bd[i] as i32).abs();
if tolerant {
difference = difference
.min((ad[j + 1] as i32 - bd[i] as i32).abs())
.min((ad[j - 1] as i32 - bd[i] as i32).abs())
.min((ad[j + a.width] as i32 - bd[i] as i32).abs())
.min((ad[j - a.width] as i32 - bd[i] as i32).abs());
}
let clipped = difference.min(70) as f64;
let q = (((y - y0) / B).floor() as usize) * bw + ((x - x0) / B).floor() as usize;
error += clipped;
block_error[q] += clipped;
block_n[q] += 1.0;
if difference > 18 {
bad += 1;
block_bad[q] += 1.0;
}
n += 1;
x += 2.0;
}
y += 2.0;
}
let (mut blocks, mut agreeing, mut agreeing_error) = (0u32, 0u32, 0f64);
for q in 0..block_n.len() {
if block_n[q] < 12.0 {
continue;
}
blocks += 1;
if block_error[q] / block_n[q] < 10.0 && block_bad[q] / block_n[q] < 0.2 {
agreeing += 1;
agreeing_error += block_error[q] / block_n[q];
}
}
Audit {
error: if n >= 24 {
error / n as f64
} else {
f64::INFINITY
},
mismatch: if n > 0 { bad as f64 / n as f64 } else { 1.0 },
overlap,
samples: n,
blocks,
agreeing,
agreement: if blocks > 0 {
agreeing as f64 / blocks as f64
} else {
0.0
},
agreeing_error: if agreeing > 0 {
agreeing_error / agreeing as f64
} else {
f64::INFINITY
},
}
}
pub fn refine_translation(
a: Gray<'_>,
b: Gray<'_>,
p: Point,
roi: Option<Rect>,
radius: i32,
) -> (i32, i32) {
let r = roi.unwrap_or(Rect {
x: 0.0,
y: 0.0,
width: b.width as f64,
height: b.height as f64,
});
let (px, py) = (js_round(p.x), js_round(p.y));
let (mut best, mut error) = ((px, py), f64::INFINITY);
for dy in py - radius..=py + radius {
for dx in px - radius..=px + radius {
let e = patch_error(a, b, dx as f64, dy as f64, r, 7).error;
let closer = hypot(dx as f64 - p.x, dy as f64 - p.y)
< hypot(best.0 as f64 - p.x, best.1 as f64 - p.y);
if e < error - 1e-4 || ((e - error).abs() < 1e-4 && closer) {
error = e;
best = (dx, dy);
}
}
}
best
}
pub fn detect_scale(matches: &[MatchPoints]) -> f64 {
let m: Vec<&MatchPoints> = matches.iter().filter(|m| m.unique).collect();
if m.len() < 8 {
return 1.0;
}
let mut rng = Rng::new(7641);
let (mut best, mut best_scale) = (0usize, 1.0);
for _ in 0..96 {
let p = m[(rng.next_f64() * m.len() as f64).floor() as usize];
let q = m[(rng.next_f64() * m.len() as f64).floor() as usize];
let (bx, by, ax, ay) = (q.bx - p.bx, q.by - p.by, q.ax - p.ax, q.ay - p.ay);
let den = bx * bx + by * by;
if den < 1000.0 {
continue;
}
let u = (ax * bx + ay * by) / den;
let v = (ay * bx - ax * by) / den;
let s = hypot(u, v);
if !(0.55..=1.8).contains(&s) {
continue;
}
let tx = p.ax - u * p.bx + v * p.by;
let ty = p.ay - v * p.bx - u * p.by;
let n = m
.iter()
.filter(|r| {
hypot(
u * r.bx - v * r.by + tx - r.ax,
v * r.bx + u * r.by + ty - r.ay,
) < 2.5
})
.count();
if n > best {
best = n;
best_scale = s;
}
}
if best as f64 >= (8.0f64).max(m.len() as f64 * 0.5) {
best_scale
} else {
1.0
}
}
#[derive(Clone, Debug, PartialEq)]
pub struct MotionField {
pub motions: Vec<Motion>,
pub labels: Vec<u8>,
pub confidence: Vec<u8>,
pub dynamic: Vec<u8>,
pub cols: usize,
pub rows: usize,
pub cell: usize,
pub difference: f64,
pub feature_count: u32,
pub unknown: bool,
pub zoom: f64,
}
pub fn estimate_motion(
a: Gray<'_>,
b: Gray<'_>,
matches: &[MatchPoints],
feature_count: u32,
) -> MotionField {
let cols = b.width.div_ceil(MOTION_CELL);
let rows = b.height.div_ceil(MOTION_CELL);
let n = cols * rows;
let difference = crate::kernels::raster::mean_difference(a.data, b.data);
if difference < 0.12 {
return MotionField {
motions: vec![Motion {
x: 0.0,
y: 0.0,
support: 0,
unique: 0,
confidence: 0.98,
error: difference,
ambiguous: false,
}],
labels: vec![0; n],
confidence: vec![245; n],
dynamic: vec![0; n],
cols,
rows,
cell: MOTION_CELL,
difference,
feature_count: 0,
unknown: false,
zoom: 1.0,
};
}
let mut motions = translation_hypotheses(matches, 6);
if !motions.iter().any(|m| hypot(m.x, m.y) < 1.0) {
motions.push(Motion {
x: 0.0,
y: 0.0,
support: 0,
unique: 0,
confidence: 0.5,
error: 0.0,
ambiguous: false,
});
}
if motions.is_empty() {
motions.push(Motion {
x: 0.0,
y: 0.0,
support: 0,
unique: 0,
confidence: 0.0,
error: 255.0,
ambiguous: true,
});
}
for m in motions.iter_mut() {
let inliers: Vec<&MatchPoints> = matches
.iter()
.filter(|p| hypot(p.ax - p.bx - m.x, p.ay - p.by - m.y) < 3.0)
.take(45)
.collect();
let (mut best, mut bx, mut by) = (f64::INFINITY, m.x, m.y);
let (mx, my) = (js_round(m.x), js_round(m.y));
for y in my - 1..=my + 1 {
for x in mx - 1..=mx + 1 {
let (mut sum, mut k) = (0f64, 0u32);
for p in &inliers {
let e = patch_error(
a,
b,
x as f64,
y as f64,
Rect {
x: p.bx - 7.0,
y: p.by - 7.0,
width: 15.0,
height: 15.0,
},
3,
)
.error;
if e.is_finite() {
sum += e;
k += 1;
}
}
if k > 0 && sum / (k as f64) < best {
best = sum / k as f64;
bx = x as f64;
by = y as f64;
}
}
}
m.x = bx;
m.y = by;
m.error = if best.is_finite() {
best
} else {
verify_translation(a, b, m.x, m.y, None)
};
if m.support > 0 {
m.confidence *= libm::exp(-(m.error.min(80.0)) / 35.0);
}
}
motions.sort_by(|a, b| (b.support + b.unique).cmp(&(a.support + a.unique)));
let dominant = motions
.iter()
.position(|m| hypot(m.x, m.y) > 1.0 && m.support >= 4)
.unwrap_or(0);
let mut labels = vec![0u8; n];
let mut confidence = vec![0u8; n];
let mut dynamic = vec![0u8; n];
let mut informative = vec![0u8; n];
for cy in 0..rows {
for cx in 0..cols {
let idx = cy * cols + cx;
let r = Rect {
x: (cx * MOTION_CELL) as f64,
y: (cy * MOTION_CELL) as f64,
width: MOTION_CELL.min(b.width - cx * MOTION_CELL) as f64,
height: MOTION_CELL.min(b.height - cy * MOTION_CELL) as f64,
};
let (mut best, mut second, mut choice, mut texture, mut dominant_error) =
(f64::INFINITY, f64::INFINITY, dominant, 0f64, f64::INFINITY);
for (k, m) in motions.iter().enumerate() {
let p = patch_error(a, b, m.x, m.y, r, 3);
texture = texture.max(p.texture);
if k == dominant {
dominant_error = p.error;
}
if p.error < best {
second = best;
best = p.error;
choice = k;
} else if p.error < second {
second = p.error;
}
}
if texture < 3.0
|| !best.is_finite()
|| second - best < 0.6
|| (!dominant_error.is_finite() && choice != dominant)
{
labels[idx] = dominant as u8;
confidence[idx] = 65;
} else {
labels[idx] = choice as u8;
confidence[idx] = js_round(
255.0 * libm::exp(-best / 24.0) * ((second - best) / 8.0).clamp(0.25, 1.0),
) as u8;
informative[idx] = (confidence[idx] > 100) as u8;
if best > 22.0 {
dynamic[idx] = 1;
labels[idx] = dominant as u8;
confidence[idx] = 45;
}
}
}
}
for _ in 0..3 {
let mut next = labels.clone();
for y in 0..rows as i64 {
for x in 0..cols as i64 {
let i = (y * cols as i64 + x) as usize;
if informative[i] != 0 {
continue;
}
let mut votes = vec![0f64; motions.len()];
for dy in -2i64..=2 {
for dx in -2i64..=2 {
let (xx, yy) = (x + dx, y + dy);
if xx < 0 || yy < 0 || xx >= cols as i64 || yy >= rows as i64 {
continue;
}
let j = (yy * cols as i64 + xx) as usize;
votes[labels[j] as usize] +=
confidence[j] as f64 / (1 + dx * dx + dy * dy) as f64;
}
}
let mut k = dominant;
for (j, v) in votes.iter().enumerate() {
if *v > votes[k] {
k = j;
}
}
next[i] = k as u8;
}
}
labels = next;
}
let strongest =
motions.iter().fold(
&motions[0],
|m, n| if n.confidence > m.confidence { n } else { m },
);
let unknown = strongest.support < 4 && difference > 5.0;
MotionField {
motions,
labels,
confidence,
dynamic,
cols,
rows,
cell: MOTION_CELL,
difference,
feature_count,
unknown,
zoom: detect_scale(matches),
}
}
pub struct NativeRefinement {
pub x: i32,
pub y: i32,
pub error: f64,
pub samples: u32,
pub runner_up: f64,
pub trimmed: f64,
}
pub const TRIMMED_KEEP: f64 = 0.75;
#[allow(clippy::too_many_arguments)]
fn trimmed_error(
a: &[u8],
b: &[u8],
w: i64,
h: i64,
dx: i64,
dy: i64,
inside: impl Fn(i64, i64) -> bool,
points: &[i64],
) -> f64 {
let mut errors: Vec<f64> = Vec::with_capacity(points.len());
for &p in points {
let (x, y) = (p % w, p / w);
let (xx, yy) = (x + dx, y + dy);
if xx < 1 || yy < 1 || xx >= w - 1 || yy >= h - 1 || !inside(xx, yy) {
continue;
}
let i = (p * 4) as usize;
let j = ((yy * w + xx) * 4) as usize;
let d = (a[j] as i32 - b[i] as i32).abs()
+ (a[j + 1] as i32 - b[i + 1] as i32).abs()
+ (a[j + 2] as i32 - b[i + 2] as i32).abs();
errors.push((90.0f64).min(d as f64 / 3.0));
}
if errors.len() < 12 {
return f64::INFINITY;
}
errors.sort_by(|x, y| x.total_cmp(y));
let keep = ((errors.len() as f64) * TRIMMED_KEEP).ceil() as usize;
errors[..keep].iter().sum::<f64>() / keep as f64
}
pub fn select_native_points(
b: &[u8],
width: usize,
height: usize,
region: Rect,
mask: Option<(&[u8], u8)>,
guide: Option<(Gray<'_>, usize)>,
) -> Vec<i64> {
let (w, h) = (width as i64, height as i64);
let step = 3i64.max(((region.width * region.height / 1600.0).sqrt()).floor() as i64);
let inside =
|x: i64, y: i64| mask.is_none_or(|(labels, code)| labels[(y * w + x) as usize] == code);
let y_limit = ((h - 2) as f64).min(region.y + region.height);
let x_limit = ((w - 2) as f64).min(region.x + region.width);
let x_start = 2i64.max(region.x.ceil() as i64);
let y_start = 2i64.max(region.y.ceil() as i64);
if (x_start as f64) >= x_limit || (y_start as f64) >= y_limit {
return Vec::new();
}
if let Some((g, f)) = guide {
return select_native_points_guided(
b,
w,
h,
x_start,
y_start,
x_limit,
y_limit,
step,
&inside,
g,
f.max(1) as i64,
);
}
let (bx0, by0) = ((x_start - 1).max(0), (y_start - 1).max(0));
let (bx1, by1) = (
(js_ceil(x_limit) as i64 + 1).min(w),
(js_ceil(y_limit) as i64 + 1).min(h),
);
let (bw, bh) = ((bx1 - bx0).max(0) as usize, (by1 - by0).max(0) as usize);
let mut luma_plane = vec![0u8; bw * bh];
for row in 0..bh {
let src = (((by0 + row as i64) * w + bx0) * 4) as usize;
let dst = row * bw;
for col in 0..bw {
let i = src + col * 4;
luma_plane[dst + col] =
((b[i] as u32 * 77 + b[i + 1] as u32 * 150 + b[i + 2] as u32 * 29) >> 8) as u8;
}
}
let mut points: Vec<i64> = Vec::new();
let mut cy = y_start;
while (cy as f64) < y_limit {
let mut cx = x_start;
while (cx as f64) < x_limit {
let mut best: Option<(i32, i64)> = None;
let mut dy = 0i64;
while dy < step {
let y = cy + dy;
if (y as f64) >= y_limit {
break;
}
let (row_mid, row_up, row_down) = (
(y - by0) as usize * bw,
(y - 1 - by0) as usize * bw,
(y + 1 - by0) as usize * bw,
);
let mut dx = 0i64;
while dx < step {
let x = cx + dx;
if (x as f64) >= x_limit {
break;
}
if inside(x, y) {
let col = (x - bx0) as usize;
let grad = (luma_plane[row_mid + col - 1] as i32
- luma_plane[row_mid + col + 1] as i32)
.abs()
+ (luma_plane[row_up + col] as i32 - luma_plane[row_down + col] as i32)
.abs();
if best.is_none_or(|(bg, _)| grad > bg) {
best = Some((grad, y * w + x));
}
}
dx += 1;
}
dy += 1;
}
if let Some((grad, p)) = best {
if grad > 12 {
points.push(p);
}
}
cx += step;
}
cy += step;
}
points
}
#[allow(clippy::too_many_arguments)]
fn select_native_points_guided(
b: &[u8],
w: i64,
h: i64,
x_start: i64,
y_start: i64,
x_limit: f64,
y_limit: f64,
step: i64,
inside: &impl Fn(i64, i64) -> bool,
g: Gray<'_>,
f: i64,
) -> Vec<i64> {
let (gw, gh) = (g.width as i64, g.height as i64);
let native_luma = |x: i64, y: i64| -> i32 {
let i = ((y * w + x) * 4) as usize;
((b[i] as u32 * 77 + b[i + 1] as u32 * 150 + b[i + 2] as u32 * 29) >> 8) as i32
};
let (window_x_hi, window_y_hi) = (
(js_ceil(x_limit) as i64 - 1).min(w - 1),
(js_ceil(y_limit) as i64 - 1).min(h - 1),
);
let mut points: Vec<i64> = Vec::new();
let mut cy = y_start;
while (cy as f64) < y_limit {
let mut cx = x_start;
while (cx as f64) < x_limit {
let (cell_x_hi, cell_y_hi) = (
(cx + step - 1).min(window_x_hi),
(cy + step - 1).min(window_y_hi),
);
let mut chosen: Option<(i64, i64)> = None;
let mut best_analysis_grad = i32::MIN;
let mut ay = (cy / f).max(1);
while ay <= (cell_y_hi / f).min(gh - 2) {
let mut ax = (cx / f).max(1);
while ax <= (cell_x_hi / f).min(gw - 2) {
let (nx0, ny0) = (ax * f, ay * f);
let (nx1, ny1) = ((nx0 + f).min(w), (ny0 + f).min(h));
let (cx_mid, cy_mid) = (nx0 + (nx1 - nx0) / 2, ny0 + (ny1 - ny0) / 2);
if inside(cx_mid, cy_mid) {
let gi = (ay * gw + ax) as usize;
let grad = (g.data[gi - 1] as i32 - g.data[gi + 1] as i32).abs()
+ (g.data[gi - gw as usize] as i32 - g.data[gi + gw as usize] as i32)
.abs();
if grad > best_analysis_grad {
best_analysis_grad = grad;
chosen = Some((ax, ay));
}
}
ax += 1;
}
ay += 1;
}
if let Some((ax, ay)) = chosen {
let (nx0, ny0) = (ax * f, ay * f);
let (nx1, ny1) = ((nx0 + f).min(w), (ny0 + f).min(h));
let mut best: Option<(i32, i64)> = None;
let mut y = ny0.max(cy).max(y_start);
let y_hi = ny1.min(cy + step).min(h);
while y < y_hi && (y as f64) < y_limit {
let mut x = nx0.max(cx).max(x_start);
let x_hi = nx1.min(cx + step).min(w);
while x < x_hi && (x as f64) < x_limit {
if inside(x, y) {
let grad = (native_luma(x - 1, y) - native_luma(x + 1, y)).abs()
+ (native_luma(x, y - 1) - native_luma(x, y + 1)).abs();
if best.is_none_or(|(bg, _)| grad > bg) {
best = Some((grad, y * w + x));
}
}
x += 1;
}
y += 1;
}
if let Some((grad, p)) = best {
if grad > 12 {
points.push(p);
}
}
}
cx += step;
}
cy += step;
}
points
}
#[allow(clippy::too_many_arguments)]
pub fn refine_native_at(
a: &[u8],
b: &[u8],
width: usize,
height: usize,
guess: Point,
mask: Option<(&[u8], u8)>,
radius: i32,
points: &[i64],
) -> NativeRefinement {
let (w, h) = (width as i64, height as i64);
let (gx, gy) = (js_round(guess.x), js_round(guess.y));
let inside =
|x: i64, y: i64| mask.is_none_or(|(labels, code)| labels[(y * w + x) as usize] == code);
if points.len() < 12 {
return NativeRefinement {
x: gx,
y: gy,
error: f64::INFINITY,
samples: points.len() as u32,
runner_up: f64::INFINITY,
trimmed: f64::INFINITY,
};
}
let mut samples = 0u32;
let mut cost = |dx: i64, dy: i64| -> f64 {
let (mut error, mut n) = (0f64, 0u32);
for &p in points {
let (x, y) = (p % w, p / w);
let (xx, yy) = (x + dx, y + dy);
if xx < 1 || yy < 1 || xx >= w - 1 || yy >= h - 1 || !inside(xx, yy) {
continue;
}
let i = (p * 4) as usize;
let j = ((yy * w + xx) * 4) as usize;
let d = (a[j] as i32 - b[i] as i32).abs()
+ (a[j + 1] as i32 - b[i + 1] as i32).abs()
+ (a[j + 2] as i32 - b[i + 2] as i32).abs();
error += (90.0f64).min(d as f64 / 3.0);
n += 1;
}
samples = samples.max(n);
if n >= 12 {
error / n as f64
} else {
f64::INFINITY
}
};
let (mut best, mut px, mut py) = (f64::INFINITY, gx, gy);
let mut costs: Vec<(i32, i32, f64)> = Vec::new();
for y in gy - radius..=gy + radius {
for x in gx - radius..=gx + radius {
let e = cost(x as i64, y as i64);
costs.push((x, y, e));
let closer = hypot(x as f64 - guess.x, y as f64 - guess.y)
< hypot(px as f64 - guess.x, py as f64 - guess.y);
if e < best - 1e-6 || ((e - best).abs() < 1e-6 && closer) {
best = e;
px = x;
py = y;
}
}
}
let runner_up = costs
.iter()
.filter(|c| (c.0 - px).abs().max((c.1 - py).abs()) > 2)
.map(|c| c.2)
.fold(f64::INFINITY, f64::min);
let trimmed = if best.is_finite() {
trimmed_error(a, b, w, h, px as i64, py as i64, inside, points)
} else {
f64::INFINITY
};
NativeRefinement {
x: px,
y: py,
error: best,
samples,
runner_up,
trimmed,
}
}
#[allow(clippy::too_many_arguments)]
pub fn sampling_insensitive_error(
a: &[u8],
b: &[u8],
width: usize,
height: usize,
offset: (i32, i32),
mask: Option<(&[u8], u8)>,
radius: i32,
points: &[i64],
) -> (f64, f64) {
let (w, h) = (width as i64, height as i64);
let inside =
|x: i64, y: i64| mask.is_none_or(|(labels, code)| labels[(y * w + x) as usize] == code);
let stride = width * 4;
let span = |image: &[u8], k: usize| {
let v = image[k] as i32;
[k - 4, k + 4, k - stride, k + stride]
.iter()
.fold((2 * v, 2 * v), |(low, high), &n| {
(low.min(v + image[n] as i32), high.max(v + image[n] as i32))
})
};
let outside = |v: i32, (low, high): (i32, i32)| (v - high).max(low - v).max(0);
let cost = |dx: i64, dy: i64| -> f64 {
let (mut error, mut n) = (0f64, 0u32);
for &p in points {
let (x, y) = (p % w, p / w);
let (xx, yy) = (x + dx, y + dy);
if xx < 1 || yy < 1 || xx >= w - 1 || yy >= h - 1 || !inside(xx, yy) {
continue;
}
let i = (p * 4) as usize;
let j = ((yy * w + xx) * 4) as usize;
let d: i32 = (0..3)
.map(|c| {
outside(2 * b[i + c] as i32, span(a, j + c))
.min(outside(2 * a[j + c] as i32, span(b, i + c)))
})
.sum();
error += (90.0f64).min(d as f64 / 6.0);
n += 1;
}
if n >= 12 {
error / n as f64
} else {
f64::INFINITY
}
};
let (ox, oy) = offset;
let mut runner_up = f64::INFINITY;
for y in oy - radius..=oy + radius {
for x in ox - radius..=ox + radius {
if (x - ox).abs().max((y - oy).abs()) > 2 {
runner_up = runner_up.min(cost(x as i64, y as i64));
}
}
}
(cost(ox as i64, oy as i64), runner_up)
}
#[allow(clippy::too_many_arguments)]
pub fn refine_native(
a: &[u8],
b: &[u8],
width: usize,
height: usize,
guess: Point,
region: Rect,
mask: Option<(&[u8], u8)>,
radius: i32,
) -> NativeRefinement {
let points = select_native_points(b, width, height, region, mask, None);
refine_native_at(a, b, width, height, guess, mask, radius, &points)
}
pub struct Patch<'a> {
pub x: i32,
pub y: i32,
pub size: usize,
pub data: &'a [u8],
}
pub fn refine_patches(
patches: &[Patch<'_>],
native: Gray<'_>,
region: Rect,
guess: Point,
radius: i32,
) -> NativeRefinement {
refine_weighted_patches(patches, native, region, guess, radius, &[])
}
pub fn refine_spatial_patches(
patches: &[Patch<'_>],
native: Gray<'_>,
region: Rect,
guess: Point,
radius: i32,
) -> NativeRefinement {
let mut weights = vec![0.0; patches.len()];
if let Some(first) = patches.first() {
let step = first.size;
for y in (step / 2..js_round(region.height) as usize).step_by(step) {
for x in (step / 2..js_round(region.width) as usize).step_by(step) {
let nearest = patches
.iter()
.enumerate()
.min_by_key(|(_, p)| {
let dx = x as i64 - p.x as i64 - p.size as i64 / 2;
let dy = y as i64 - p.y as i64 - p.size as i64 / 2;
dx * dx + dy * dy
})
.unwrap()
.0;
weights[nearest] += 1.0;
}
}
}
refine_weighted_patches(patches, native, region, guess, radius, &weights)
}
fn refine_weighted_patches(
patches: &[Patch<'_>],
native: Gray<'_>,
region: Rect,
guess: Point,
radius: i32,
weights: &[f64],
) -> NativeRefinement {
let (rx, ry, rw, rh) = (
js_round(region.x),
js_round(region.y),
js_round(region.width),
js_round(region.height),
);
let w = native.width as i64;
let (gx, gy) = (js_round(guess.x), js_round(guess.y));
let region_inside = rx >= 0
&& ry >= 0
&& rw >= 0
&& rh >= 0
&& (rx + rw) as usize <= native.width
&& (ry + rh) as usize <= native.height;
if patches.is_empty() || !region_inside {
return NativeRefinement {
x: gx,
y: gy,
error: f64::INFINITY,
samples: 0,
runner_up: f64::INFINITY,
trimmed: f64::INFINITY,
};
}
let mut samples = 0u32;
let mut cost = |dx: i32, dy: i32| -> f64 {
let (mut error, mut n, mut weight_sum) = (0f64, 0u32, 0f64);
for (i, p) in patches.iter().enumerate() {
let weight = weights.get(i).copied().unwrap_or(1.0);
let (cx, cy) = (p.x - dx, p.y - dy);
if cx < 0 || cy < 0 || cx + p.size as i32 > rw || cy + p.size as i32 > rh {
continue;
}
let mut py = 0usize;
while py < p.size {
let row_a = ((ry + cy) as i64 + py as i64) * w + (rx + cx) as i64;
let row_p = py * p.size;
let mut px = 0usize;
while px < p.size {
error += weight
* (90.0f64).min(
(native.data[(row_a + px as i64) as usize] as i32
- p.data[row_p + px] as i32)
.abs() as f64,
);
n += 1;
weight_sum += weight;
px += 2;
}
py += 2;
}
}
samples = samples.max(n);
if n >= 64 {
error / weight_sum
} else {
f64::INFINITY
}
};
let (mut best, mut bx, mut by) = (f64::INFINITY, gx, gy);
let mut costs: Vec<(i32, i32, f64)> = Vec::new();
for y in gy - radius..=gy + radius {
for x in gx - radius..=gx + radius {
let e = cost(x, y);
costs.push((x, y, e));
let closer = hypot(x as f64 - guess.x, y as f64 - guess.y)
< hypot(bx as f64 - guess.x, by as f64 - guess.y);
if e < best - 1e-6 || ((e - best).abs() < 1e-6 && closer) {
best = e;
bx = x;
by = y;
}
}
}
let runner_up = costs
.iter()
.filter(|c| (c.0 - bx).abs().max((c.1 - by).abs()) > 2)
.map(|c| c.2)
.fold(f64::INFINITY, f64::min);
NativeRefinement {
x: bx,
y: by,
error: best,
samples,
runner_up,
trimmed: f64::INFINITY,
}
}
pub struct ExtractedPatch {
pub x: f64,
pub y: f64,
pub size: usize,
pub data: Vec<u8>,
}
#[allow(clippy::too_many_arguments)]
pub fn extract_patches(
native: &[u8],
native_width: usize,
native_height: usize,
region: Rect,
features: &[(f64, f64)],
factor: f64,
count: usize,
size: usize,
) -> Vec<ExtractedPatch> {
let mut out: Vec<ExtractedPatch> = Vec::new();
let (rx, ry, rw, rh) = (
js_round(region.x),
js_round(region.y),
js_round(region.width),
js_round(region.height),
);
if (rw as f64) < size as f64 + 2.0 || (rh as f64) < size as f64 + 2.0 {
return out;
}
if rx < 0 || ry < 0 || (rx + rw) as usize > native_width || (ry + rh) as usize > native_height {
return out;
}
let mut taken: Vec<(f64, f64)> = Vec::new();
for &(fx, fy) in features {
if out.len() >= count {
break;
}
let x = (js_round(fx * factor) as f64 - size as f64 / 2.0)
.clamp(0.0, (rw - size as i32) as f64);
let y = (js_round(fy * factor) as f64 - size as f64 / 2.0)
.clamp(0.0, (rh - size as i32) as f64);
if taken
.iter()
.any(|&(tx, ty)| (tx - x).abs() < size as f64 && (ty - y).abs() < size as f64)
{
continue;
}
taken.push((x, y));
let mut data = vec![0u8; size * size];
for row in 0..size {
let src = ((ry + y as i32 + row as i32) as i64 * native_width as i64
+ (rx + x as i32) as i64) as usize;
data[row * size..row * size + size].copy_from_slice(&native[src..src + size]);
}
out.push(ExtractedPatch { x, y, size, data });
}
out
}
pub fn probe_scale(
previous: Gray<'_>,
current: Gray<'_>,
current_features: &[crate::kernels::features::Feature],
roi: Option<Rect>,
scales: &[f64],
) -> Option<(f64, f64)> {
let mut best: Option<(f64, f64)> = None;
for &scale in scales {
let (sw, sh, sdata) = resample_gray(previous, scale);
let scaled_features = crate::kernels::features::extract_features(&sdata, sw, sh, 320, None);
let matches =
crate::kernels::features::match_features(&scaled_features, current_features, true);
let match_points: Vec<MatchPoints> = matches
.iter()
.map(|m| MatchPoints {
ax: scaled_features[m.a as usize].x as f64,
ay: scaled_features[m.a as usize].y as f64,
bx: current_features[m.b as usize].x as f64,
by: current_features[m.b as usize].y as f64,
unique: m.unique,
})
.collect();
let scaled_gray = Gray {
width: sw,
height: sh,
data: &sdata,
};
for m in translation_hypotheses(&match_points, 4) {
if m.support < 8 {
continue;
}
let audit = audit_translation(scaled_gray, current, m.x, m.y, roi, true);
if audit.samples < 200
|| !audit.error.is_finite()
|| audit.agreement < 0.5
|| audit.agreeing_error > 10.0
{
continue;
}
if best.is_none_or(|(_, e)| audit.agreeing_error < e) {
best = Some((scale, audit.agreeing_error));
}
}
}
best
}
pub fn resample_gray(g: Gray<'_>, scale: f64) -> (usize, usize, Vec<u8>) {
let width = 2usize.max(js_round(g.width as f64 * scale) as usize);
let height = 2usize.max(js_round(g.height as f64 * scale) as usize);
let mut data = vec![0u8; width * height];
for y in 0..height {
for x in 0..width {
let sx = (g.width as f64 - 1.001).min(x as f64 / scale).max(0.0);
let sy = (g.height as f64 - 1.001).min(y as f64 / scale).max(0.0);
let (x0, y0) = (sx.floor(), sy.floor());
let (fx, fy) = (sx - x0, sy - y0);
let (x0, y0) = (x0 as usize, y0 as usize);
let (x1, y1) = ((x0 + 1).min(g.width - 1), (y0 + 1).min(g.height - 1));
let d = g.data;
let at = |xx: usize, yy: usize| d[yy * g.width + xx] as f64;
let value = (at(x0, y0) * (1.0 - fx) + at(x1, y0) * fx) * (1.0 - fy)
+ (at(x0, y1) * (1.0 - fx) + at(x1, y1) * fx) * fy;
data[y * width + x] = value as u8;
}
}
(width, height, data)
}
#[cfg(test)]
mod tests {
use super::*;
use crate::kernels::features::{extract_features, match_features};
#[test]
fn tolerant_audit_keeps_faint_narrow_texture_samples() {
let (w, h) = (128, 128);
let pixels = |offset: usize| {
(0..w * h)
.map(|i| if (i % w + offset) % 16 == 8 { 45 } else { 20 })
.collect::<Vec<u8>>()
};
let a = pixels(0);
let b = pixels(3);
let audit = audit_translation(
Gray {
width: w,
height: h,
data: &a,
},
Gray {
width: w,
height: h,
data: &b,
},
3.0,
0.0,
None,
true,
);
assert!(audit.samples >= 24);
assert_eq!(audit.error, 0.0);
assert_eq!(audit.agreement, 1.0);
}
#[test]
fn tolerant_audit_preserves_texture_at_the_pane_boundary() {
let (w, h) = (128, 128);
let mut pixels = vec![20; w * h];
for y in 0..h {
pixels[y * w + 111] = 80;
}
let gray = Gray {
width: w,
height: h,
data: &pixels,
};
let roi = Some(Rect {
x: 16.0,
y: 16.0,
width: 96.0,
height: 96.0,
});
let raw = audit_translation(gray, gray, 0.0, 0.0, roi, false);
let tolerant = audit_translation(gray, gray, 0.0, 0.0, roi, true);
assert!(raw.samples >= 24);
assert_eq!(tolerant.samples, raw.samples);
assert_eq!(tolerant.error, 0.0);
}
fn texture(width: usize, height: usize, seed: u32) -> Vec<u8> {
let mut rng = Rng::new(seed | 1);
let mut data = vec![0u8; width * height];
for y in 0..height {
for x in 0..width {
let block = ((x >> 5) * 7 + (y >> 5) * 13) & 3;
let base = match block {
0 => 30,
1 => 120,
2 => 200,
_ => ((x + y) & 255) as i32,
};
data[y * width + x] =
(base + (rng.next_f64() * 24.0).floor() as i32 - 12).clamp(0, 255) as u8;
}
}
data
}
#[test]
fn the_trimmed_error_drops_a_static_band_but_not_a_wrong_offset() {
let (w, h, dy, band) = (240usize, 240usize, 7i64, 48usize);
let mut rng = Rng::new(11);
let cells: Vec<u8> = (0..(w / 2) * ((h + 64) / 2))
.map(|_| (rng.next_f64() * 256.0) as u8)
.collect();
let world: Vec<u8> = (0..w * (h + 64))
.map(|i| cells[(i / w / 2) * (w / 2) + (i % w) / 2])
.collect();
let frame = |scroll: usize| -> Vec<u8> {
let mut out = vec![255u8; w * h * 4];
for y in 0..h {
for x in 0..w {
let v = if y >= h - band {
if (x / 7 + y / 7) % 2 == 0 {
0
} else {
255
}
} else {
world[(y + scroll) * w + x]
};
out[(y * w + x) * 4..(y * w + x) * 4 + 3].copy_from_slice(&[v, v, v]);
}
}
out
};
let (a, b) = (frame(0), frame(dy as usize));
let region = Rect {
x: 0.0,
y: 0.0,
width: w as f64,
height: h as f64,
};
let points = select_native_points(&b, w, h, region, None, None);
let right = refine_native_at(
&a,
&b,
w,
h,
Point {
x: 0.0,
y: dy as f64,
},
None,
3,
&points,
);
assert_eq!((right.x, right.y), (0, dy as i32));
assert!(
right.error >= 14.0,
"the band must push the plain mean past the limit: {}",
right.error
);
assert!(
right.trimmed < 8.0,
"trimmed {} at the true offset",
right.trimmed
);
let slipped = refine_native_at(
&a,
&b,
w,
h,
Point {
x: 0.0,
y: (dy + 10) as f64,
},
None,
3,
&points,
);
assert!(
slipped.trimmed >= 8.0,
"trimmed {} at a wrong offset",
slipped.trimmed
);
}
fn crop(world: &[u8], ww: usize, x: usize, y: usize, w: usize, h: usize) -> Vec<u8> {
let mut out = vec![0u8; w * h];
for row in 0..h {
out[row * w..(row + 1) * w]
.copy_from_slice(&world[(y + row) * ww + x..(y + row) * ww + x + w]);
}
out
}
#[test]
fn whole_motion_pipeline_runs_on_textured_shifts() {
for (w, h, dx, dy) in [
(320usize, 240usize, 17i64, 23i64),
(640, 360, -41, 8),
(200, 320, 0, 0),
(256, 256, 3, -37),
] {
let world = texture(w + 200, h + 200, (w * 31 + h) as u32);
let a = crop(&world, w + 200, 100, 100, w, h);
let b = crop(
&world,
w + 200,
(100 + dx) as usize,
(100 + dy) as usize,
w,
h,
);
let fa = extract_features(&a, w, h, 480, None);
let fb = extract_features(&b, w, h, 480, None);
let matches: Vec<MatchPoints> = match_features(&fa, &fb, true)
.iter()
.map(|m| MatchPoints {
ax: fa[m.a as usize].x as f64,
ay: fa[m.a as usize].y as f64,
bx: fb[m.b as usize].x as f64,
by: fb[m.b as usize].y as f64,
unique: m.unique,
})
.collect();
let ga = Gray {
width: w,
height: h,
data: &a,
};
let gb = Gray {
width: w,
height: h,
data: &b,
};
let hyp = translation_hypotheses(&matches, 6);
assert!(!hyp.is_empty() || matches.len() < 3);
let _ = detect_scale(&matches);
for roi in [
None,
Some(Rect {
x: 10.5,
y: 7.0,
width: w as f64 / 2.0,
height: h as f64 / 2.0,
}),
] {
for (tx, ty) in [
(dx as f64, dy as f64),
(dx as f64 + 1.0, dy as f64 - 2.0),
(0.0, 0.0),
] {
let _ = verify_translation(ga, gb, tx, ty, roi);
let _ = audit_translation(ga, gb, tx, ty, roi, true);
let _ = refine_translation(
ga,
gb,
Point {
x: tx + 0.4,
y: ty - 0.4,
},
roi,
2,
);
}
}
let field = estimate_motion(ga, gb, &matches, fb.len() as u32);
assert_eq!(field.labels.len(), field.cols * field.rows);
let rgba = |g: &[u8]| -> Vec<u8> {
g.iter()
.flat_map(|&v| [v, ((v as u32 * 3) & 255) as u8, 255 - v, 255])
.collect()
};
let (ra, rb) = (rgba(&a), rgba(&b));
let region = Rect {
x: 4.5,
y: 3.0,
width: (w - 20) as f64,
height: (h - 9) as f64,
};
for guess in [
Point {
x: dx as f64 + 0.3,
y: dy as f64 - 0.6,
},
Point { x: 0.0, y: 40.0 },
] {
let _ = refine_native(&ra, &rb, w, h, guess, region, None, 3);
}
let _ = resample_gray(ga, 2.0);
let _ = resample_gray(ga, 0.5);
let features: Vec<(f64, f64)> = fb.iter().map(|f| (f.x as f64, f.y as f64)).collect();
let patches = extract_patches(&b, w, h, region, &features, 1.0, 24, 32);
assert!(patches.len() <= 24);
for p in &patches {
assert_eq!(p.data.len(), 32 * 32);
assert!(p.x >= 0.0 && p.y >= 0.0);
}
let _ = refine_patches(
&patches
.iter()
.map(|p| Patch {
x: p.x as i32,
y: p.y as i32,
size: p.size,
data: &p.data,
})
.collect::<Vec<_>>(),
Gray {
width: w,
height: h,
data: &b,
},
region,
Point {
x: -(dx as f64),
y: -(dy as f64),
},
3,
);
assert!(extract_patches(
&b,
w,
h,
Rect {
x: 0.0,
y: 0.0,
width: 10.0,
height: 10.0
},
&features,
1.0,
24,
32
)
.is_empty());
let probe = probe_scale(ga, gb, &fb, None, &[1.0, 1.1, 0.9]);
if let Some((scale, error)) = probe {
assert!(scale > 0.0 && error.is_finite());
}
}
}
fn gray_to_rgba(g: &[u8]) -> Vec<u8> {
g.iter().flat_map(|&v| [v, v, v, 255]).collect()
}
#[test]
fn native_refinement_recovers_a_sparse_shift_the_lattice_would_miss() {
let (w, h) = (280usize, 280usize);
let (dx, dy) = (4i64, -3i64);
let (v0, v_lo, v_mid, v_hi) = (100u8, 20u8, 150u8, 220u8);
let region = Rect {
x: 6.0,
y: 6.0,
width: 270.0,
height: 270.0,
};
let step = 3i64.max(((region.width * region.height / 1600.0).sqrt()).floor() as i64);
assert!(
step >= 6,
"test assumes a wide enough cell, got step={step}"
);
let x_start = 2i64.max(region.x.ceil() as i64);
let y_start = 2i64.max(region.y.ceil() as i64);
let mut a = vec![v0; w * h];
let mut b = vec![v0; w * h];
let mut cy = y_start;
while (cy as f64) < region.y + region.height {
let mut cx = x_start;
while (cx as f64) < region.x + region.width {
let (px, py) = (cx + 3, cy + 3);
b[(py * w as i64 + px - 1) as usize] = v_lo;
b[(py * w as i64 + px) as usize] = v_mid;
b[(py * w as i64 + px + 1) as usize] = v_hi;
let (qx, qy) = (px + dx, py + dy);
a[(qy * w as i64 + qx - 1) as usize] = v_lo;
a[(qy * w as i64 + qx) as usize] = v_mid;
a[(qy * w as i64 + qx + 1) as usize] = v_hi;
cx += step;
}
cy += step;
}
let (ra, rb) = (gray_to_rgba(&a), gray_to_rgba(&b));
let points = select_native_points(&rb, w, h, region, None, None);
assert!(
points.len() >= 12,
"expected many per-cell points, got {}",
points.len()
);
let r = refine_native_at(
&ra,
&rb,
w,
h,
Point {
x: dx as f64,
y: dy as f64,
},
None,
3,
&points,
);
assert_eq!((r.x, r.y), (dx as i32, dy as i32));
assert!(r.error < 1.0, "expected a near-zero error, got {}", r.error);
}
#[test]
fn native_refinement_recovers_a_sparse_shift_via_the_guide() {
let (w, h) = (280usize, 280usize);
let (dx, dy) = (4i64, -3i64);
let (v0, v_lo, v_mid, v_hi) = (100u8, 20u8, 150u8, 220u8);
let region = Rect {
x: 6.0,
y: 6.0,
width: 270.0,
height: 270.0,
};
let step = 3i64.max(((region.width * region.height / 1600.0).sqrt()).floor() as i64);
assert!(
step >= 6,
"test assumes a wide enough cell, got step={step}"
);
let factor = 6usize;
let x_start = 2i64.max(region.x.ceil() as i64);
let y_start = 2i64.max(region.y.ceil() as i64);
let mut a = vec![v0; w * h];
let mut b = vec![v0; w * h];
let mut cy = y_start;
while (cy as f64) < region.y + region.height {
let mut cx = x_start;
while (cx as f64) < region.x + region.width {
let (px, py) = (cx + 3, cy + 3);
b[(py * w as i64 + px - 1) as usize] = v_lo;
b[(py * w as i64 + px) as usize] = v_mid;
b[(py * w as i64 + px + 1) as usize] = v_hi;
let (qx, qy) = (px + dx, py + dy);
a[(qy * w as i64 + qx - 1) as usize] = v_lo;
a[(qy * w as i64 + qx) as usize] = v_mid;
a[(qy * w as i64 + qx + 1) as usize] = v_hi;
cx += step;
}
cy += step;
}
let (ra, rb) = (gray_to_rgba(&a), gray_to_rgba(&b));
let (gw, gh) = crate::kernels::raster::downscaled_size(w, h, factor);
let mut guide_data = vec![0u8; gw * gh];
crate::kernels::raster::downscale_gray(&rb, w, h, factor, &mut guide_data);
let guide = Gray {
width: gw,
height: gh,
data: &guide_data,
};
let points = select_native_points(&rb, w, h, region, None, Some((guide, factor)));
assert!(
points.len() >= 12,
"expected many per-cell points, got {}",
points.len()
);
let r = refine_native_at(
&ra,
&rb,
w,
h,
Point {
x: dx as f64,
y: dy as f64,
},
None,
3,
&points,
);
assert_eq!((r.x, r.y), (dx as i32, dy as i32));
assert!(r.error < 1.0, "expected a near-zero error, got {}", r.error);
}
#[test]
fn native_refinement_guide_at_factor_one_matches_the_unguided_scan() {
let (w, h) = (200usize, 200usize);
let world = texture(w + 200, h + 200, (w * 13 + h) as u32);
let b = crop(&world, w + 200, 100, 100, w, h);
let rb = gray_to_rgba(&b);
let region = Rect {
x: 4.0,
y: 4.0,
width: (w - 20) as f64,
height: (h - 20) as f64,
};
let unguided = select_native_points(&rb, w, h, region, None, None);
let (gw, gh) = crate::kernels::raster::downscaled_size(w, h, 1);
let mut guide_data = vec![0u8; gw * gh];
crate::kernels::raster::downscale_gray(&rb, w, h, 1, &mut guide_data);
let guide = Gray {
width: gw,
height: gh,
data: &guide_data,
};
let guided = select_native_points(&rb, w, h, region, None, Some((guide, 1)));
assert_eq!(unguided, guided);
assert!(
unguided.len() >= 12,
"fixture should be dense enough to compare meaningfully"
);
}
#[test]
fn native_refinement_recovers_a_shift_past_a_screen_fixed_line_on_a_lattice_row() {
let (w, h) = (280usize, 280usize);
let (dx, dy) = (5i64, -4i64);
let (v0, v_lo, v_mid, v_hi, v_bar) = (100u8, 20u8, 150u8, 220u8, 250u8);
let region = Rect {
x: 6.0,
y: 6.0,
width: 270.0,
height: 270.0,
};
let step = 3i64.max(((region.width * region.height / 1600.0).sqrt()).floor() as i64);
assert!(
step >= 6,
"test assumes a wide enough cell, got step={step}"
);
let x_start = 2i64.max(region.x.ceil() as i64);
let y_start = 2i64.max(region.y.ceil() as i64);
let line_y = y_start + 12 * step;
let mut a = vec![v0; w * h];
let mut b = vec![v0; w * h];
let mut cy = y_start;
while cy < line_y {
let mut cx = x_start;
while (cx as f64) < region.x + region.width {
let (px, py) = (cx + 3, cy + 3);
b[(py * w as i64 + px - 1) as usize] = v_lo;
b[(py * w as i64 + px) as usize] = v_mid;
b[(py * w as i64 + px + 1) as usize] = v_hi;
let (qx, qy) = (px + dx, py + dy);
a[(qy * w as i64 + qx - 1) as usize] = v_lo;
a[(qy * w as i64 + qx) as usize] = v_mid;
a[(qy * w as i64 + qx + 1) as usize] = v_hi;
cx += step;
}
cy += step;
}
for y in line_y as usize..h {
for x in 0..w {
let i = y * w + x;
a[i] = v_bar;
b[i] = v_bar;
}
}
let (ra, rb) = (gray_to_rgba(&a), gray_to_rgba(&b));
let points = select_native_points(&rb, w, h, region, None, None);
let r = refine_native_at(
&ra,
&rb,
w,
h,
Point {
x: dx as f64,
y: dy as f64,
},
None,
3,
&points,
);
assert_eq!((r.x, r.y), (dx as i32, dy as i32));
assert!(
r.error < 14.0,
"expected the true shift to clear the odometry gate, got {}",
r.error
);
}
}