use rayon::prelude::*;
pub const MAXZ: usize = 8;
const J: usize = 8;
#[derive(Clone, Debug)]
pub struct MtaParams {
pub tol: f32,
pub window: usize,
pub line_nb: usize,
pub line_window: usize,
pub min_votes: f32,
pub max_ratio: f32,
pub weak_db: f32,
pub window2: usize,
pub line_nb2: usize,
pub min_votes2: f32,
pub window3: usize,
pub line_nb3: usize,
pub bucket: f32,
pub min_density3: f32,
pub ratio3: f32,
pub max_range: f32,
pub zone_scale: f32,
}
impl Default for MtaParams {
fn default() -> Self {
MtaParams {
tol: 1.0,
window: 30,
line_nb: 1,
line_window: 3,
min_votes: 8.0,
max_ratio: 0.25,
weak_db: -15.0,
window2: 60,
line_nb2: 2,
min_votes2: 2.0,
window3: 60,
line_nb3: 4,
bucket: 2.0,
min_density3: 2.0,
ratio3: 4.0,
max_range: 420.0,
zone_scale: 2.0,
}
}
}
#[derive(Clone, Debug, Default)]
pub struct LineEchoes {
pub shot: Vec<u32>,
pub zr: Vec<[f32; MAXZ]>,
pub nzone: usize,
pub reflectance: Vec<f32>,
pub n_shots: usize,
}
#[derive(Clone, Copy, Debug, PartialEq)]
pub struct Evidence {
pub votes_own: [f32; MAXZ],
pub votes_nb: [f32; MAXZ],
pub anchors: [f32; MAXZ],
pub density: [f32; MAXZ],
}
impl Default for Evidence {
fn default() -> Self {
Evidence { votes_own: [-1.0; MAXZ], votes_nb: [-1.0; MAXZ], anchors: [-1.0; MAXZ], density: [-1.0; MAXZ] }
}
}
#[derive(Clone, Copy, Debug, Default, PartialEq)]
pub struct Decision {
pub zone: u8,
pub stage: u8,
pub score: f32,
pub rival: f32,
pub evidence: Evidence,
}
pub struct Grid {
n_shots: usize,
k: usize,
hyp: Vec<f32>,
res: Vec<f32>,
cum: Vec<f32>,
nb: usize,
bucket: f32,
}
impl Grid {
pub fn new(line: &LineEchoes, p: &MtaParams) -> Grid {
let n = line.n_shots.max(line.shot.last().map(|&s| s as usize + 1).unwrap_or(0));
let mut hyp = vec![f32::NAN; (n + 1) * J * MAXZ];
let mut fill = vec![0usize; n + 1];
for (i, &s) in line.shot.iter().enumerate() {
let s = s as usize;
if s >= n || line.reflectance[i] < p.weak_db {
continue;
}
let j = fill[s];
if j >= J {
continue;
}
fill[s] += 1;
let base = (s * J + j) * MAXZ;
hyp[base..base + MAXZ].copy_from_slice(&line.zr[i]);
}
let nb = (p.max_range / p.bucket).ceil() as usize + 2;
Grid { n_shots: n, k: line.nzone.min(MAXZ), hyp, res: vec![f32::NAN; (n + 1) * J], cum: Vec::new(), nb, bucket: p.bucket }
}
pub fn set_anchors(&mut self, line: &LineEchoes, dec: &[Decision], p: &MtaParams) {
let n = self.n_shots;
let mut fill = vec![0usize; n + 1];
let mut heat = vec![0f32; (n + 1) * self.nb];
for (i, d) in dec.iter().enumerate() {
if d.stage == 0 || d.zone == 0 || line.reflectance[i] < p.weak_db {
continue;
}
let s = line.shot[i] as usize;
if s >= n {
continue;
}
let r = line.zr[i][d.zone as usize - 1];
if !r.is_finite() {
continue;
}
let j = fill[s];
if j < J {
fill[s] += 1;
self.res[s * J + j] = r;
}
let b = ((r / self.bucket) as usize).min(self.nb - 1);
heat[s * self.nb + b] += 1.0;
}
let nb = self.nb;
let mut cum = vec![0f32; (n + 2) * nb];
for s in 0..=n {
let (prev, cur) = cum.split_at_mut((s + 1) * nb);
let prev = &prev[s * nb..];
for b in 0..nb {
cur[b] = prev[b] + heat[s * nb + b];
}
}
self.cum = cum;
}
#[inline]
fn hyp_at(&self, s: usize, j: usize) -> &[f32] {
let base = (s * J + j) * MAXZ;
&self.hyp[base..base + MAXZ]
}
fn votes(&self, shot: i64, zr: &[f32; MAXZ], w: usize, excl_self: bool, tol: f32, out: &mut [f32; MAXZ]) {
let k = self.k;
for d in -(w as i64)..=(w as i64) {
if excl_self && d == 0 {
continue;
}
let s2 = shot + d;
if s2 < 0 || s2 >= self.n_shots as i64 {
continue;
}
for j in 0..J {
let c = self.hyp_at(s2 as usize, j);
if !c[0].is_finite() && !c[1].is_finite() {
if c.iter().all(|v| !v.is_finite()) {
break;
}
}
let mut hit = usize::MAX;
let mut nhit = 0usize;
for z in 0..k {
if (c[z] - zr[z]).abs() < tol {
nhit += 1;
hit = z;
}
}
if nhit == 1 {
out[hit] += 1.0;
}
}
}
}
fn anchor_votes(&self, shot: i64, zr: &[f32; MAXZ], w: usize, excl_self: bool, tol: f32, out: &mut [f32; MAXZ]) {
let k = self.k;
for d in -(w as i64)..=(w as i64) {
if excl_self && d == 0 {
continue;
}
let s2 = shot + d;
if s2 < 0 || s2 >= self.n_shots as i64 {
continue;
}
let row = &self.res[s2 as usize * J..(s2 as usize + 1) * J];
for &a in row {
if !a.is_finite() {
break;
}
for z in 0..k {
if (a - zr[z]).abs() < tol {
out[z] += 1.0;
}
}
}
}
}
fn heat(&self, shot: i64, w: usize, acc: &mut [f32]) {
if self.cum.is_empty() {
return;
}
let n = self.n_shots as i64;
let lo = (shot - w as i64).clamp(0, n) as usize;
let hi = (shot + w as i64 + 1).clamp(0, n) as usize;
let nb = self.nb;
let (a, b) = (&self.cum[lo * nb..(lo + 1) * nb], &self.cum[hi * nb..(hi + 1) * nb]);
for i in 0..nb {
acc[i] += b[i] - a[i];
}
}
}
#[inline]
fn zone_factor(p: &MtaParams, zone: usize) -> f32 {
p.zone_scale.powi((zone as i32 - 2).max(0))
}
fn best_two(s: &[f32; MAXZ], k: usize) -> (usize, f32, f32) {
let mut bi = 0;
let mut b1 = f32::NEG_INFINITY;
let mut b2 = f32::NEG_INFINITY;
for (z, &v) in s.iter().enumerate().take(k) {
if v > b1 {
b2 = b1;
b1 = v;
bi = z;
} else if v > b2 {
b2 = v;
}
}
(bi, b1, b2.max(0.0))
}
pub fn stage1(grids: &[Option<&Grid>], line: &LineEchoes, p: &MtaParams) -> Vec<Decision> {
let c = p.line_nb;
let own = grids[c].expect("centre grid");
let k = line.nzone.min(MAXZ);
(0..line.shot.len())
.into_par_iter()
.map(|i| {
let zr = &line.zr[i];
let shot = line.shot[i] as i64;
let mut so = [0f32; MAXZ];
let mut sn = [0f32; MAXZ];
own.votes(shot, zr, p.window, true, p.tol, &mut so);
for (gi, g) in grids.iter().enumerate() {
if gi == c {
continue;
}
if let Some(g) = g {
g.votes(shot, zr, p.line_window, false, p.tol, &mut sn);
}
}
let mut s = [0f32; MAXZ];
for z in 0..MAXZ {
if z < k && zr[z].is_finite() {
s[z] = so[z] + sn[z];
} else {
s[z] = -1.0;
so[z] = -1.0;
sn[z] = -1.0;
}
}
let evidence = Evidence { votes_own: so, votes_nb: sn, ..Default::default() };
let (bi, b1, b2) = best_two(&s, k);
let zone = if b1 < 0.0 { 0 } else { bi as u8 + 1 };
if b1 >= p.min_votes * zone_factor(p, bi + 1) && b2 <= p.max_ratio * b1 {
Decision { zone, stage: 1, score: b1, rival: b2, evidence }
} else {
Decision { zone, stage: 0, score: b1.max(0.0), rival: b2, evidence }
}
})
.collect()
}
pub fn stage2(grids: &[Option<&Grid>], line: &LineEchoes, dec: &mut [Decision], p: &MtaParams) {
let c = p.line_nb2;
let own = grids[c].expect("centre grid");
let k = line.nzone.min(MAXZ);
dec.par_iter_mut().enumerate().for_each(|(i, d)| {
if d.zone == 0 {
return;
}
let decided = d.stage != 0;
let zr = &line.zr[i];
let shot = line.shot[i] as i64;
let mut s = [0f32; MAXZ];
own.anchor_votes(shot, zr, p.window2, true, p.tol, &mut s);
for (gi, g) in grids.iter().enumerate() {
if gi == c {
continue;
}
if let Some(g) = g {
g.anchor_votes(shot, zr, p.line_window, false, p.tol, &mut s);
}
}
for (z, v) in s.iter_mut().enumerate() {
if z >= k || !zr[z].is_finite() {
*v = -1.0;
}
}
d.evidence.anchors = s;
if decided {
return;
}
let (bi, b1, b2) = best_two(&s, k);
if b1 >= p.min_votes2 * zone_factor(p, bi + 1) && b1 > b2 {
d.zone = bi as u8 + 1;
d.stage = 2;
d.score = b1;
d.rival = b2;
}
});
}
pub fn stage3(grids: &[Option<&Grid>], line: &LineEchoes, dec: &mut [Decision], p: &MtaParams) {
let c = p.line_nb3;
let own = grids[c].expect("centre grid");
let k = line.nzone.min(MAXZ);
let nb = own.nb;
dec.par_iter_mut().enumerate().for_each(|(i, d)| {
if d.zone == 0 {
return;
}
let decided = d.stage != 0;
let zr = &line.zr[i];
let shot = line.shot[i] as i64;
let mut acc = vec![0f32; nb];
for g in grids.iter().flatten() {
g.heat(shot, p.window3, &mut acc);
}
let mut s3 = [-1f32; MAXZ];
for z in 0..k {
let r = zr[z];
if !r.is_finite() {
continue;
}
let b = ((r / p.bucket) as usize).clamp(1, nb - 2);
s3[z] = acc[b] + 0.5 * (acc[b - 1] + acc[b + 1]);
}
d.evidence.density = s3;
if decided {
return;
}
let (bi, b1, b2) = best_two(&s3, k);
let zone = if b1 < 0.0 { d.zone } else { bi as u8 + 1 };
let evidence = d.evidence;
if b1 >= p.min_density3 * zone_factor(p, bi + 1) && b1 >= p.ratio3 * b2 {
*d = Decision { zone, stage: 3, score: b1, rival: b2, evidence };
} else {
*d = Decision { zone, stage: 0, score: b1.max(0.0), rival: b2, evidence };
}
});
}
pub fn stage23(grids: &[Option<&Grid>], line: &LineEchoes, dec: &mut [Decision], p: &MtaParams) {
let c = p.line_nb3;
let lo = c.saturating_sub(p.line_nb2);
let hi = (c + p.line_nb2 + 1).min(grids.len());
let mut g2: Vec<Option<&Grid>> = vec![None; 2 * p.line_nb2 + 1];
for (j, g) in grids[lo..hi].iter().enumerate() {
g2[j + p.line_nb2 - (c - lo)] = *g;
}
stage2(&g2, line, dec, p);
stage3(grids, line, dec, p);
}
#[cfg(test)]
mod tests {
use super::*;
fn synthetic(n_lines: usize, n_shots: usize) -> (Vec<LineEchoes>, Vec<Vec<(u8, bool)>>) {
let code = [58.4f32, 65.1, 71.1, 77.7, 74.7, 68.1, 61.4];
let mut lines = Vec::new();
let mut truth = Vec::new();
for l in 0..n_lines {
let mut tr: Vec<(u8, bool)> = Vec::new();
let iv: Vec<f32> = (0..n_shots).map(|s| code[(s + 3 * l) % 7]).collect();
let mut line = LineEchoes { nzone: 3, n_shots, ..Default::default() };
let push = |s: usize, true_range: f32, true_zone: usize, refl: f32, line: &mut LineEchoes| {
let mut off = 0f32;
for i in 1..true_zone {
off += iv[s - i];
}
let r1 = true_range - off;
let mut zr = [f32::NAN; MAXZ];
let mut o = 0f32;
for z in 0..3 {
if z > 0 {
if s < z {
break;
}
o += iv[s - z];
}
zr[z] = r1 + o;
}
line.shot.push(s as u32);
line.zr.push(zr);
line.reflectance.push(refl);
};
for s in 3..n_shots {
let wall = 20.0 + 0.002 * (s as f32) + 0.01 * l as f32;
push(s, wall, 1, -5.0, &mut line);
tr.push((1, false));
if (100..300).contains(&s) {
push(s, 90.0 + 0.001 * s as f32, 2, -8.0, &mut line);
tr.push((2, false));
}
if s % 97 == 5 {
push(s, 33.0 + s as f32, 1, -20.0, &mut line); tr.push((1, true));
}
}
lines.push(line);
truth.push(tr);
}
(lines, truth)
}
#[test]
fn resolves_two_surfaces_and_flags_isolated_echoes() {
let p = MtaParams::default();
let (lines, truth) = synthetic(3, 400);
let mut grids: Vec<Grid> = lines.iter().map(|l| Grid::new(l, &p)).collect();
let nl = lines.len();
fn neighbours(i: usize, grids: &[Grid]) -> Vec<Option<&Grid>> {
let nl = grids.len();
vec![if i >= 1 { Some(&grids[i - 1]) } else { None }, Some(&grids[i]), if i + 1 < nl { Some(&grids[i + 1]) } else { None }]
}
let decs: Vec<Vec<Decision>> = (0..nl).map(|i| stage1(&neighbours(i, &grids), &lines[i], &p)).collect();
for i in 0..nl {
grids[i].set_anchors(&lines[i], &decs[i], &p);
}
let centre = 1usize;
let mut dec = decs[centre].clone();
let mut g3: Vec<Option<&Grid>> = vec![None; 2 * p.line_nb3 + 1];
for i in 0..nl {
g3[p.line_nb3 + i - centre] = Some(&grids[i]);
}
stage23(&g3, &lines[centre], &mut dec, &p);
let (mut ok, mut n_wall, mut n_far, mut iso_unc, mut n_iso) = (0, 0, 0, 0, 0);
for (i, d) in dec.iter().enumerate() {
let (zone, isolated) = truth[centre][i];
if isolated {
n_iso += 1;
if d.stage == 0 {
iso_unc += 1;
}
} else {
if zone == 1 {
n_wall += 1;
} else {
n_far += 1;
}
if d.stage != 0 && d.zone == zone {
ok += 1;
}
}
}
assert!(n_wall > 390 && n_far > 190, "synthetic set: {n_wall} {n_far}");
assert!(ok as f64 >= 0.99 * (n_wall + n_far) as f64, "resolved {ok} of {}", n_wall + n_far);
assert!(iso_unc >= n_iso - 1, "isolated echoes should be uncertain: {iso_unc}/{n_iso}");
}
#[test]
fn best_two_orders() {
let mut s = [0f32; MAXZ];
s[0] = 3.0;
s[1] = 10.0;
s[2] = 7.0;
assert_eq!(best_two(&s, 3), (1, 10.0, 7.0));
assert_eq!(best_two(&s, 1), (0, 3.0, 0.0));
}
}