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
#![no_std]
use cv_core::nalgebra::{self, Matrix3, MatrixMN, VectorN, U3, U8, U9};
use cv_core::sample_consensus::Estimator;
use cv_core::{pinhole::NormalizedKeyPoint, EssentialMatrix, FeatureMatch};
fn encode_epipolar_equation(
matches: impl Iterator<Item = FeatureMatch<NormalizedKeyPoint>>,
) -> MatrixMN<f64, U8, U9> {
let mut out: MatrixMN<f64, U8, U9> = nalgebra::zero();
for (i, FeatureMatch(a, b)) in (0..8).zip(matches) {
let mut row = VectorN::<f64, U9>::zeros();
let ap = a.epipolar_point().0.coords;
let bp = b.epipolar_point().0.coords;
for j in 0..3 {
let v = ap[j] * bp;
row.fixed_rows_mut::<U3>(3 * j).copy_from(&v);
}
out.row_mut(i).copy_from(&row.transpose());
}
out
}
pub struct EightPoint {
pub epsilon: f64,
pub iterations: usize,
}
impl EightPoint {
pub fn new() -> Self {
Default::default()
}
}
impl Default for EightPoint {
fn default() -> Self {
Self {
epsilon: 1e-9,
iterations: 100,
}
}
}
impl Estimator<FeatureMatch<NormalizedKeyPoint>> for EightPoint {
type Model = EssentialMatrix;
type ModelIter = Option<EssentialMatrix>;
const MIN_SAMPLES: usize = 8;
fn estimate<I>(&self, data: I) -> Self::ModelIter
where
I: Iterator<Item = FeatureMatch<NormalizedKeyPoint>> + Clone,
{
let epipolar_constraint = encode_epipolar_equation(data);
let eet = epipolar_constraint.transpose() * epipolar_constraint;
let eigens = eet.try_symmetric_eigen(self.epsilon, self.iterations)?;
let eigenvector = eigens
.eigenvalues
.iter()
.enumerate()
.min_by_key(|&(_, &n)| float_ord::FloatOrd(n))
.map(|(ix, _)| eigens.eigenvectors.column(ix).into_owned())?;
let mat = Matrix3::from_iterator(eigenvector.iter().copied());
Some(EssentialMatrix(mat))
}
}