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,
}
}
}
#[cfg(target_feature = "avx2")]
mod wide {
use super::*;
use std::arch::x86_64::*;
const L: usize = 8;
const _: () = assert!(GRID % L == 0);
pub(super) fn fits(ga: &GridTaps, la: &Lod, lb: &Lod) -> bool {
let level = |l: (&[u8], usize, usize, f32)| l.1 >= 2 && l.2 >= 2 && l.0.len() >= l.1 * l.2;
let grid = |t: &LevelTaps| matches!(t, LevelTaps::Grid { .. });
level(la.lo)
&& grid(&ga.lo)
&& match (la.hi, &ga.hi) {
(None, None) => true,
(Some(h), Some(t)) => level(h) && grid(t),
_ => false,
}
&& level(lb.lo)
&& lb.hi.is_none_or(level)
}
#[inline(always)]
unsafe fn split(v: __m256, n: usize) -> (__m256i, __m256) {
unsafe {
let zero = _mm256_setzero_ps();
let hi = _mm256_set1_ps(n as f32 - 1.001);
let below = _mm256_cmp_ps::<_CMP_LT_OQ>(v, hi);
let above0 = _mm256_cmp_ps::<_CMP_GT_OQ>(v, zero);
let c = _mm256_blendv_ps(zero, _mm256_blendv_ps(hi, v, below), above0);
let i = _mm256_cvttps_epi32(c);
(i, _mm256_sub_ps(c, _mm256_cvtepi32_ps(i)))
}
}
#[inline(always)]
unsafe fn lerp(px: &[u8], w: usize, i: __m256i, fx: __m256, fy: __m256) -> __m256 {
unsafe {
let base = px.as_ptr() as *const i32;
let top = _mm256_i32gather_epi32::<1>(base, i);
let bot = _mm256_i32gather_epi32::<1>(base, _mm256_add_epi32(i, _mm256_set1_epi32(w as i32 - 2)));
let m = _mm256_set1_epi32(0xff);
let p00 = _mm256_cvtepi32_ps(_mm256_and_si256(top, m));
let p01 = _mm256_cvtepi32_ps(_mm256_and_si256(_mm256_srli_epi32::<8>(top), m));
let p10 = _mm256_cvtepi32_ps(_mm256_and_si256(_mm256_srli_epi32::<16>(bot), m));
let p11 = _mm256_cvtepi32_ps(_mm256_srli_epi32::<24>(bot));
let one = _mm256_set1_ps(1.0);
let gx = _mm256_sub_ps(one, fx);
let a = _mm256_add_ps(_mm256_mul_ps(p00, gx), _mm256_mul_ps(p01, fx));
let b = _mm256_add_ps(_mm256_mul_ps(p10, gx), _mm256_mul_ps(p11, fx));
_mm256_add_ps(_mm256_mul_ps(a, _mm256_sub_ps(one, fy)), _mm256_mul_ps(b, fy))
}
}
#[inline(always)]
unsafe fn tap(l: (&[u8], usize, usize, f32), x: __m256, y: __m256) -> __m256 {
unsafe {
let f = _mm256_set1_ps(l.3);
let (x0, fx) = split(_mm256_mul_ps(x, f), l.1);
let (y0, fy) = split(_mm256_mul_ps(y, f), l.2);
let i = _mm256_add_epi32(_mm256_mullo_epi32(y0, _mm256_set1_epi32(l.1 as i32)), x0);
lerp(l.0, l.1, i, fx, fy)
}
}
#[inline(always)]
unsafe fn grid_tap(t: &LevelTaps, px: &[u8], w: usize, ix: usize, iy: usize) -> __m256 {
let LevelTaps::Grid { x, y } = t else { unreachable!("`fits` admits only grids") };
unsafe {
let xi = _mm256_loadu_si256(x.i.as_ptr().add(ix) as *const __m256i);
let fx = _mm256_loadu_ps(x.f.as_ptr().add(ix));
let row = _mm256_set1_epi32((y.i[iy] as usize * w) as i32);
lerp(px, w, _mm256_add_epi32(row, xi), fx, _mm256_set1_ps(y.f[iy]))
}
}
#[inline(always)]
unsafe fn blend(a: __m256, b: __m256, t: f32) -> __m256 {
unsafe { _mm256_add_ps(a, _mm256_mul_ps(_mm256_sub_ps(b, a), _mm256_set1_ps(t))) }
}
#[allow(clippy::too_many_arguments)]
pub(super) unsafe fn sample(
ga: &GridTaps,
la: &Lod,
lb: &Lod,
b_scale: f32,
m: &Affine,
gux: &[f32; GRID],
gvx: &[f32; GRID],
rows: &[f32; GRID],
(bw, bh): (f32, f32),
invert: bool,
va: &mut [f32; GRID * GRID],
vb: &mut [f32; GRID * GRID],
ok: &mut [bool; GRID * GRID],
) {
unsafe {
let zero = _mm256_setzero_ps();
let (vbw, vbh) = (_mm256_set1_ps(bw), _mm256_set1_ps(bh));
let (m2, m5) = (_mm256_set1_ps(m[2]), _mm256_set1_ps(m[5]));
let bs = _mm256_set1_ps(b_scale);
let c255 = _mm256_set1_ps(255.0);
for (iy, &y) in rows.iter().enumerate() {
let (uy, vy) = (_mm256_set1_ps(m[1] * y), _mm256_set1_ps(m[4] * y));
for ix in (0..GRID).step_by(L) {
let u = _mm256_add_ps(_mm256_add_ps(_mm256_loadu_ps(gux.as_ptr().add(ix)), uy), m2);
let v = _mm256_add_ps(_mm256_add_ps(_mm256_loadu_ps(gvx.as_ptr().add(ix)), vy), m5);
let out = _mm256_or_ps(
_mm256_or_ps(_mm256_cmp_ps::<_CMP_LT_OQ>(u, zero), _mm256_cmp_ps::<_CMP_GE_OQ>(u, vbw)),
_mm256_or_ps(_mm256_cmp_ps::<_CMP_LT_OQ>(v, zero), _mm256_cmp_ps::<_CMP_GE_OQ>(v, vbh)),
);
let outside = _mm256_movemask_ps(out) as u32;
if outside == 0xff {
continue;
}
let mut s = grid_tap(&ga.lo, la.lo.0, la.lo.1, ix, iy);
if let (Some(t), Some(h)) = (&ga.hi, la.hi) {
s = blend(s, grid_tap(t, h.0, h.1, ix, iy), la.t);
}
if invert {
s = _mm256_sub_ps(c255, s);
}
let (x, y) = (_mm256_mul_ps(u, bs), _mm256_mul_ps(v, bs));
let mut r = tap(lb.lo, x, y);
if let Some(h) = lb.hi {
r = blend(r, tap(h, x, y), lb.t);
}
let k = iy * GRID + ix;
_mm256_storeu_ps(va.as_mut_ptr().add(k), _mm256_andnot_ps(out, s));
_mm256_storeu_ps(vb.as_mut_ptr().add(k), _mm256_andnot_ps(out, r));
for lane in 0..L {
ok[k + lane] = outside >> lane & 1 == 0;
}
}
}
}
}
}
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;
}
#[allow(unused_mut)]
let mut sampled = false;
#[cfg(target_feature = "avx2")]
if wide::fits(&grid_a, &lod_a, &lod_b) {
let rows: [f32; GRID] = std::array::from_fn(|iy| y0 + (y1 - y0) * iy as f32 / (GRID - 1) as f32);
unsafe { wide::sample(&grid_a, &lod_a, &lod_b, tb.scale, m, &gux, &gvx, &rows, (bw, bh), invert, &mut va, &mut vb, &mut ok) };
sampled = true;
}
if !sampled {
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
);
}
}
#[test]
#[ignore]
fn pixel_timings() {
let mut rng = Lcg(0x6a09_e667_f3bc_c908);
let thumb = |rng: &mut Lcg, w: usize, h: usize| {
let (a, b, c) = (rng.f(0.2) + 0.02, rng.f(0.2) + 0.02, rng.f(6.0));
let px: Vec<u8> = (0..w * h)
.map(|i| {
let (x, y) = ((i % w) as f32, (i / w) as f32);
let v = 128.0 + 90.0 * (a * x + c).sin() * (b * y).cos() + (rng.byte() % 24) as f32;
v.clamp(0.0, 255.0) as u8
})
.collect();
Thumb::new(w as u16, h as u16, w as f32 / 384.0, px)
};
const POOL: usize = 64;
let ta: Vec<Thumb> = (0..POOL).map(|_| thumb(&mut rng, 128, 96)).collect();
let tb: Vec<Thumb> = (0..POOL).map(|_| thumb(&mut rng, 128, 85)).collect();
let (aw, ah, bw, bh) = (384.0f32, 288.0f32, 384.0f32, 255.0f32);
let cases: [(&str, Affine); 4] = [
("near-identity", [1.01, 0.01, 2.0, -0.01, 0.99, -3.0]),
("third, inside", [0.33, 0.0, 120.0, 0.0, 0.33, 60.0]),
("rotated 30", [0.75, -0.43, 150.0, 0.43, 0.75, -40.0]),
("enlarged", [2.4, 0.1, -200.0, -0.1, 2.4, -150.0]),
];
let mut sum = 0u64;
for (name, m) in cases.iter() {
for inv in [false, true] {
for k in 0..POOL {
let (b, n, c) = pixel_check(&ta[k], &tb[k], m, aw, ah, bw, bh, inv);
sum = sum.wrapping_mul(0x100000001b3).wrapping_add(b.to_bits() as u64 ^ (n as u64) << 32 ^ c.to_bits() as u64);
}
}
let t = ms(|| {
for k in 0..POOL {
std::hint::black_box(pixel_check(&ta[k], &tb[k], m, aw, ah, bw, bh, false));
}
});
println!("pixel_check {name:14}: {:7.2} us per call", t * 1000.0 / POOL as f64);
}
println!("pixel_check checksum {sum:016x}");
}
}