kiss_icp_ops_eigen/
lib.rs

1use std::ops::MulAssign;
2
3use kiss_icp_ops_core::matrix::MatrixVectorOps;
4use nalgebra::{Matrix3xX, Matrix4, Vector3, SVD};
5
6pub fn umeyama(src: &Matrix3xX<f64>, dst: &Matrix3xX<f64>, with_scaling: bool) -> Matrix4<f64> {
7    let n = src.ncols(); // number of measurements
8
9    // required for demeaning ...
10    let one_over_n = 1.0 / n as f64;
11
12    // computation of mean
13    let src_mean = src.column_mean();
14    let dst_mean = dst.column_mean();
15
16    // demeaning of src and dst points
17    let src_demean = src.vector_sub(&src_mean);
18    let dst_demean = dst.vector_sub(&dst_mean);
19
20    // Eq. (36)-(37)
21    let src_var = src_demean
22        .row_iter()
23        .map(|column| column.norm_squared())
24        .sum::<f64>()
25        * one_over_n;
26
27    // Eq. (38)
28    let sigma = one_over_n * dst_demean * src_demean.transpose();
29
30    let SVD {
31        u,
32        v_t,
33        singular_values,
34    } = sigma.svd(true, true);
35    let u = u.unwrap();
36    let v_t = v_t.unwrap();
37
38    // Initialize the resulting transformation with an identity matrix...
39    let mut rt = Matrix4::identity();
40
41    // Eq. (39)
42    let mut s = Vector3::<f64>::from_element(1.0);
43
44    if u.determinant() * v_t.transpose().determinant() < 0.0 {
45        s[2] = -1.0;
46    }
47
48    // Eq. (40) and (43)
49    let rot = u.vector_mul(&s) * v_t;
50    rt.fixed_view_mut::<3, 3>(0, 0).copy_from(&rot);
51
52    let mut block = rt.fixed_view_mut::<3, 1>(0, 3);
53    block.copy_from(&dst_mean);
54
55    if with_scaling {
56        // Eq. (42)
57        let c = 1.0 / src_var * singular_values.dot(&s);
58
59        // Eq. (41)
60        block -= c * rot * src_mean;
61        rt.fixed_view_mut::<3, 3>(0, 0).mul_assign(c);
62    } else {
63        block -= rot * src_mean;
64    }
65
66    rt
67}
kiss_icp_ops_eigen/lib.rs

kiss_icp_ops_eigen/
lib.rs
