Struct Graph 

pub struct Graph { /* private fields */ }

Structure to represent a nonlinear factor graph

Main usage will be via add_factor to add new factors to the graph. Also of note is the linearize function that returns a linear (aka Gaussian) factor graph.

Since the graph represents a nonlinear least-squares problem, during optimization it will be iteratively linearized about a set of variables and solved iteratively.

   assign_symbols,
   containers::{Graph, FactorBuilder},
   residuals::PriorResidual,
   robust::GemanMcClure,
   traits::*,
   variables::SO2,
};
let mut graph = Graph::new();
graph.add_factor(factor);

Implementations

impl Graph

pub fn new() -> Self

Examples found in repository

examples/graph_viz.rs (line 25)

fn main() {
    let mut graph = Graph::new();
    let id = SE2::identity();

    graph.add_factor(fac![PriorResidual::new(id.clone()), X(1)]);
    graph.add_factor(fac![BetweenResidual::new(id.clone()), (X(1), X(2))]);
    graph.add_factor(fac![BetweenResidual::new(id.clone()), (X(2), X(3))]);

    graph.add_factor(fac![BetweenResidual::new(id.clone()), (X(1), X(4))]);
    graph.add_factor(fac![BetweenResidual::new(id.clone()), (X(2), X(4))]);
    graph.add_factor(fac![BetweenResidual::new(id.clone()), (X(3), X(5))]);

    rerun_viz(&graph);
}

examples/readme.rs (line 21)

fn main() {
    // Make all the values
    let mut values = Values::new();

    let x = SO2::from_theta(1.0);
    let y = SO2::from_theta(2.0);
    values.insert(X(0), SO2::identity());
    values.insert(X(1), SO2::identity());

    // Make the factors & insert into graph
    let mut graph = Graph::new();
    let res = PriorResidual::new(x.clone());
    let factor = fac![res, X(0)];
    graph.add_factor(factor);

    let res = BetweenResidual::new(y.minus(&x));
    let factor = fac![res, (X(0), X(1)), 0.1 as std, Huber::default()];
    graph.add_factor(factor);

    // Optimize!
    let mut opt: GaussNewton = GaussNewton::new_default(graph);
    let result = opt.optimize(values).unwrap();
    println!("Results {result:#}");
}

examples/serde.rs (line 31)

fn main() {
    // ------------------------- Serialize values ------------------------- //
    let x = SO2::from_theta(0.6);
    let y = SE2::new(1.0, 2.0, 0.3);
    let mut values = Values::new();
    values.insert(X(0), x.clone());
    values.insert(Y(1), y.clone());

    println!("------ Serializing Values ------");

    let serialized = serde_json::to_string_pretty(&values).unwrap();
    println!("serialized = {serialized}");

    // Convert the JSON string back to a Point.
    let deserialized: Values = serde_json::from_str(&serialized).unwrap();
    println!("deserialized = {deserialized:#}");

    // ------------------------- Serialize graph ------------------------- //
    let prior = PriorResidual::new(x);
    let bet = BetweenResidual::new(y);

    let prior = fac![prior, X(0), 0.1 as cov, GemanMcClure::default()];
    let bet = fac![bet, (Y(0), Y(1)), 10.0 as cov];

    let mut graph = Graph::new();
    graph.add_factor(prior);
    graph.add_factor(bet);

    println!("\n\n------ Serializing Graph ------");

    let serialized = serde_json::to_string_pretty(&graph).unwrap();
    println!("serialized = {serialized}");

    let deserialized: Graph = serde_json::from_str(&serialized).unwrap();
    println!("deserialized = {deserialized:#?}");
}

examples/gps.rs (line 88)

fn main() {
    let mut graph = Graph::new();

    // Add odometry factors
    let res = BetweenResidual::new(SE2::new(0.0, 2.0, 0.0));
    let odometry_01 = fac![res.clone(), (X(0), X(1)), (0.1, 0.2) as cov];
    let odometry_12 = fac![res, (X(1), X(2)), (0.1, 0.2) as cov];
    graph.add_factor(odometry_01);
    graph.add_factor(odometry_12);

    // Add gps factors
    let g0 = fac![GpsResidual::new(0.0, 0.0), X(0), 1.0 as std];
    let g1 = fac![GpsResidual::new(2.0, 0.0), X(1), 1.0 as std];
    let g2 = fac![GpsResidual::new(4.0, 0.0), X(2), 1.0 as std];
    graph.add_factor(g0);
    graph.add_factor(g1);
    graph.add_factor(g2);

    // Make values
    let mut values = Values::new();
    values.insert(X(0), SE2::new(1.0, 2.0, 3.0));
    values.insert(X(1), SE2::identity());
    values.insert(X(2), SE2::identity());

    // // These will all compile-time error
    // // mismatched symbol-variable types
    // values.insert(X(5), VectorVar2::identity());
    // // wrong number of keys
    // let f = fac![GpsResidual::new(0.0, 0.0), (X(0), X(1))];
    // // wrong noise-model dimension
    // let n = factrs::noise::GaussianNoise::<5>::from_scalar_sigma(0.1);
    // let f = fac![GpsResidual::new(0.0, 0.0), X(0), n];
    // // mismatched symbol-variable types
    // assign_symbols!(Y : VectorVar2);
    // let f = fac![GpsResidual::new(0.0, 0.0), Y(0), 0.1 as std];

    // optimize
    let mut opt: GaussNewton = GaussNewton::new_default(graph);
    let result = opt.optimize(values).expect("Optimization failed");

    println!("Final Result: {result:#?}");
}

pub fn with_capacity(capacity: usize) -> Self

pub fn at(&self, idx: usize) -> &Factor

pub fn add_factor(&mut self, factor: Factor)

Examples found in repository

examples/graph_viz.rs (line 28)

fn main() {
    let mut graph = Graph::new();
    let id = SE2::identity();

    graph.add_factor(fac![PriorResidual::new(id.clone()), X(1)]);
    graph.add_factor(fac![BetweenResidual::new(id.clone()), (X(1), X(2))]);
    graph.add_factor(fac![BetweenResidual::new(id.clone()), (X(2), X(3))]);

    graph.add_factor(fac![BetweenResidual::new(id.clone()), (X(1), X(4))]);
    graph.add_factor(fac![BetweenResidual::new(id.clone()), (X(2), X(4))]);
    graph.add_factor(fac![BetweenResidual::new(id.clone()), (X(3), X(5))]);

    rerun_viz(&graph);
}

examples/readme.rs (line 24)

fn main() {
    // Make all the values
    let mut values = Values::new();

    let x = SO2::from_theta(1.0);
    let y = SO2::from_theta(2.0);
    values.insert(X(0), SO2::identity());
    values.insert(X(1), SO2::identity());

    // Make the factors & insert into graph
    let mut graph = Graph::new();
    let res = PriorResidual::new(x.clone());
    let factor = fac![res, X(0)];
    graph.add_factor(factor);

    let res = BetweenResidual::new(y.minus(&x));
    let factor = fac![res, (X(0), X(1)), 0.1 as std, Huber::default()];
    graph.add_factor(factor);

    // Optimize!
    let mut opt: GaussNewton = GaussNewton::new_default(graph);
    let result = opt.optimize(values).unwrap();
    println!("Results {result:#}");
}

examples/serde.rs (line 32)

fn main() {
    // ------------------------- Serialize values ------------------------- //
    let x = SO2::from_theta(0.6);
    let y = SE2::new(1.0, 2.0, 0.3);
    let mut values = Values::new();
    values.insert(X(0), x.clone());
    values.insert(Y(1), y.clone());

    println!("------ Serializing Values ------");

    let serialized = serde_json::to_string_pretty(&values).unwrap();
    println!("serialized = {serialized}");

    // Convert the JSON string back to a Point.
    let deserialized: Values = serde_json::from_str(&serialized).unwrap();
    println!("deserialized = {deserialized:#}");

    // ------------------------- Serialize graph ------------------------- //
    let prior = PriorResidual::new(x);
    let bet = BetweenResidual::new(y);

    let prior = fac![prior, X(0), 0.1 as cov, GemanMcClure::default()];
    let bet = fac![bet, (Y(0), Y(1)), 10.0 as cov];

    let mut graph = Graph::new();
    graph.add_factor(prior);
    graph.add_factor(bet);

    println!("\n\n------ Serializing Graph ------");

    let serialized = serde_json::to_string_pretty(&graph).unwrap();
    println!("serialized = {serialized}");

    let deserialized: Graph = serde_json::from_str(&serialized).unwrap();
    println!("deserialized = {deserialized:#?}");
}

examples/gps.rs (line 94)

fn main() {
    let mut graph = Graph::new();

    // Add odometry factors
    let res = BetweenResidual::new(SE2::new(0.0, 2.0, 0.0));
    let odometry_01 = fac![res.clone(), (X(0), X(1)), (0.1, 0.2) as cov];
    let odometry_12 = fac![res, (X(1), X(2)), (0.1, 0.2) as cov];
    graph.add_factor(odometry_01);
    graph.add_factor(odometry_12);

    // Add gps factors
    let g0 = fac![GpsResidual::new(0.0, 0.0), X(0), 1.0 as std];
    let g1 = fac![GpsResidual::new(2.0, 0.0), X(1), 1.0 as std];
    let g2 = fac![GpsResidual::new(4.0, 0.0), X(2), 1.0 as std];
    graph.add_factor(g0);
    graph.add_factor(g1);
    graph.add_factor(g2);

    // Make values
    let mut values = Values::new();
    values.insert(X(0), SE2::new(1.0, 2.0, 3.0));
    values.insert(X(1), SE2::identity());
    values.insert(X(2), SE2::identity());

    // // These will all compile-time error
    // // mismatched symbol-variable types
    // values.insert(X(5), VectorVar2::identity());
    // // wrong number of keys
    // let f = fac![GpsResidual::new(0.0, 0.0), (X(0), X(1))];
    // // wrong noise-model dimension
    // let n = factrs::noise::GaussianNoise::<5>::from_scalar_sigma(0.1);
    // let f = fac![GpsResidual::new(0.0, 0.0), X(0), n];
    // // mismatched symbol-variable types
    // assign_symbols!(Y : VectorVar2);
    // let f = fac![GpsResidual::new(0.0, 0.0), Y(0), 0.1 as std];

    // optimize
    let mut opt: GaussNewton = GaussNewton::new_default(graph);
    let result = opt.optimize(values).expect("Optimization failed");

    println!("Final Result: {result:#?}");
}

pub fn len(&self) -> usize

Examples found in repository

examples/g2o.rs (line 80)

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(())
}

pub fn is_empty(&self) -> bool

pub fn error(&self, values: &Values) -> dtype

pub fn linearize(&self, values: &Values) -> LinearGraph

pub fn sparsity_pattern(&self, order: ValuesOrder) -> GraphOrder

pub fn iter(&self) -> Iter<'_, Factor>

pub fn iter_mut(&mut self) -> IterMut<'_, Factor>

Trait Implementations

impl Clone for Graph

fn clone(&self) -> Graph

Returns a duplicate of the value.

fn clone_from(&mut self, source: &Self)

Performs copy-assignment from source.

impl Debug for Graph

fn fmt(&self, f: &mut Formatter<'_>) -> Result

Formats the value using the given formatter.

impl Default for Graph

fn default() -> Graph

Returns the “default value” for a type.

impl<'de> Deserialize<'de> for Graph

fn deserialize<__D>(__deserializer: __D) -> Result<Self, __D::Error>

where __D: Deserializer<'de>,

Deserialize this value from the given Serde deserializer.

impl From<&Graph> for (GraphNodes, GraphEdges)

Available on crate feature rerun only.

fn from(graph: &Graph) -> (GraphNodes, GraphEdges)

Converts to this type from the input type.

impl From<Graph> for (GraphNodes, GraphEdges)

Available on crate feature rerun only.

fn from(graph: Graph) -> (GraphNodes, GraphEdges)

Converts to this type from the input type.

impl IntoIterator for Graph

type Item = Factor

The type of the elements being iterated over.

type IntoIter = IntoIter<Factor>

Which kind of iterator are we turning this into?

fn into_iter(self) -> Self::IntoIter

Creates an iterator from a value.

impl Serialize for Graph

fn serialize<__S>(&self, __serializer: __S) -> Result<__S::Ok, __S::Error>

where __S: Serializer,

Serialize this value into the given Serde serializer.