Merge pull request #26 from rpl-cmu/split-gaussian

Added split gaussian option to fac!

Author: contagon

README.md

@@ -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::*,
 };
 
@@ -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);
 

examples/gps.rs

@@ -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,
@@ -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);
 

examples/readme.rs

@@ -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::*,
 };
 
@@ -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);
 

factrs-proc/src/fac.rs

@@ -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,
@@ -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)),

src/containers/factor.rs

@@ -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::*;
@@ -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();
@@ -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());

src/lib.rs

@@ -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
@@ -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)]

src/noise/gaussian.rs

@@ -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));