inlier 0.1.0

Robust model fitting primitives for RANSAC-based pipelines in Rust.
Documentation 
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111

//! Adaptive Reordering (AR) sampler.

use crate::core::Sampler;
use crate::types::DataMatrix;
use rand::Rng;
use rand::SeedableRng;

/// Adaptive Reordering (AR) sampler.
///
/// This is a simplified but behaviorally similar port of the C++ sampler in
/// `adaptive_reordering_sampler.h`. It maintains a priority queue of points
/// ordered by their current inlier probability estimate and always samples
/// the highest-probability points first, while slightly randomizing updates.
pub struct AdaptiveReorderingSampler {
    /// (p, index, appearance_count, a, b)
    probabilities: Vec<(f64, usize, usize, f64, f64)>,
    /// Max-heap by current probability `p`.
    queue: std::collections::BinaryHeap<(ordered_float::OrderedFloat<f64>, usize)>,
    /// Randomness parameter (see C++ implementation).
    randomness: f64,
    /// RNG for the small random perturbation.
    rng: rand::rngs::StdRng,
}

impl AdaptiveReorderingSampler {
    /// Initialize from inlier probability estimates and an estimator variance.
    pub fn new_with_seed(
        inlier_probabilities: &[f64],
        estimator_variance: f64,
        randomness: f64,
        seed: u64,
    ) -> Self {
        assert!(
            !inlier_probabilities.is_empty(),
            "AdaptiveReorderingSampler requires non-empty probabilities"
        );

        let mut probabilities = Vec::with_capacity(inlier_probabilities.len());
        let mut queue = std::collections::BinaryHeap::new();

        for (idx, &p) in inlier_probabilities.iter().enumerate() {
            let mut prob = p;
            if prob == 1.0 {
                prob -= 1e-6;
            }

            let a = prob * prob * (1.0 - prob) / estimator_variance - prob;
            let b = a * (1.0 - prob) / prob;

            probabilities.push((prob, idx, 0usize, a, b));
            queue.push((ordered_float::OrderedFloat(prob), idx));
        }

        let rng = rand::rngs::StdRng::seed_from_u64(seed);

        Self {
            probabilities,
            queue,
            randomness,
            rng,
        }
    }

    /// Convenience constructor with default variance/randomness.
    pub fn new(inlier_probabilities: &[f64]) -> Self {
        Self::new_with_seed(inlier_probabilities, 0.9765, 0.01, 42)
    }
}

impl Sampler for AdaptiveReorderingSampler {
    fn sample(&mut self, data: &DataMatrix, sample_size: usize, out_indices: &mut [usize]) -> bool {
        let n = data.nrows();
        if sample_size == 0 || n == 0 || out_indices.len() < sample_size {
            return false;
        }
        for out_idx in out_indices.iter_mut().take(sample_size) {
            if let Some((_, idx)) = self.queue.pop() {
                *out_idx = idx;
            } else {
                return false;
            }
        }
        true
    }

    fn update(
        &mut self,
        sample: &[usize],
        sample_size: usize,
        _iteration: usize,
        _score_hint: f64,
    ) {
        let count = sample_size.min(sample.len());
        for &idx in sample.iter().take(count) {
            if let Some(entry) = self.probabilities.get_mut(idx) {
                let (ref mut p, _point_idx, ref mut appearance, a, b) = *entry;
                *appearance += 1;

                let base = (a / (a + b + (*appearance as f64))).abs();
                let jitter: f64 = self
                    .rng
                    .random_range(-self.randomness / 2.0..self.randomness / 2.0);
                let mut updated = base + jitter;
                updated = updated.clamp(0.0, 0.999);

                *p = updated;
                self.queue.push((ordered_float::OrderedFloat(updated), idx));
            }
        }
    }
}