similarity_least_squares/
lib.rs

1pub extern crate nalgebra;
2
3use nalgebra::{
4    constraint::{DimEq, ShapeConstraint},
5    Dim, Matrix, Matrix2, RawStorageMut, RealField, Rotation2, SimilarityMatrix2,
6    Translation2, SVD, U2,
7};
8
9#[cfg(feature = "std")]
10#[inline]
11pub fn from_point_slices<T: RealField>(
12    from: &[nalgebra::Point2<T>],
13    to: &[nalgebra::Point2<T>],
14    eps: T,
15    max_niter: usize,
16) -> Option<SimilarityMatrix2<T>>
17where
18    nalgebra::DefaultAllocator: nalgebra::allocator::Allocator<U2, nalgebra::Dyn>,
19{
20    from_matrices(
21        point_slice_to_matrix(from),
22        point_slice_to_matrix(to),
23        eps,
24        max_niter,
25    )
26}
27
28#[cfg(feature = "std")]
29#[inline]
30pub fn point_slice_to_matrix<T: RealField>(
31    slice: &[nalgebra::Point2<T>],
32) -> nalgebra::OMatrix<T, U2, nalgebra::Dyn>
33where
34    nalgebra::DefaultAllocator: nalgebra::allocator::Allocator<U2, nalgebra::Dyn>,
35{
36    nalgebra::OMatrix::<T, U2, nalgebra::Dyn>::from_iterator(
37        slice.len(),
38        slice
39            .iter()
40            .map(|p| p.coords.iter())
41            .flatten()
42            .map(|v| v.to_owned()),
43    )
44}
45
46#[inline]
47pub fn from_matrices<T, D1, D2, S>(
48    mut from: Matrix<T, U2, D1, S>,
49    mut to: Matrix<T, U2, D2, S>,
50    eps: T,
51    max_niter: usize,
52) -> Option<SimilarityMatrix2<T>>
53where
54    T: RealField,
55    D1: Dim,
56    D2: Dim,
57    ShapeConstraint: DimEq<D1, D2>,
58    S: RawStorageMut<T, U2, D1> + RawStorageMut<T, U2, D2>,
59{
60    let size_recip = T::from_usize(from.column_iter().count())
61        .expect("Cannot convert usize to T")
62        .recip();
63
64    let mean_from = from.column_mean();
65    let mean_to = to.column_mean();
66
67    for mut col in from.column_iter_mut() {
68        col -= &mean_from;
69    }
70    for mut col in to.column_iter_mut() {
71        col -= &mean_to;
72    }
73
74    let mut sigma = from
75        .column_iter()
76        .fold(T::zero(), |sigma, col| sigma + col.norm_squared());
77
78    let mut cov = from
79        .column_iter()
80        .zip(to.column_iter())
81        .fold(Matrix2::zeros(), |cov, (from_col, to_col)| {
82            cov + (to_col * from_col.transpose())
83        });
84
85    sigma *= size_recip.to_owned();
86    cov.scale_mut(size_recip);
87
88    let det = cov.determinant();
89    let SVD {
90        u,
91        v_t,
92        singular_values,
93    } = cov.try_svd_unordered(true, true, eps, max_niter)?;
94
95    let u = u?;
96    let v_t = v_t?;
97    let d = Matrix2::from_diagonal(&singular_values);
98
99    let mut s = Matrix2::identity();
100
101    if det < T::zero() || (det == T::zero() && (u.determinant() * v_t.determinant()) < T::zero()) {
102        let index = if d[(1, 1)] < d[(0, 0)] {
103            (1, 1)
104        } else {
105            (0, 0)
106        };
107
108        s[index] = T::one().neg();
109    }
110
111    let scaling = if sigma != T::zero() {
112        sigma.recip() * (d * &s).trace()
113    } else {
114        T::one()
115    };
116
117    let rotation = Rotation2::from_matrix_unchecked(u * s * v_t);
118    let translation: Translation2<T> =
119        (mean_to - (&rotation * mean_from).scale(scaling.to_owned())).into();
120
121    Some(SimilarityMatrix2::from_parts(
122        translation,
123        rotation,
124        scaling,
125    ))
126}
similarity_least_squares/lib.rs

similarity_least_squares/
lib.rs
