Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Added split gaussian option to fac! #26

Merged
merged 1 commit into from
Jan 3, 2025
Merged

Added split gaussian option to fac! #26

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
13 changes: 6 additions & 7 deletions README.md
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -42,9 +42,9 @@ Additionally, we recommend checking out the [tests](/tests/) folder for more exa

```rust
use factrs::{
core::{
assign_symbols, fac, BetweenResidual, GaussNewton, Graph, Huber, PriorResidual, Values, SO2,
},
assign_symbols,
core::{BetweenResidual, GaussNewton, Graph, Huber, PriorResidual, Values, SO2},
fac,
traits::*,
};

Expand All @@ -67,13 +67,12 @@ fn main() {
graph.add_factor(factor);

let res = BetweenResidual::new(y.minus(&x));
let robust = Huber::default();
let factor = fac![res, (X(0), X(1)), 0.1 as std, robust];
let factor = fac![res, (X(0), X(1)), 0.1 as std, Huber::default()];
// fac! is syntactic sugar for the following
// let noise = GaussianNoise::from_scalar_sigma(0.1);
// let factor = FactorBuilder::new2(res, X(0), X(1))
// .noise(noise)
// .robust(robust)
// .noise(GaussianNoise::from_scalar_sigma(0.1))
// .robust(Huber::default())
// .build();
graph.add_factor(factor);

Expand Down
7 changes: 3 additions & 4 deletions examples/gps.rs
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -15,7 +15,7 @@ A simple 2D pose slam example with "GPS" measurements
use factrs::variables::{VectorVar2, SE2};
use factrs::{
assign_symbols,
core::{BetweenResidual, GaussNewton, GaussianNoise, Graph, Values},
core::{BetweenResidual, GaussNewton, Graph, Values},
dtype, fac,
linalg::{Const, ForwardProp, Numeric, NumericalDiff, VectorX},
residuals::Residual1,
Expand Down Expand Up @@ -87,10 +87,9 @@ fn main() {
let mut graph = Graph::new();

// Add odometry factors
let noise = GaussianNoise::<3>::from_diag_covs(0.1, 0.2, 0.2);
let res = BetweenResidual::new(SE2::new(0.0, 2.0, 0.0));
let odometry_01 = fac![res.clone(), (X(0), X(1)), noise.clone()];
let odometry_12 = fac![res, (X(1), X(2)), noise];
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);

Expand Down
13 changes: 6 additions & 7 deletions examples/readme.rs
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,7 +1,7 @@
use factrs::{
core::{
assign_symbols, fac, BetweenResidual, GaussNewton, Graph, Huber, PriorResidual, Values, SO2,
},
assign_symbols,
core::{BetweenResidual, GaussNewton, Graph, Huber, PriorResidual, Values, SO2},
fac,
traits::*,
};

Expand All @@ -24,13 +24,12 @@ fn main() {
graph.add_factor(factor);

let res = BetweenResidual::new(y.minus(&x));
let robust = Huber::default();
let factor = fac![res, (X(0), X(1)), 0.1 as std, robust];
let factor = fac![res, (X(0), X(1)), 0.1 as std, Huber::default()];
// fac! is syntactic sugar for the following
// let noise = GaussianNoise::from_scalar_sigma(0.1);
// let factor = FactorBuilder::new2(res, X(0), X(1))
// .noise(noise)
// .robust(robust)
// .noise(GaussianNoise::from_scalar_sigma(0.1))
// .robust(Huber::default())
// .build();
graph.add_factor(factor);

Expand Down
39 changes: 29 additions & 10 deletions factrs-proc/src/fac.rs
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,10 +1,9 @@
use proc_macro2::Span;
use proc_macro2::TokenStream as TokenStream2;
use quote::quote;
use quote::ToTokens;
use syn::parse_quote;
use syn::ExprCast;
use syn::{parse::Parse, punctuated::Punctuated, Expr, Ident, Token};
use proc_macro2::{Span, TokenStream as TokenStream2};
use quote::{format_ident, quote, ToTokens};
use syn::{
parse::Parse, parse_quote, punctuated::Punctuated, spanned::Spanned, Expr, ExprCast, Ident,
Token,
};

pub struct Factor {
residual: Expr,
Expand Down Expand Up @@ -81,10 +80,30 @@ impl Parse for Factor {
let m = quote!(factrs::noise);
match &input[2] {
Expr::Cast(ExprCast { expr, ty, .. }) => {
match ty.to_token_stream().to_string().as_str() {
"std" => Some(parse_quote!(#m::GaussianNoise::from_scalar_sigma(#expr))),
"cov" => Some(parse_quote!(#m::GaussianNoise::from_scalar_cov(#expr))),
// Make sure it's a cov or std cast
let ty = match ty.to_token_stream().to_string().as_str() {
"cov" => Ident::new("cov", ty.span()),
"std" | "sigma" | "sig" => Ident::new("sigma", ty.span()),
_ => return Err(syn::Error::new_spanned(ty, "Unknown cast for noise")),
};

// Check if it's a tuple or a single variable
match expr.as_ref() {
Expr::Tuple(t) => {
if t.elems.len() != 2 {
return Err(syn::Error::new_spanned(
t,
"Expected tuple with two elements for split std/cov",
));
}
let (a, b) = (&t.elems[0], &t.elems[1]);
let func = format_ident!("from_split_{}", ty);
Some(parse_quote!(#m::GaussianNoise::#func(#a, #b)))
}
_ => {
let func = format_ident!("from_scalar_{}", ty);
Some(parse_quote!(#m::GaussianNoise::#func(#expr)))
}
}
}
Expr::Infer(_) => Some(parse_quote!(#m::UnitNoise)),
Expand Down
11 changes: 3 additions & 8 deletions src/containers/factor.rs
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -274,6 +274,7 @@ impl<const DIM_OUT: usize> FactorBuilder<DIM_OUT> {
#[cfg(test)]
mod tests {

use factrs_proc::fac;
use matrixcompare::assert_matrix_eq;

use super::*;
Expand Down Expand Up @@ -307,10 +308,7 @@ mod tests {
let noise = GaussianNoise::<3>::from_diag_sigmas(1e-1, 2e-1, 3e-1);
let robust = GemanMcClure::default();

let factor = FactorBuilder::new1(residual, X(0))
.noise(noise)
.robust(robust)
.build();
let factor: Factor = fac![residual, X(0), noise, robust];

let f = |x: VectorVar3| {
let mut values = Values::new();
Expand Down Expand Up @@ -340,10 +338,7 @@ mod tests {
let noise = GaussianNoise::<3>::from_diag_sigmas(1e-1, 2e-1, 3e-1);
let robust = GemanMcClure::default();

let factor = FactorBuilder::new2(residual, X(0), X(1))
.noise(noise)
.robust(robust)
.build();
let factor: Factor = fac![residual, (X(0), X(1)), noise, robust];

let mut values = Values::new();
values.insert_unchecked(X(0), x.clone());
Expand Down
31 changes: 14 additions & 17 deletions src/lib.rs
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -37,14 +37,10 @@
//! # Example
//! ```
//! use factrs::{
//! assign_symbols,
//! containers::{FactorBuilder, Graph, Values},
//! noise::GaussianNoise,
//! optimizers::GaussNewton,
//! residuals::{BetweenResidual, PriorResidual},
//! robust::Huber,
//! traits::*,
//! variables::SO2,
//! assign_symbols,
//! core::{BetweenResidual, GaussNewton, Graph, Huber, PriorResidual, Values, SO2},
//! fac,
//! traits::*,
//! };
//!
//! // Assign symbols to variable types
Expand All @@ -60,23 +56,24 @@
//!
//! // Make the factors & insert into graph
//! let mut graph = Graph::new();
//!
//! let res = PriorResidual::new(x.clone());
//! let factor = FactorBuilder::new1(res, X(0)).build();
//! let factor = fac![res, X(0)];
//! graph.add_factor(factor);
//!
//! let res = BetweenResidual::new(y.minus(&x));
//! let noise = GaussianNoise::from_scalar_sigma(0.1);
//! let robust = Huber::default();
//! let factor = FactorBuilder::new2(res, X(0), X(1))
//! .noise(noise)
//! .robust(robust)
//! .build();
//! let factor = fac![res, (X(0), X(1)), 0.1 as std, Huber::default()];
//! // fac! is syntactic sugar for the following
//! // let noise = GaussianNoise::from_scalar_sigma(0.1);
//! // let factor = FactorBuilder::new2(res, X(0), X(1))
//! // .noise(GaussianNoise::from_scalar_sigma(0.1))
//! // .robust(Huber::default())
//! // .build();
//! graph.add_factor(factor);
//!
//! // Optimize!
//! let mut opt: GaussNewton = GaussNewton::new(graph);
//! let result = opt.optimize(values);
//! let result = opt.optimize(values).unwrap();
//! println!("Results {:#}", result);
//! ```

#![warn(clippy::unwrap_used)]
Expand Down
36 changes: 36 additions & 0 deletions src/noise/gaussian.rs
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -51,6 +51,42 @@ impl<const N: usize> GaussianNoise<N> {
Self { sqrt_inf }
}

/// Create from split scalar sigmas.
///
/// Will apply the first scalar to the first N/2 elements and the second
/// scalar to the last N/2 elements. In the case of an odd N, the first N/2
/// elements will have one less element than the last N/2 elements.
pub fn from_split_sigma(sigma1: dtype, sigma2: dtype) -> Self {
let mut sqrt_inf = Matrix::<N, N>::zeros();
let inf1 = 1.0 / sigma1;
let inf2 = 1.0 / sigma2;
for i in 0..N / 2 {
sqrt_inf[(i, i)] = inf1;
}
for i in N / 2..N {
sqrt_inf[(i, i)] = inf2;
}
Self { sqrt_inf }
}

/// Create from split scalar covariances.
///
/// Will apply the first scalar to the first N/2 elements and the second
/// scalar to the last N/2 elements. In the case of an odd N, the first N/2
/// elements will have one less element than the last N/2 elements.
pub fn from_split_cov(cov1: dtype, cov2: dtype) -> Self {
let mut sqrt_inf = Matrix::<N, N>::zeros();
let inf1 = 1.0 / cov1.sqrt();
let inf2 = 1.0 / cov2.sqrt();
for i in 0..N / 2 {
sqrt_inf[(i, i)] = inf1;
}
for i in N / 2..N {
sqrt_inf[(i, i)] = inf2;
}
Self { sqrt_inf }
}

/// Create a diagonal Gaussian noise from a vector of sigmas.
pub fn from_vec_sigma(sigma: VectorView<N>) -> Self {
let sqrt_inf = Matrix::<N, N>::from_diagonal(&sigma.map(|x| 1.0 / x));
Expand Down
Loading