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

use faer_ext::IntoNalgebra;

use super::{BaseOptParams, OptObserverVec, OptResult, Optimizer};
use crate::{
    containers::{Graph, GraphOrder, Values, ValuesOrder},
    dtype,
    linalg::DiffResult,
    linear::{LinearSolver, LinearValues},
};

/// The Gauss-Newton optimizer
///
/// Solves $A \Delta \Theta = b$ directly for each optimizer steps. It defaults
/// to using [CholeskySolver](crate::linear::CholeskySolver) under the hood, but
/// this can be changed using [set_solver](GaussNewton::set_solver). See
/// the [linear](crate::linear) module for more linear solver options.
pub struct GaussNewton {
    graph: Graph,
    solver: Box<dyn LinearSolver>,
    /// Basic parameters for the optimizer
    params: BaseOptParams,
    /// Observers for the optimizer
    observers: OptObserverVec,
    // For caching computation between steps
    graph_order: Option<GraphOrder>,
}

impl GaussNewton {
    /// Sets the linear solver to use for the optimizer.
    pub fn set_solver(&mut self, solver: impl LinearSolver + 'static) {
        self.solver = Box::new(solver);
    }
}

impl Optimizer for GaussNewton {
    type Params = BaseOptParams;

    fn new(params: Self::Params, graph: Graph) -> Self {
        Self {
            graph,
            solver: Default::default(),
            observers: OptObserverVec::default(),
            params,
            graph_order: None,
        }
    }

    fn observers(&self) -> &OptObserverVec {
        &self.observers
    }

    fn observers_mut(&mut self) -> &mut OptObserverVec {
        &mut self.observers
    }

    fn graph(&self) -> &Graph {
        &self.graph
    }

    fn graph_mut(&mut self) -> &mut Graph {
        &mut self.graph
    }

    fn error(&self, values: &Values) -> dtype {
        self.graph.error(values)
    }

    fn params(&self) -> &BaseOptParams {
        &self.params
    }

    fn init(&mut self, _values: &Values) -> Vec<&'static str> {
        // TODO: Some way to manual specify how to compute ValuesOrder
        // Precompute the sparsity pattern
        self.graph_order = Some(
            self.graph
                .sparsity_pattern(ValuesOrder::from_values(_values)),
        );

        Vec::new()
    }

    fn step(&mut self, mut values: Values, _idx: usize) -> OptResult<(Values, String)> {
        // Solve the linear system
        let linear_graph = self.graph.linearize(&values);
        let DiffResult { value: r, diff: j } =
            linear_graph.residual_jacobian(self.graph_order.as_ref().expect("Missing graph order"));

        // Solve Ax = b
        let delta = self
            .solver
            .solve_lst_sq(j.as_ref(), r.as_ref())
            .as_ref()
            .into_nalgebra()
            .column(0)
            .clone_owned();

        // Update the values
        let dx = LinearValues::from_order_and_vector(
            self.graph_order
                .as_ref()
                .expect("Missing graph order")
                .order
                .clone(),
            delta,
        );
        values.oplus_mut(&dx);

        Ok((values, String::new()))
    }
}

#[cfg(test)]
mod test {
    use super::*;
    use crate::test_optimizer;

    test_optimizer!(GaussNewton);
}