use crate::decode::Gray;
use crate::sift::{Features, Keypoint, DESC_LEN};
use crate::timed;
pub type Affine = [f32; 6];
#[derive(Clone, Copy, Debug, Default, PartialEq, Eq)]
pub struct Variant {
pub mirror: bool,
pub invert: bool,
}
#[inline]
pub fn mirror_affine(w: f32) -> Affine {
[-1.0, 0.0, w - 1.0, 0.0, 1.0, 0.0]
}
#[derive(Clone, Copy, Debug)]
pub struct Verdict {
pub m: Affine,
#[allow(dead_code)]
pub variant: Variant,
pub n_in: u32,
pub centred: bool,
pub n_match: u32,
pub ov_a: f32,
pub ov_b: f32,
pub scale: f32,
pub rot_deg: f32,
pub blk: f32,
pub blk_n: u32,
pub ncc: f32,
}
impl Default for Verdict {
fn default() -> Self {
Verdict {
m: [1.0, 0.0, 0.0, 0.0, 1.0, 0.0],
variant: Variant::default(),
n_in: 0,
centred: false,
n_match: 0,
ov_a: 0.0,
ov_b: 0.0,
scale: 0.0,
rot_deg: 0.0,
blk: 0.0,
blk_n: 0,
ncc: 0.0,
}
}
}
#[derive(Clone, Copy, Debug)]
pub struct Rules {
pub min_aligned_points: u32,
pub min_frame_overlap: f32,
pub min_pixel_correlation: f32,
pub max_scale: f32,
pub centred_evidence: bool,
}
#[derive(Clone, Copy, Debug)]
pub struct Policy {
pub anchor: Rules,
pub propagated: Rules,
pub corroborated: Rules,
}
const GRID: usize = 48;
const BLOCK: usize = 8;
const CLUSTER_SLACK_POINTS: u32 = 2;
const CLUSTER_SLACK_CORRELATION: f32 = 0.1;
impl Default for Rules {
fn default() -> Self {
Rules {
min_aligned_points: 8,
min_frame_overlap: 0.85,
min_pixel_correlation: 0.6,
max_scale: 16.0,
centred_evidence: false,
}
}
}
impl Policy {
pub fn new(min_aligned_points: u32, min_frame_overlap: f32, min_pixel_correlation: f32) -> Policy {
let anchor = Rules {
min_aligned_points,
min_frame_overlap,
min_pixel_correlation,
centred_evidence: true,
..Default::default()
};
Policy {
anchor,
propagated: Rules {
min_aligned_points: 0,
centred_evidence: false,
..anchor
},
corroborated: Rules {
min_aligned_points: min_aligned_points.saturating_sub(CLUSTER_SLACK_POINTS),
min_pixel_correlation: (min_pixel_correlation - CLUSTER_SLACK_CORRELATION).max(0.0),
centred_evidence: false,
..anchor
},
}
}
}
impl Verdict {
pub fn accepted(&self, r: &Rules) -> bool {
self.scale.is_finite()
&& self.scale >= 1.0 / r.max_scale
&& self.scale <= r.max_scale
&& self.n_in >= r.min_aligned_points
&& (!r.centred_evidence || self.centred)
&& self.ov_a.max(self.ov_b) >= r.min_frame_overlap
&& self.blk >= r.min_pixel_correlation
}
}
use crate::index::dist2;
pub fn correspond(
a: &Features,
b: &Features,
cands: &[(u32, u32)],
out: &mut Vec<(u32, u32)>,
) {
out.clear();
if cands.is_empty() {
return;
}
let mut i = 0;
while i < cands.len() {
let qi = cands[i].0;
let mut best = (u32::MAX, 0u32);
let da: &[u8; DESC_LEN] = a.d(qi as usize).try_into().unwrap();
while i < cands.len() && cands[i].0 == qi {
let tj = cands[i].1;
prefetch_descriptor(b, cands, i + PREFETCH);
let d = dist2(da, b.d(tj as usize));
if d < best.0 {
best = (d, tj);
}
i += 1;
}
if best.0 != u32::MAX {
out.push((qi, best.1));
}
}
}
const PREFETCH: usize = 8;
#[inline]
fn prefetch_descriptor(b: &Features, cands: &[(u32, u32)], k: usize) {
let Some(&(_, tj)) = cands.get(k) else { return };
let tj = tj as usize;
if tj >= b.kps.len() {
return;
}
#[cfg(target_arch = "x86_64")]
unsafe {
use std::arch::x86_64::_mm_prefetch;
let d = b.desc.as_ptr().add(tj * DESC_LEN);
_mm_prefetch(d as *const i8, std::arch::x86_64::_MM_HINT_T0);
_mm_prefetch(d.add(64) as *const i8, std::arch::x86_64::_MM_HINT_T0);
_mm_prefetch(b.kps.as_ptr().add(tj) as *const i8, std::arch::x86_64::_MM_HINT_T0);
}
#[cfg(not(target_arch = "x86_64"))]
let _ = (b, tj);
}
#[inline]
fn apply(m: &Affine, x: f32, y: f32) -> (f32, f32) {
(m[0] * x + m[1] * y + m[2], m[3] * x + m[4] * y + m[5])
}
fn from_single(ka: &Keypoint, kb: &Keypoint) -> Affine {
let s = kb.sigma / ka.sigma.max(1e-6);
let th = (kb.angle - ka.angle).to_radians();
let (sn, cs) = th.sin_cos();
let (c, sn) = (cs * s, sn * s);
[c, -sn, kb.x - (c * ka.x - sn * ka.y), sn, c, kb.y - (sn * ka.x + c * ka.y)]
}
fn fit_affine(a: &Features, b: &Features, pairs: &[(u32, u32)], mask: &[bool]) -> Option<Affine> {
let (mut sxx, mut sxy, mut sx, mut syy, mut sy, mut n) = (0f64, 0f64, 0f64, 0f64, 0f64, 0f64);
let (mut tx1, mut tx2, mut tx3) = (0f64, 0f64, 0f64);
let (mut ty1, mut ty2, mut ty3) = (0f64, 0f64, 0f64);
for (k, &(i, j)) in pairs.iter().enumerate() {
if !mask[k] {
continue;
}
let (x, y) = (a.kps[i as usize].x as f64, a.kps[i as usize].y as f64);
let (u, v) = (b.kps[j as usize].x as f64, b.kps[j as usize].y as f64);
sxx += x * x;
sxy += x * y;
sx += x;
syy += y * y;
sy += y;
n += 1.0;
tx1 += x * u;
tx2 += y * u;
tx3 += u;
ty1 += x * v;
ty2 += y * v;
ty3 += v;
}
if n < 3.0 {
return None;
}
let g = [[sxx, sxy, sx], [sxy, syy, sy], [sx, sy, n]];
let r1 = solve3(g, [tx1, tx2, tx3])?;
let r2 = solve3(g, [ty1, ty2, ty3])?;
Some([r1[0] as f32, r1[1] as f32, r1[2] as f32, r2[0] as f32, r2[1] as f32, r2[2] as f32])
}
fn solve3(a: [[f64; 3]; 3], b: [f64; 3]) -> Option<[f64; 3]> {
let det = a[0][0] * (a[1][1] * a[2][2] - a[1][2] * a[2][1])
- a[0][1] * (a[1][0] * a[2][2] - a[1][2] * a[2][0])
+ a[0][2] * (a[1][0] * a[2][1] - a[1][1] * a[2][0]);
if det.abs() < 1e-9 {
return None;
}
let inv = 1.0 / det;
let mut out = [0f64; 3];
for i in 0..3 {
let mut m = a;
for r in 0..3 {
m[r][i] = b[r];
}
let d = m[0][0] * (m[1][1] * m[2][2] - m[1][2] * m[2][1])
- m[0][1] * (m[1][0] * m[2][2] - m[1][2] * m[2][0])
+ m[0][2] * (m[1][0] * m[2][1] - m[1][1] * m[2][0]);
out[i] = d * inv;
}
Some(out)
}
pub fn invert_affine(m: &Affine) -> Option<Affine> {
let det = m[0] * m[4] - m[1] * m[3];
if det.abs() < 1e-12 {
return None;
}
let inv = 1.0 / det;
let (a, b, c, d) = (m[4] * inv, -m[1] * inv, -m[3] * inv, m[0] * inv);
Some([a, b, -(a * m[2] + b * m[5]), c, d, -(c * m[2] + d * m[5])])
}
pub fn compose(m1: &Affine, m2: &Affine) -> Affine {
[
m2[0] * m1[0] + m2[1] * m1[3],
m2[0] * m1[1] + m2[1] * m1[4],
m2[0] * m1[2] + m2[1] * m1[5] + m2[2],
m2[3] * m1[0] + m2[4] * m1[3],
m2[3] * m1[1] + m2[4] * m1[4],
m2[3] * m1[2] + m2[4] * m1[5] + m2[5],
]
}
fn best_transform(a: &Features, b: &Features, pairs: &[(u32, u32)], bw: f32, bh: f32, scratch: &mut Scratch) -> Option<(Affine, usize)> {
if pairs.len() < 3 {
return None;
}
let tol = (0.015 * (bw * bw + bh * bh).sqrt()).max(3.0);
let tol2 = tol * tol;
let n = pairs.len();
let sc = &mut *scratch;
sc.ax.clear();
sc.ay.clear();
sc.bx.clear();
sc.by.clear();
for (k, &(p, q)) in pairs.iter().enumerate() {
prefetch_keypoints(a, b, pairs, k + PREFETCH);
let ka = &a.kps[p as usize];
let kb = &b.kps[q as usize];
sc.ax.push(ka.x);
sc.ay.push(ka.y);
sc.bx.push(kb.x);
sc.by.push(kb.y);
}
let (ax, ay, bx, by) = (&sc.ax[..n], &sc.ay[..n], &sc.bx[..n], &sc.by[..n]);
sc.mask.clear();
sc.mask.resize(n, false);
sc.hit.clear();
sc.hit.resize(n, false);
let mut best: Option<(usize, Affine)> = None;
let cap = n.min(600);
for &(i, j) in pairs.iter().take(cap) {
let m = from_single(&a.kps[i as usize], &b.kps[j as usize]);
let s = (m[0] * m[4] - m[1] * m[3]).abs().sqrt();
if !(s.is_finite() && s > 1e-3 && s < 1e3) {
continue;
}
let need = best.map_or(3, |(c, _)| c + 1);
let count = count_inliers(&m, ax, ay, bx, by, tol2, need);
if count >= need {
let marked = mark_inliers(&m, ax, ay, bx, by, tol2, &mut sc.hit);
debug_assert_eq!(marked, count);
best = Some((marked, m));
sc.mask.copy_from_slice(&sc.hit);
}
}
let (n_in, mut m) = best?;
let mut held_after = n_in;
for _ in 0..3 {
let held = sc.mask.iter().filter(|v| **v).count();
let Some(m2) = fit_affine(a, b, pairs, &sc.mask) else { break };
let count = count_inliers(&m2, ax, ay, bx, by, tol2, held);
if count < held {
break;
}
mark_inliers(&m2, ax, ay, bx, by, tol2, &mut sc.hit);
m = m2;
sc.mask.copy_from_slice(&sc.hit);
held_after = count;
}
Some((m, held_after))
}
#[inline]
fn prefetch_keypoints(a: &Features, b: &Features, pairs: &[(u32, u32)], k: usize) {
let Some(&(p, q)) = pairs.get(k) else { return };
#[cfg(target_arch = "x86_64")]
unsafe {
use std::arch::x86_64::{_mm_prefetch, _MM_HINT_T0};
if (p as usize) < a.kps.len() {
_mm_prefetch(a.kps.as_ptr().add(p as usize) as *const i8, _MM_HINT_T0);
}
if (q as usize) < b.kps.len() {
_mm_prefetch(b.kps.as_ptr().add(q as usize) as *const i8, _MM_HINT_T0);
}
}
#[cfg(not(target_arch = "x86_64"))]
let _ = (a, b, p, q);
}
#[inline]
fn count_inliers(m: &Affine, ax: &[f32], ay: &[f32], bx: &[f32], by: &[f32], tol2: f32, need: usize) -> usize {
const CHUNK: usize = 64;
let n = ax.len();
let mut count = 0usize;
let mut k = 0usize;
while k < n {
let end = (k + CHUNK).min(n);
for t in k..end {
let px = m[0] * ax[t] + m[1] * ay[t] + m[2];
let py = m[3] * ax[t] + m[4] * ay[t] + m[5];
let (dx, dy) = (px - bx[t], py - by[t]);
count += (dx * dx + dy * dy < tol2) as usize;
}
k = end;
if count + (n - k) < need {
return count;
}
}
count
}
#[inline]
fn mark_inliers(m: &Affine, ax: &[f32], ay: &[f32], bx: &[f32], by: &[f32], tol2: f32, hit: &mut [bool]) -> usize {
let mut count = 0usize;
for t in 0..ax.len() {
let px = m[0] * ax[t] + m[1] * ay[t] + m[2];
let py = m[3] * ax[t] + m[4] * ay[t] + m[5];
let (dx, dy) = (px - bx[t], py - by[t]);
let ok = dx * dx + dy * dy < tol2;
hit[t] = ok;
count += ok as usize;
}
count
}
fn distinct_inliers(a: &Features, pairs: &[(u32, u32)], mask: &[bool], pts: &mut Vec<(i32, i32)>) -> u32 {
pts.clear();
pts.extend(
pairs
.iter()
.zip(mask)
.filter(|&(_, &m)| m)
.map(|(&(i, _), _)| {
let k = &a.kps[i as usize];
((k.x * 2.0) as i32, (k.y * 2.0) as i32)
}),
);
pts.sort_unstable();
pts.dedup();
pts.len() as u32
}
fn encloses_centre(a: &Features, b: &Features, m: &Affine, pairs: &[(u32, u32)], mask: &[bool]) -> bool {
let (aw, ah) = (a.w as f32, a.h as f32);
let (bw, bh) = (b.w as f32, b.h as f32);
let mut ca = (0f32, 0f32, 0f32);
for k in &a.kps {
let (u, v) = apply(m, k.x, k.y);
if u >= 0.0 && u < bw && v >= 0.0 && v < bh {
ca = (ca.0 + k.x, ca.1 + k.y, ca.2 + 1.0);
}
}
let mut cb = (0f32, 0f32, 0f32);
if let Some(mi) = invert_affine(m) {
for k in &b.kps {
let (u, v) = apply(&mi, k.x, k.y);
if u >= 0.0 && u < aw && v >= 0.0 && v < ah {
cb = (cb.0 + k.x, cb.1 + k.y, cb.2 + 1.0);
}
}
}
let mut ax = (f32::MAX, f32::MIN);
let mut ay = (f32::MAX, f32::MIN);
let mut bx = (f32::MAX, f32::MIN);
let mut by = (f32::MAX, f32::MIN);
for (k, &(i, j)) in pairs.iter().enumerate() {
if !mask[k] {
continue;
}
let (p, q) = (&a.kps[i as usize], &b.kps[j as usize]);
ax = (ax.0.min(p.x), ax.1.max(p.x));
ay = (ay.0.min(p.y), ay.1.max(p.y));
bx = (bx.0.min(q.x), bx.1.max(q.x));
by = (by.0.min(q.y), by.1.max(q.y));
}
let holds = |sp: (f32, f32), c: f32| sp.0 <= c && c <= sp.1;
let side = |c: (f32, f32, f32), x: (f32, f32), y: (f32, f32)| {
c.2 > 0.0 && holds(x, c.0 / c.2) && holds(y, c.1 / c.2)
};
side(ca, ax, ay) || side(cb, bx, by)
}
fn overlap(m: &Affine, aw: f32, ah: f32, bw: f32, bh: f32) -> (f32, f32) {
const N: usize = 16;
let mut inside = 0;
for iy in 0..N {
for ix in 0..N {
let x = (ix as f32 + 0.5) / N as f32 * aw;
let y = (iy as f32 + 0.5) / N as f32 * ah;
let (u, v) = apply(m, x, y);
if u >= 0.0 && u < bw && v >= 0.0 && v < bh {
inside += 1;
}
}
}
let ov_a = inside as f32 / (N * N) as f32;
let Some(mi) = invert_affine(m) else { return (ov_a, 0.0) };
let mut inside = 0;
for iy in 0..N {
for ix in 0..N {
let x = (ix as f32 + 0.5) / N as f32 * bw;
let y = (iy as f32 + 0.5) / N as f32 * bh;
let (u, v) = apply(&mi, x, y);
if u >= 0.0 && u < aw && v >= 0.0 && v < ah {
inside += 1;
}
}
}
(ov_a, inside as f32 / (N * N) as f32)
}
#[derive(Clone, Debug, Default)]
pub struct Thumb {
pub w: u16,
pub h: u16,
pub scale: f32,
pub px: Vec<u8>,
mips: Vec<(u16, u16, Vec<u8>)>,
}
impl Thumb {
pub fn build(g: &Gray, long: usize) -> Thumb {
let long_side = g.w.max(g.h);
let owned;
let t: &Gray = if long == 0 || long_side <= long {
g
} else {
let s = long as f32 / long_side as f32;
let tw = ((g.w as f32 * s).round() as usize).max(1);
let th = ((g.h as f32 * s).round() as usize).max(1);
owned = crate::decode::resize_area(g, tw, th);
&owned
};
let scale = t.w as f32 / g.w as f32;
Thumb::new(
t.w as u16,
t.h as u16,
scale,
t.px.iter().map(|v| (v.clamp(0.0, 1.0) * 255.0 + 0.5) as u8).collect(),
)
}
pub fn new(w: u16, h: u16, scale: f32, px: Vec<u8>) -> Thumb {
let mut mips: Vec<(u16, u16, Vec<u8>)> = Vec::new();
let (mut cw, mut ch) = (w as usize, h as usize);
while cw >= 4 && ch >= 4 {
let (nw, nh) = (cw / 2, ch / 2);
let cur: &[u8] = match mips.last() {
None => &px,
Some((_, _, p)) => p,
};
let mut next = Vec::with_capacity(nw * nh);
for y in 0..nh {
next.extend((0..nw).map(|x| {
let i = 2 * y * cw + 2 * x;
let s = cur[i] as u32
+ cur[i + 1] as u32
+ cur[i + cw] as u32
+ cur[i + cw + 1] as u32;
((s + 2) / 4) as u8
}));
}
mips.push((nw as u16, nh as u16, next));
cw = nw;
ch = nh;
}
Thumb { w, h, scale, px, mips }
}
#[inline]
fn split(v: f32, n: usize) -> (usize, f32) {
let hi = n as f32 - 1.001;
let v = if v > 0.0 {
if v < hi { v } else { hi }
} else {
0.0
};
let i = unsafe { v.to_int_unchecked::<usize>() };
(i, v - i as f32)
}
#[inline]
fn lerp(px: &[u8], w: usize, i: usize, fx: f32, fy: f32) -> f32 {
debug_assert!(i + w + 1 < px.len());
let (p00, p01, p10, p11) = unsafe {
(
*px.get_unchecked(i) as f32,
*px.get_unchecked(i + 1) as f32,
*px.get_unchecked(i + w) as f32,
*px.get_unchecked(i + w + 1) as f32,
)
};
let a = p00 * (1.0 - fx) + p01 * fx;
let b = p10 * (1.0 - fx) + p11 * fx;
a * (1.0 - fy) + b * fy
}
#[inline]
fn tap(px: &[u8], w: usize, h: usize, x: f32, y: f32) -> f32 {
if w < 2 || h < 2 {
return px.first().copied().unwrap_or(0) as f32;
}
let (x0, fx) = Thumb::split(x, w);
let (y0, fy) = Thumb::split(y, h);
Thumb::lerp(px, w, y0 * w + x0, fx, fy)
}
fn lod(&self, footprint: f32) -> Lod<'_> {
let whole = Lod {
lo: (&self.px[..], self.w as usize, self.h as usize, 1.0),
hi: None,
t: 0.0,
};
if !(footprint > 1.0) || self.mips.is_empty() {
return whole;
}
let l = footprint.log2();
let li = (l as usize).min(self.mips.len());
let lo: (&[u8], usize, usize, f32) = if li == 0 {
(&self.px[..], self.w as usize, self.h as usize, 1.0)
} else {
let (w, h, ref p) = self.mips[li - 1];
(&p[..], w as usize, h as usize, 1.0 / (1 << li) as f32)
};
if li >= self.mips.len() {
return Lod { lo, hi: None, t: 0.0 };
}
let (w, h, ref p) = self.mips[li];
Lod {
lo,
hi: Some((&p[..], w as usize, h as usize, 1.0 / (1 << (li + 1)) as f32)),
t: l - li as f32,
}
}
}
struct Lod<'a> {
lo: (&'a [u8], usize, usize, f32),
hi: Option<(&'a [u8], usize, usize, f32)>,
t: f32,
}
impl Lod<'_> {
#[inline]
fn at(&self, x: f32, y: f32) -> f32 {
let (p, w, h, f) = self.lo;
let a = Thumb::tap(p, w, h, x * f, y * f);
match self.hi {
None => a,
Some((p, w, h, f)) => {
let b = Thumb::tap(p, w, h, x * f, y * f);
a + (b - a) * self.t
}
}
}
}
struct AxisTaps {
i: [u32; GRID],
f: [f32; GRID],
}
impl AxisTaps {
fn of(start: f32, span: f32, scale: f32, f: f32, n: usize) -> AxisTaps {
let mut t = AxisTaps { i: [0; GRID], f: [0.0; GRID] };
for k in 0..GRID {
let p = (start + span * k as f32 / (GRID - 1) as f32) * scale * f;
let (i, fr) = Thumb::split(p, n);
t.i[k] = i as u32;
t.f[k] = fr;
}
t
}
}
enum LevelTaps {
Degenerate(f32),
Grid { x: AxisTaps, y: AxisTaps },
}
impl LevelTaps {
fn of(lvl: (&[u8], usize, usize, f32), gx: (f32, f32), gy: (f32, f32), scale: f32) -> LevelTaps {
let (px, w, h, f) = lvl;
if w < 2 || h < 2 {
return LevelTaps::Degenerate(px.first().copied().unwrap_or(0) as f32);
}
LevelTaps::Grid {
x: AxisTaps::of(gx.0, gx.1, scale, f, w),
y: AxisTaps::of(gy.0, gy.1, scale, f, h),
}
}
#[inline]
fn at(&self, px: &[u8], w: usize, ix: usize, iy: usize) -> f32 {
match self {
LevelTaps::Degenerate(v) => *v,
LevelTaps::Grid { x, y } => {
let i = y.i[iy] as usize * w + x.i[ix] as usize;
Thumb::lerp(px, w, i, x.f[ix], y.f[iy])
}
}
}
}
struct GridTaps {
lo: LevelTaps,
hi: Option<LevelTaps>,
}
impl GridTaps {
fn of(l: &Lod, gx: (f32, f32), gy: (f32, f32), scale: f32) -> GridTaps {
GridTaps {
lo: LevelTaps::of(l.lo, gx, gy, scale),
hi: l.hi.map(|h| LevelTaps::of(h, gx, gy, scale)),
}
}
#[inline]
fn at(&self, l: &Lod, ix: usize, iy: usize) -> f32 {
let a = self.lo.at(l.lo.0, l.lo.1, ix, iy);
match (&self.hi, l.hi) {
(Some(t), Some(h)) => {
let b = t.at(h.0, h.1, ix, iy);
a + (b - a) * l.t
}
_ => a,
}
}
}
fn pixel_check(
ta: &Thumb,
tb: &Thumb,
m: &Affine,
aw: f32,
ah: f32,
bw: f32,
bh: f32,
invert: bool,
) -> (f32, u32, f32) {
const P: usize = 24;
let mut pax = [0f32; P];
let mut pux = [0f32; P];
let mut pvx = [0f32; P];
for ix in 0..P {
let x = (ix as f32 + 0.5) / P as f32 * aw;
pax[ix] = x;
pux[ix] = m[0] * x;
pvx[ix] = m[3] * x;
}
let (mut x0, mut y0, mut x1, mut y1) = (f32::MAX, f32::MAX, f32::MIN, f32::MIN);
for iy in 0..P {
let y = (iy as f32 + 0.5) / P as f32 * ah;
let (uy, vy) = (m[1] * y, m[4] * y);
for ix in 0..P {
let (u, v) = (pux[ix] + uy + m[2], pvx[ix] + vy + m[5]);
if u >= 0.0 && u < bw && v >= 0.0 && v < bh {
let x = pax[ix];
x0 = x0.min(x);
y0 = y0.min(y);
x1 = x1.max(x);
y1 = y1.max(y);
}
}
}
if !(x1 > x0 && y1 > y0) {
return (0.0, 0, 0.0);
}
let mut va = [0f32; GRID * GRID];
let mut vb = [0f32; GRID * GRID];
let mut ok = [false; GRID * GRID];
let step_a = ((x1 - x0) / (GRID - 1) as f32).max((y1 - y0) / (GRID - 1) as f32);
let lin = (m[0] * m[4] - m[1] * m[3]).abs().sqrt();
let raw_a = (step_a * ta.scale).max(1e-6);
let raw_b = (step_a * lin * tb.scale).max(1e-6);
let lod_a = ta.lod(raw_a);
let lod_b = tb.lod(raw_b);
let grid_a = GridTaps::of(&lod_a, (x0, x1 - x0), (y0, y1 - y0), ta.scale);
let mut gux = [0f32; GRID];
let mut gvx = [0f32; GRID];
for ix in 0..GRID {
let x = x0 + (x1 - x0) * ix as f32 / (GRID - 1) as f32;
gux[ix] = m[0] * x;
gvx[ix] = m[3] * x;
}
for iy in 0..GRID {
let y = y0 + (y1 - y0) * iy as f32 / (GRID - 1) as f32;
let (uy, vy) = (m[1] * y, m[4] * y);
for ix in 0..GRID {
let (u, v) = (gux[ix] + uy + m[2], gvx[ix] + vy + m[5]);
if u < 0.0 || u >= bw || v < 0.0 || v >= bh {
continue;
}
let k = iy * GRID + ix;
let s = grid_a.at(&lod_a, ix, iy);
va[k] = if invert { 255.0 - s } else { s };
vb[k] = lod_b.at(u * tb.scale, v * tb.scale);
ok[k] = true;
}
}
let mut agree = 0f32;
let mut total = 0u32;
let (mut ta, mut tb_, mut taa, mut tbb, mut tab, mut tn) = (0f64, 0f64, 0f64, 0f64, 0f64, 0f64);
for by in (0..GRID).step_by(BLOCK) {
for bx in (0..GRID).step_by(BLOCK) {
let mut n = 0usize;
let (mut sa, mut sb, mut saa, mut sbb, mut sab) = (0f64, 0f64, 0f64, 0f64, 0f64);
for y in by..by + BLOCK {
for x in bx..bx + BLOCK {
let k = y * GRID + x;
if !ok[k] {
continue;
}
n += 1;
let (p, q) = (va[k] as f64, vb[k] as f64);
sa += p;
sb += q;
saa += p * p;
sbb += q * q;
sab += p * q;
}
}
ta += sa;
tb_ += sb;
taa += saa;
tbb += sbb;
tab += sab;
tn += n as f64;
if n < BLOCK * BLOCK {
continue;
}
let nf = n as f64;
let vara = saa - sa * sa / nf;
let varb = sbb - sb * sb / nf;
let flat = 4.0 * 4.0 * nf;
if vara < flat || varb < flat {
continue;
}
total += 1;
let cov = sab - sa * sb / nf;
agree += (cov / (vara * varb).sqrt()).abs() as f32;
}
}
let (sa, sb, saa, sbb, sab, n) = (ta, tb_, taa, tbb, tab, tn);
let ncc = if n > 8.0 {
let vara = saa - sa * sa / n;
let varb = sbb - sb * sb / n;
if vara > 0.0 && varb > 0.0 {
((sab - sa * sb / n) / (vara * varb).sqrt()) as f32
} else {
0.0
}
} else {
0.0
};
(if total > 0 { agree / total as f32 } else { 0.0 }, total, ncc)
}
#[derive(Default)]
pub struct Scratch {
ax: Vec<f32>,
ay: Vec<f32>,
bx: Vec<f32>,
by: Vec<f32>,
mask: Vec<bool>,
hit: Vec<bool>,
pts: Vec<(i32, i32)>,
}
pub struct Pair<'a> {
pub fa: &'a Features,
pub fb: &'a Features,
pub ta: &'a Thumb,
pub tb: &'a Thumb,
}
pub fn verify(p: &Pair, cands: &[(u32, u32)], var: Variant, gate: (u32, f32), matches: &mut Vec<(u32, u32)>, scratch: &mut Scratch) -> Verdict {
let mut v = Verdict { variant: var, ..Default::default() };
timed!(16, correspond(p.fa, p.fb, cands, matches));
v.n_match = matches.len() as u32;
let (bw, bh) = (p.fb.w as f32, p.fb.h as f32);
let (aw, ah) = (p.fa.w as f32, p.fa.h as f32);
let Some((m, n_agreeing)) = timed!(17, best_transform(p.fa, p.fb, matches, bw, bh, scratch)) else { return v };
let m_query = m;
let m = if var.mirror { compose(&mirror_affine(aw), &m) } else { m };
v.m = m;
v.scale = (m[0] * m[4] - m[1] * m[3]).abs().sqrt();
v.rot_deg = m[3].atan2(m[0]).to_degrees();
if n_agreeing < gate.0.max(3) as usize {
return v;
}
v.n_in = distinct_inliers(p.fa, matches, &scratch.mask, &mut scratch.pts);
if v.n_in < gate.0.max(3) {
return v;
}
v.centred = timed!(18, encloses_centre(p.fa, p.fb, &m_query, matches, &scratch.mask));
let (oa, ob) = timed!(19, overlap(&m, aw, ah, bw, bh));
v.ov_a = oa;
v.ov_b = ob;
if v.ov_a.max(v.ov_b) >= gate.1 {
let (blk, n, ncc) = timed!(20, pixel_check(p.ta, p.tb, &m, aw, ah, bw, bh, var.invert));
v.blk = blk;
v.blk_n = n;
v.ncc = ncc;
}
v
}
pub fn verify_transform(p: &Pair, m: &Affine, var: Variant, min_ov: f32) -> Verdict {
let mut v = Verdict { m: *m, variant: var, ..Default::default() };
let (aw, ah) = (p.fa.w as f32, p.fa.h as f32);
let (bw, bh) = (p.fb.w as f32, p.fb.h as f32);
v.scale = (m[0] * m[4] - m[1] * m[3]).abs().sqrt();
if !(v.scale.is_finite() && v.scale > 1e-3) {
return v;
}
v.rot_deg = m[3].atan2(m[0]).to_degrees();
let (oa, ob) = overlap(m, aw, ah, bw, bh);
v.ov_a = oa;
v.ov_b = ob;
if v.ov_a.max(v.ov_b) >= min_ov {
let (blk, n, ncc) = timed!(43, pixel_check(p.ta, p.tb, m, aw, ah, bw, bh, var.invert));
v.blk = blk;
v.blk_n = n;
v.ncc = ncc;
}
v
}
#[cfg(test)]
mod bench {
use super::*;
use crate::sift::DESC_LEN;
struct Lcg(u64);
impl Lcg {
fn next(&mut self) -> u64 {
self.0 = self.0.wrapping_mul(6364136223846793005).wrapping_add(1442695040888963407);
self.0
}
fn byte(&mut self) -> u8 {
(self.next() >> 33) as u8
}
fn f(&mut self, hi: f32) -> f32 {
(self.next() >> 40) as f32 / (1 << 24) as f32 * hi
}
}
fn feats(rng: &mut Lcg, n: usize, w: u32, h: u32) -> Features {
let mut f = Features { w, h, kps: Vec::with_capacity(n), desc: Vec::with_capacity(n * DESC_LEN) };
for _ in 0..n {
f.kps.push(Keypoint {
x: rng.f(w as f32),
y: rng.f(h as f32),
sigma: 1.6 + rng.f(20.0),
angle: rng.f(360.0),
response: rng.f(1.0),
});
for _ in 0..DESC_LEN {
f.desc.push(rng.byte());
}
}
f
}
fn ms(mut f: impl FnMut()) -> f64 {
let mut best = f64::MAX;
for _ in 0..7 {
let t = std::time::Instant::now();
f();
best = best.min(t.elapsed().as_secs_f64() * 1e3);
}
best
}
#[test]
#[ignore]
fn match_timings() {
let mut rng = Lcg(0x2545_f491_4f6c_dd1d);
const POOL: usize = 96;
let imgs: Vec<Features> = (0..POOL).map(|_| feats(&mut rng, 600, 640, 480)).collect();
for &n_cand in &[16usize, 56, 200] {
let mut sets: Vec<Vec<(u32, u32)>> = Vec::new();
for _ in 0..POOL {
let mut v: Vec<(u32, u32)> = (0..n_cand)
.map(|_| ((rng.next() % 600) as u32, (rng.next() % 600) as u32))
.collect();
v.sort_unstable();
v.dedup();
sets.push(v);
}
let mut matches = Vec::new();
let mut scratch = Scratch::default();
let t_corr = ms(|| {
for k in 0..POOL {
correspond(&imgs[k], &imgs[(k + 1) % POOL], &sets[k], &mut matches);
std::hint::black_box(&matches);
}
});
let mut ms_sets: Vec<Vec<(u32, u32)>> = Vec::new();
for k in 0..POOL {
correspond(&imgs[k], &imgs[(k + 1) % POOL], &sets[k], &mut matches);
ms_sets.push(matches.clone());
}
let n_match: usize = ms_sets.iter().map(|m| m.len()).sum::<usize>() / POOL;
let t_geom = ms(|| {
for k in 0..POOL {
let r = best_transform(&imgs[k], &imgs[(k + 1) % POOL], &ms_sets[k], 640.0, 480.0, &mut scratch);
std::hint::black_box(&r);
}
});
let m = [1.01f32, 0.02, 3.0, -0.02, 1.01, -4.0];
let mask = vec![true; ms_sets[0].len()];
let t_encl = ms(|| {
for k in 0..POOL {
let mask = &mask[..ms_sets[k].len().min(mask.len())];
std::hint::black_box(encloses_centre(&imgs[k], &imgs[(k + 1) % POOL], &m, &ms_sets[k][..mask.len()], mask));
}
});
println!(
"{n_cand:4} candidates -> {n_match:3} matches: correspond {:7.1} us, best_transform {:7.1} us, encloses {:7.1} us (per call)",
t_corr * 1000.0 / POOL as f64,
t_geom * 1000.0 / POOL as f64,
t_encl * 1000.0 / POOL as f64
);
}
}
}