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::{env, time::Instant};

#[cfg(feature = "rerun")]
use factrs::rerun::RerunObserver;
use factrs::{
    core::{GaussNewton, LevenMarquardt, SE2, SE3},
    optimizers::{BaseOptParams, GncGemanMcClure, GncParams, GraduatedNonConvexity, LevenParams},
    traits::Optimizer,
    utils::load_g20,
};
#[cfg(feature = "rerun")]
use rerun::{Arrows2D, Arrows3D, Points2D, Points3D};

// Setups rerun and a callback for iteratively sending to rerun
// Must run with --features rerun for it to work
#[cfg(feature = "rerun")]
fn rerun_init(opt: &mut impl Optimizer, dim: &str, obj: &str) {
    // Setup the rerun & the callback
    let rec = rerun::RecordingStreamBuilder::new("factrs-g2o-example")
        .connect_grpc()
        .unwrap();

    // Log the graph
    let (nodes, edges) = opt.graph().into();
    rec.log_static("graph", &[&nodes as &dyn rerun::AsComponents, &edges])
        .expect("log failed");

    let topic = "base/solution";

    match (dim, obj) {
        ("se2", "points") => {
            let callback = RerunObserver::<SE2, Points2D>::new(rec, topic);
            opt.observers_mut().add(callback)
        }
        ("se2", "arrows") => {
            let callback = RerunObserver::<SE2, Arrows2D>::new(rec, topic);
            opt.observers_mut().add(callback)
        }
        ("se3", "points") => {
            let callback = RerunObserver::<SE3, Points3D>::new(rec, topic);
            opt.observers_mut().add(callback)
        }
        ("se3", "arrows") => {
            let callback = RerunObserver::<SE3, Arrows3D>::new(rec, topic);
            opt.observers_mut().add(callback)
        }
        _ => panic!("Invalid arguments"),
    };
}

#[cfg(not(feature = "rerun"))]
fn rerun_init(_opt: &mut impl Optimizer, _dim: &str, _obj: &str) {}

fn main() -> Result<(), Box<dyn std::error::Error>> {
    // ---------------------- Parse Arguments & Load data ---------------------- //
    let mut args: Vec<String> = env::args().collect();
    match args.len() {
        2 => {
            args.push(String::from("gauss"));
            args.push(String::from("arrows"));
        }
        3 => {
            args.push(String::from("arrows"));
        }
        4 => {}
        _ => {
            println!(
                "Usage: {} <g2o file> <optimizer: [gauss|leven|gnc] = gauss> <vis: [points|arrows] = arrows>",
                args[0]
            );
            return Ok(());
        }
    }

    pretty_env_logger::init();

    // Load the graph from the g2o file
    let filename = &args[1];
    let (graph, init) = load_g20(filename);
    println!("File loaded, {} factors", graph.len());

    let obj = &args[3];
    let dim = if init.filter::<SE2>().count() != 0 {
        "se2"
    } else if init.filter::<SE3>().count() != 0 {
        "se3"
    } else {
        panic!("Graph doesn't have SE2 or SE3 variables");
    };

    // Make optimizer
    let mut optimizer: Box<dyn Optimizer> = match args[2].as_str() {
        "gauss" => {
            let params = BaseOptParams {
                max_iterations: 200,
                error_tol_relative: 1e-4,
                ..Default::default()
            };
            let mut opt = GaussNewton::new(params, graph);
            rerun_init(&mut opt, dim, obj);
            Box::new(opt)
        }
        "leven" => {
            let mut params = LevenParams::default();
            params.base.max_iterations = 200;
            params.base.error_tol_relative = 1e-4;
            params.min_model_fidelity = -1e4;
            let mut opt = LevenMarquardt::new(params, graph);
            rerun_init(&mut opt, dim, obj);
            Box::new(opt)
        }
        #[allow(clippy::field_reassign_with_default)]
        "gnc" => {
            let mut params: GncParams<LevenMarquardt> = GncParams::default();
            params.mu_step_size = 1.4;
            params.base.max_iterations = 200;
            params.inner.base.max_iterations = 5;
            params.inner.base.error_tol_absolute = 1e-4;
            params.inner.base.error_tol_relative = 1e-4;
            let mut opt: GraduatedNonConvexity<GncGemanMcClure, _> =
                GraduatedNonConvexity::new(params, graph);
            rerun_init(&mut opt, dim, obj);
            Box::new(opt)
        }
        _ => {
            println!("Optimizer not recognized");
            return Ok(());
        }
    };

    // ------------------------- Optimize ------------------------- //
    let start = Instant::now();
    let result = optimizer.optimize(init);
    let duration = start.elapsed();

    match result {
        Ok(_) => println!("Optimization converged!"),
        Err(_) => println!("Optimization failed!"),
    }
    println!("Optimization took: {duration:?}");
    Ok(())
}