factrs 0.3.0

Factor graph optimization for robotics
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

use crate::{
    containers::Key,
    dtype,
    linalg::{MatrixBlock, VectorX},
    linear::LinearValues,
};

/// Represents a linear (aka Gaussian) factor.
///
/// This is the linear equivalent of [Factor](crate::containers::Factor). It
/// consists of the relevant keys, a [MatrixBlock] A, and a [VectorX] b. Again,
/// this *shouldn't* ever need to be used by hand.
pub struct LinearFactor {
    pub keys: Vec<Key>,
    pub a: MatrixBlock,
    pub b: VectorX,
}
impl LinearFactor {
    pub fn new(keys: Vec<Key>, a: MatrixBlock, b: VectorX) -> Self {
        assert!(
            keys.len() == a.idx().len(),
            "Mismatch between keys and matrix blocks in LinearFactor::new"
        );
        assert!(
            a.mat().nrows() == b.len(),
            "Mismatch between matrix block and vector in LinearFactor::new"
        );
        Self { keys, a, b }
    }

    pub fn dim_out(&self) -> usize {
        self.b.len()
    }

    pub fn error(&self, vector: &LinearValues) -> dtype {
        let ax: VectorX = self
            .keys
            .iter()
            .enumerate()
            .map(|(idx, key)| {
                self.a.mul(
                    idx,
                    vector
                        .get(*key)
                        .expect("Missing key in LinearValues::error"),
                )
            })
            .sum();
        (ax - &self.b).norm_squared() / 2.0
    }
}