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
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

use std::collections::hash_map::Iter as HashMapIter;

use foldhash::HashMap;

use super::{Key, Symbol, Values};

/// Location of a variable in a list
///
/// Since the map isn't ordered, we need to track both idx and len of each
/// variable
#[derive(Debug, Clone)]
pub struct Idx {
    pub idx: usize,
    pub dim: usize,
}

/// Tracks the location of each variable in the graph via an [Idx].
///
/// Likely won't need to ever interface with this unless a custom optimizer is
/// being implemented. Since the map isn't ordered, we need to track both idx
/// and len of each variable
#[derive(Debug, Clone)]
pub struct ValuesOrder {
    map: HashMap<Key, Idx>,
    dim: usize,
}

impl ValuesOrder {
    pub fn new(map: HashMap<Key, Idx>) -> Self {
        let dim = map.values().map(|idx| idx.dim).sum();
        Self { map, dim }
    }
    pub fn from_values(values: &Values) -> Self {
        let map = values
            .iter()
            .scan(0, |idx, (key, val)| {
                let order = *idx;
                *idx += val.dim();
                Some((
                    *key,
                    Idx {
                        idx: order,
                        dim: val.dim(),
                    },
                ))
            })
            .collect::<HashMap<Key, Idx>>();

        let dim = map.values().map(|idx| idx.dim).sum();

        Self { map, dim }
    }

    pub fn get(&self, symbol: impl Symbol) -> Option<&Idx> {
        self.map.get(&symbol.into())
    }

    pub fn dim(&self) -> usize {
        self.dim
    }

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

    pub fn is_empty(&self) -> bool {
        self.map.is_empty()
    }

    pub fn iter(&self) -> HashMapIter<'_, Key, Idx> {
        self.map.iter()
    }
}

#[cfg(test)]
mod test {
    use super::*;
    use crate::{
        containers::Values,
        symbols::X,
        variables::{Variable, VectorVar2, VectorVar3, VectorVar6},
    };

    #[test]
    fn from_values() {
        // Create some form of values
        let mut v = Values::new();
        v.insert_unchecked(X(0), VectorVar2::identity());
        v.insert_unchecked(X(1), VectorVar6::identity());
        v.insert_unchecked(X(2), VectorVar3::identity());

        // Create an order
        let order = ValuesOrder::from_values(&v);

        // Verify the order
        assert_eq!(order.len(), 3);
        assert_eq!(order.dim(), 11);
        assert_eq!(order.get(X(0)).expect("Missing key").dim, 2);
        assert_eq!(order.get(X(1)).expect("Missing key").dim, 6);
        assert_eq!(order.get(X(2)).expect("Missing key").dim, 3);
    }
}