pub(crate) mod weights;
use crate::Result;
use crate::common::PCoords;
use rand::SeedableRng;
use rand::distr::{Distribution, Uniform};
use rand::prelude::StdRng;
use weights::MagsacWeight;
pub trait ConsensusModel<const D: usize>: Sized {
type Point: PCoords<D> + Copy;
const SAMPLE_SIZE: usize;
const RESIDUAL_DOF: usize = D;
fn from_sample(sample: &[Self::Point]) -> Option<Self>;
fn residual(&self, point: &Self::Point) -> f64;
fn refine_weighted(points: &[Self::Point], weights: &[f64], initial: &Self) -> Option<Self>;
}
#[derive(Debug, Clone)]
pub struct Magsac {
pub sigma_max: f64,
pub max_iterations: Option<usize>,
pub refinement_steps: usize,
pub confidence: f64,
pub seed: Option<u64>,
}
impl Magsac {
pub fn new(sigma_max: f64) -> Self {
Self {
sigma_max,
max_iterations: None,
refinement_steps: 4,
confidence: 0.99,
seed: None,
}
}
pub fn fit<const D: usize, M: ConsensusModel<D>>(
&self,
points: &[M::Point],
) -> Result<ConsensusFit<M>> {
self.fit_filtered(points, |_| true)
}
pub fn fit_filtered<const D: usize, M, F>(
&self,
points: &[M::Point],
accept: F,
) -> Result<ConsensusFit<M>>
where
M: ConsensusModel<D>,
F: Fn(&M) -> bool,
{
if self.sigma_max <= 0.0 || !self.sigma_max.is_finite() {
return Err("sigma_max must be a positive, finite value".into());
}
let n = points.len();
if n < M::SAMPLE_SIZE {
return Err("Not enough points to draw a minimal sample".into());
}
let weighting = MagsacWeight::new(self.sigma_max, M::RESIDUAL_DOF);
let cutoff = weighting.cutoff();
let seed = self.seed.unwrap_or_else(rand::random);
let mut rng = StdRng::seed_from_u64(seed);
let sampler = Uniform::new(0, n)?;
let hard_cap = self.max_iterations.unwrap_or(500).max(1);
let mut dynamic_cap = hard_cap;
let mut sample = Vec::with_capacity(M::SAMPLE_SIZE);
let mut weight_buf = vec![0.0; n];
let mut best: Option<(M, f64)> = None;
let mut iter = 0;
while iter < hard_cap.min(dynamic_cap) {
iter += 1;
if !draw_sample(points, M::SAMPLE_SIZE, &sampler, &mut rng, &mut sample) {
continue;
}
let Some(mut model) = M::from_sample(&sample) else {
continue;
};
if !accept(&model) {
continue;
}
refine(
&mut model,
points,
&weighting,
&mut weight_buf,
self.refinement_steps,
&accept,
);
let score = fill_weights(&model, points, &weighting, &mut weight_buf);
let improved = match &best {
None => true,
Some((_, best_score)) => score > *best_score,
};
if improved {
let inliers = count_inliers(&model, points, cutoff);
let ratio = inliers as f64 / n as f64;
dynamic_cap = required_iterations(ratio, M::SAMPLE_SIZE, self.confidence);
best = Some((model, score));
}
}
let (mut model, _) = best.ok_or("Consensus estimation failed to find any valid model")?;
refine(
&mut model,
points,
&weighting,
&mut weight_buf,
self.refinement_steps,
&accept,
);
let score = fill_weights(&model, points, &weighting, &mut weight_buf);
let inliers = (0..n)
.filter(|&i| model.residual(&points[i]).abs() < cutoff)
.collect();
Ok(ConsensusFit {
model,
inliers,
score,
})
}
}
#[derive(Debug, Clone)]
pub struct ConsensusFit<M> {
pub model: M,
pub inliers: Vec<usize>,
pub score: f64,
}
fn draw_sample<P: Copy>(
points: &[P],
size: usize,
sampler: &Uniform<usize>,
rng: &mut StdRng,
out: &mut Vec<P>,
) -> bool {
out.clear();
let mut chosen: [usize; 8] = [usize::MAX; 8];
let mut count = 0;
let mut attempts = 0;
while count < size {
let i = sampler.sample(rng);
if !chosen[..count].contains(&i) {
chosen[count] = i;
out.push(points[i]);
count += 1;
}
attempts += 1;
if attempts > size * 32 {
return false;
}
}
true
}
fn refine<const D: usize, M: ConsensusModel<D>>(
model: &mut M,
points: &[M::Point],
weighting: &MagsacWeight,
weight_buf: &mut [f64],
steps: usize,
accept: &impl Fn(&M) -> bool,
) {
for _ in 0..steps {
fill_weights(model, points, weighting, weight_buf);
match M::refine_weighted(points, weight_buf, model) {
Some(next) if accept(&next) => *model = next,
_ => break,
}
}
}
fn fill_weights<const D: usize, M: ConsensusModel<D>>(
model: &M,
points: &[M::Point],
weighting: &MagsacWeight,
weight_buf: &mut [f64],
) -> f64 {
let mut score = 0.0;
for (i, p) in points.iter().enumerate() {
let r = model.residual(p).abs();
let w = weighting.weight(r);
weight_buf[i] = w;
score += w;
}
score
}
fn count_inliers<const D: usize, M: ConsensusModel<D>>(
model: &M,
points: &[M::Point],
cutoff: f64,
) -> usize {
points
.iter()
.filter(|p| model.residual(p).abs() < cutoff)
.count()
}
fn required_iterations(inlier_ratio: f64, sample_size: usize, confidence: f64) -> usize {
if inlier_ratio <= 0.0 {
return usize::MAX;
}
let w_m = inlier_ratio.powi(sample_size as i32);
if w_m >= 1.0 {
return 1;
}
let num = (1.0 - confidence).ln();
let den = (1.0 - w_m).ln();
(num / den).ceil().max(1.0) as usize
}