eight_point/
lib.rs

1#![no_std]
2
3use cv_core::nalgebra::{self, Matrix3, MatrixMN, VectorN, U3, U8, U9};
4use cv_core::sample_consensus::Estimator;
5use cv_core::FeatureMatch;
6use cv_pinhole::{EssentialMatrix, NormalizedKeyPoint};
7
8fn encode_epipolar_equation(
9    matches: impl Iterator<Item = FeatureMatch<NormalizedKeyPoint>>,
10) -> MatrixMN<f64, U8, U9> {
11    let mut out: MatrixMN<f64, U8, U9> = nalgebra::zero();
12    for (i, FeatureMatch(a, b)) in (0..8).zip(matches) {
13        let mut row = VectorN::<f64, U9>::zeros();
14        let ap = a.virtual_image_point().coords;
15        let bp = b.virtual_image_point().coords;
16        for j in 0..3 {
17            let v = ap[j] * bp;
18            row.fixed_rows_mut::<U3>(3 * j).copy_from(&v);
19        }
20        out.row_mut(i).copy_from(&row.transpose());
21    }
22    out
23}
24
25/// Performs the
26/// [eight-point algorithm](https://en.wikipedia.org/wiki/Eight-point_algorithm)
27/// by Richard Hartley and Andrew Zisserman.
28///
29/// To recondition the matrix produced by estimation, see
30/// [`cv_core::EssentialMatrix::recondition`].
31#[derive(Copy, Clone, Debug)]
32pub struct EightPoint {
33    pub epsilon: f64,
34    pub iterations: usize,
35}
36
37impl EightPoint {
38    pub fn new() -> Self {
39        Default::default()
40    }
41}
42
43impl Default for EightPoint {
44    fn default() -> Self {
45        Self {
46            epsilon: 1e-9,
47            iterations: 100,
48        }
49    }
50}
51
52impl Estimator<FeatureMatch<NormalizedKeyPoint>> for EightPoint {
53    type Model = EssentialMatrix;
54    type ModelIter = Option<EssentialMatrix>;
55    const MIN_SAMPLES: usize = 8;
56
57    fn estimate<I>(&self, data: I) -> Self::ModelIter
58    where
59        I: Iterator<Item = FeatureMatch<NormalizedKeyPoint>> + Clone,
60    {
61        let epipolar_constraint = encode_epipolar_equation(data);
62        let eet = epipolar_constraint.transpose() * epipolar_constraint;
63        let eigens = eet.try_symmetric_eigen(self.epsilon, self.iterations)?;
64        let eigenvector = eigens
65            .eigenvalues
66            .iter()
67            .enumerate()
68            .min_by_key(|&(_, &n)| float_ord::FloatOrd(n))
69            .map(|(ix, _)| eigens.eigenvectors.column(ix).into_owned())?;
70        let mat = Matrix3::from_iterator(eigenvector.iter().copied());
71        Some(EssentialMatrix(mat))
72    }
73}
eight_point/lib.rs

eight_point/
lib.rs
