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/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(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(graph);
    let result = opt.optimize(values).expect("Optimization failed");

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

pub fn with_capacity(capacity: usize) -> Self

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

Examples found in repository

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(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(graph);
    let result = opt.optimize(values).expect("Optimization failed");

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

pub fn len(&self) -> usize

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

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