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

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

use crate::{
    containers::{Idx, Key, Symbol, Values, ValuesOrder},
    linalg::{VectorViewX, VectorX},
};

/// Structure to store linear (aka all vector) values
///
/// This structure is the linear equivalent of [Values]. It stores all values in
/// a single vector, along with indices and dimensions of each variable. Ideally
/// *shouldn't* ever be needed in practice.
pub struct LinearValues {
    values: VectorX,
    order: ValuesOrder,
}

impl LinearValues {
    /// Create a zero/identity LinearValues from a [ValuesOrder]
    pub fn zero_from_order(order: ValuesOrder) -> Self {
        let values = VectorX::zeros(order.dim());
        Self { values, order }
    }

    /// Create a zero/identity LinearValues from a [Values]
    ///
    /// The order is inferred from the values
    pub fn zero_from_values(values: &Values) -> Self {
        let order = ValuesOrder::from_values(values);
        let values = VectorX::zeros(order.dim());
        Self { values, order }
    }

    /// Create a LinearValues from a [ValuesOrder] and a vector
    pub fn from_order_and_vector(order: ValuesOrder, values: VectorX) -> Self {
        assert!(
            values.len() == order.dim(),
            "Vector and order must have the same dimension when creating LinearValues"
        );
        Self { values, order }
    }

    /// Create a LinearValues from a [Values] and a vector
    ///
    /// The order is inferred from the values
    pub fn from_values_and_vector(values: &Values, vector: VectorX) -> Self {
        let order = ValuesOrder::from_values(values);
        assert!(
            vector.len() == order.dim(),
            "Vector and values must have the same dimension when creating LinearValues"
        );
        Self::from_order_and_vector(order, vector)
    }

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

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

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

    fn get_idx(&self, idx: &Idx) -> VectorViewX<'_> {
        self.values.rows(idx.idx, idx.dim)
    }

    /// Retrieve a vector from the LinearValues
    pub fn get(&self, key: impl Symbol) -> Option<VectorViewX<'_>> {
        let idx = self.order.get(key)?;
        self.get_idx(idx).into()
    }

    pub fn iter(&self) -> Iter<'_> {
        Iter {
            values: self,
            idx: self.order.iter(),
        }
    }
}

pub struct Iter<'a> {
    values: &'a LinearValues,
    idx: HashMapIter<'a, Key, Idx>,
}

impl<'a> Iterator for Iter<'a> {
    type Item = (&'a Key, VectorViewX<'a>);

    fn next(&mut self) -> Option<Self::Item> {
        let n = self.idx.next()?;
        Some((n.0, self.values.get_idx(n.1)))
    }
}

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

    fn make_order_vector() -> (ValuesOrder, VectorX) {
        // 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);
        let vector = VectorX::from_fn(order.dim(), |i, _| i as dtype);
        (order, vector)
    }

    #[test]
    fn from_order_and_vector() {
        let (order, vector) = make_order_vector();

        // Create LinearValues
        let linear_values = LinearValues::from_order_and_vector(order, vector);
        assert!(linear_values.len() == 3);
        assert!(linear_values.dim() == 11);
        assert!(linear_values.get(X(0)).expect("Key was missing").len() == 2);
        assert!(linear_values.get(X(1)).expect("Key was missing").len() == 6);
        assert!(linear_values.get(X(2)).expect("Key was missing").len() == 3);
        assert!(linear_values.get(X(3)).is_none());
    }

    #[test]
    #[should_panic]
    fn mismatched_size() {
        let (order, vector) = make_order_vector();
        let vector = vector.push(0.0);
        LinearValues::from_order_and_vector(order, vector);
    }
}