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

// -----------------------------------------------------------------------------------------------------
//                                      gs-rs - Graph SLAM in Rust
// -----------------------------------------------------------------------------------------------------
//
// SPDX-FileCopyrightText:      © 2020 Samuel Valenzuela (samuel.valenzuela@tngtech.com)
//                              © 2020 Florian Rohm (florian.rohm@tngtech.com)
//                              © 2020 Daniel Pape (daniel.pape@tngtech.com)
// SPDX-License-Identifier:     MIT OR Apache-2.0
//
// This product includes software developed at TNG Technology Consulting GmbH (https://www.tngtech.com/).
//

//! The internal representation of a factor graph's measurement.

use nalgebra::DMatrix;

/// Enum representing a supported factor type.
#[derive(Debug, Clone, PartialEq)]
pub enum FactorType {
    /// Vehicle pose measurement in 2D.
    Position2D,
    /// Relative measurement between two poses in 2D.
    Odometry2D,
    /// Relative measurement to an observed stationary variable in 2D.
    Observation2D,
    /// Vehicle pose measurement in 3D.
    Position3D,
    /// Relative measurement between two poses in 3D.
    Odometry3D,
    /// Relative measurement to an observed stationary variable in 3D.
    Observation3D,
}

/// Structure representing a measurement.
#[derive(Debug, Clone)]
pub struct Factor {
    /// The factor's type.
    pub factor_type: FactorType,
    /// The factor's constraint.
    ///
    /// Content for Position2D and Odometry2D: vec![position_x, position_y, rotation]
    ///
    /// Content for Observation2D: vec![position_x, position_y]
    ///
    /// Content for Position3D and Odometry3D: vec![position_x, position_y, position_z, rotation_quaternion_x, rotation_quaternion_y, rotation_quaternion_z, rotation_quaternion_w]
    ///
    /// Content for Observation3D: vec![position_x, position_y, position_z]
    pub constraint: Vec<f64>,
    /// The factor's wrapped information matrix, equalling the inverse of the factor's mean matrix.
    pub information_matrix: InformationMatrix,
}

/// Structure wrapping the information matrix of a factor.
#[derive(Debug, Clone)]
pub struct InformationMatrix {
    pub content: DMatrix<f64>,
}

impl From<Vec<f64>> for InformationMatrix {
    fn from(content: Vec<f64>) -> Self {
        let dim = (content.len() as f64).sqrt() as usize;
        InformationMatrix {
            content: DMatrix::from_vec(dim, dim, content),
        }
    }
}