rtsam 0.1.4

Real Time Smoothing and Mapping (RTSAM) 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
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146

pub mod diagonal;
pub mod gaussian;
pub mod isotropic;
pub mod unit;

pub use diagonal::*;
pub use gaussian::*;
pub use isotropic::*;
pub use unit::*;

use nalgebra::base::allocator::Allocator;
use nalgebra::base::default_allocator::DefaultAllocator;
use nalgebra::base::dimension::Dim;
use nalgebra::base::{DMatrix, DVector, MatrixN, VectorN};
use nalgebra::RealField;
use std::fmt::Debug;

#[allow(non_snake_case)]
pub trait NoiseModel<D: Dim, T: RealField + Copy = f64>: Debug {
    fn is_constrained(&self) -> bool;

    fn is_unit(&self) -> bool;

    fn dim(&self) -> usize;

    fn sigmas(&self) -> DVector<T>;

    fn whiten(&self, v: &VectorN<T, D>) -> VectorN<T, D>
    where
        DefaultAllocator: Allocator<T, D>;

    fn whiten_mat(&self, m: &MatrixN<T, D>) -> MatrixN<T, D>
    where
        DefaultAllocator: Allocator<T, D, D>;

    fn unwhiten(&self, v: &VectorN<T, D>) -> VectorN<T, D>
    where
        DefaultAllocator: Allocator<T, D>;

    fn distance(&self, v: &VectorN<T, D>) -> T
    where
        DefaultAllocator: Allocator<T, D>;

    fn whiten_system<_D: Dim>(&self, A: &[DMatrix<T>], b: &VectorN<T, _D>)
    where
        DefaultAllocator: Allocator<T, _D>;
}

#[allow(non_snake_case)]
pub trait GaussianNoise<D: Dim, T: RealField + Copy = f64>: NoiseModel<D, T> {
    fn from_sqrtinfo(R: &MatrixN<T, D>, smart: bool) -> Self
    where
        DefaultAllocator: Allocator<T, D, D>;

    fn from_information(info: &MatrixN<T, D>, smart: bool) -> Self
    where
        DefaultAllocator: Allocator<T, D, D>,
        D: nalgebra::DimSub<nalgebra::Dynamic>;

    fn from_covariance(cov: &MatrixN<T, D>, smart: bool) -> Self
    where
        DefaultAllocator: Allocator<T, D, D>,
        D: nalgebra::DimSub<nalgebra::Dynamic>;

    fn sqrt_info(&self) -> Option<&MatrixN<T, D>>
    where
        DefaultAllocator: Allocator<T, D, D>;

    /// Mahalanobis distance v'*R'*R*v = <R*v,R*v>
    fn mahalanobis_dist(&self, v: &VectorN<T, D>) -> T
    where
        DefaultAllocator: Allocator<T, D>;
}

/// Check *above the diagonal* for non-zero entries and return the diagonal if true
fn check_diagonal_upper<D: Dim, T: nalgebra::RealField + Copy>(
    mat: &MatrixN<T, D>,
) -> Option<DVector<T>>
where
    DefaultAllocator: Allocator<T, D, D>,
{
    let (m, n) = (mat.nrows(), mat.ncols());
    let mut full = false;
    for i in 0..m {
        if !full {
            for j in (i + 1)..n {
                if mat[(i, j)].abs() > T::default_epsilon() {
                    full = true;
                    break;
                }
            }
        }
    }

    if full {
        None
    } else {
        let mut diag = DVector::zeros(n);
        for i in 0..n {
            diag[i] = mat[(i, i)]
        }
        Some(diag)
    }
}

#[cfg(test)]
mod tests {
    use super::*;
    use nalgebra::base::Matrix4;

    #[test]
    fn check_upper_diagonal() {
        let mat = Matrix4::<f64>::identity();
        assert_eq!(check_diagonal_upper(&mat).is_some(), true);

        let eps = std::f64::EPSILON;

        let mat1 = DMatrix::from_row_slice(
            4,
            3, // dim
            &[
                1.0, 0.0, eps, //
                eps, 0.0, 0.0, //
                0.0, 0.0, 0.0, //
                0.0, 2.0, 0.0,
            ],
        );

        assert_eq!(check_diagonal_upper(&mat1).is_some(), true);

        let eps2 = 2.0 * std::f64::EPSILON;
        let mat2 = DMatrix::from_row_slice(
            4,
            3, // dim
            &[
                1.0, 0.0, eps2, //
                0.0, 1.0, 0.0, //
                0.0, 0.0, 0.0, //
                0.0, 0.0, 0.0,
            ],
        );

        println!("{:}", mat2);
        assert_eq!(check_diagonal_upper(&mat2).is_some(), false);
    }
}