Merge pull request #20 from rpl-cmu/easton/ci

Update CI

.github/workflows/ci.yml

@@ -0,0 +1,58 @@
+name: ci
+
+on:
+  push:
+    branches: [ "master", "dev" ]
+  pull_request:
+    branches: [ "master", "dev" ]
+
+env:
+  CARGO_TERM_COLOR: always
+
+# Loosely based on
+# https://github.com/nushell/nushell/blob/8771872d861eeb94eec63051cb4938f6306d1dcd/.github/workflows/ci.yml
+# https://www.reillywood.com/blog/rust-faster-ci/
+jobs:
+  fmt-clippy:
+    runs-on: ubuntu-latest
+    steps:
+    - uses: actions/checkout@v4
+      with:
+        submodules: recursive
+
+    - name: Setup Rust toolchain and cache
+      uses: actions-rust-lang/setup-rust-toolchain@v1
+      with:
+        toolchain: 1.81
+        components: rustfmt,clippy
+
+    - name: cargo fmt
+      run: cargo fmt --all -- --check
+
+    - name: clippy
+      run: cargo clippy
+    - name: clippy of tests
+      run: cargo clippy --tests
+
+  tests:
+    runs-on: ubuntu-latest
+    steps:
+    - uses: actions/checkout@v4
+      with:
+        submodules: recursive
+
+    - name: Setup Rust toolchain and cache
+      uses: actions-rust-lang/setup-rust-toolchain@v1
+      with:
+        toolchain: 1.81
+
+    - name: default
+      run: cargo test 
+    - name: serde
+      run: cargo test --features="serde,serde_json"
+    - name: f32
+      run: cargo test --features f32
+    - name: left
+      run: cargo test --features left
+    - name: fake_exp
+      run: cargo test --features fake_exp

Cargo.toml

@@ -11,6 +11,10 @@ keywords = ["nonlinear", "optimization", "robotics", "estimation", "SLAM"]
 categories = ["science::robotics", "mathematics"]
 rust-version = "1.81"
 
+[workspace]
+members = ["factrs-proc"]
+exclude = ["factrs-typetag"]
+
 [package.metadata.docs.rs]
 rustdoc-args = ["--html-in-header", "assets/katex-header.html"]
 
@@ -20,7 +24,7 @@ ahash = "0.8.11"
 paste = "1.0.15"
 downcast-rs = "1.2.1"
 log = "0.4.22"
-factrs-proc = {version = "0.1.0", path = "./factrs-proc"}
+factrs-proc = { version = "0.1.0", path = "./factrs-proc" }
 
 # numerical 
 faer = { version = "0.19.4", default-features = false, features = [
@@ -30,12 +34,12 @@ faer = { version = "0.19.4", default-features = false, features = [
 ] }
 faer-ext = { version = "0.3.0", features = ["nalgebra"] }
 nalgebra = { version = "0.33.2", features = ["compare"] }
-num-dual = "0.10.0"
+num-dual = "0.11.0"
 matrixcompare = { version = "0.3" }
 
 # serialization
 serde = { version = "1.0.214", optional = true }
-typetag = { version = "0.2.18", optional = true, path = "factrs-typetag" }
+typetag = { version = "0.2.18", optional = true, path = "./factrs-typetag" }
 serde_json = { version = "1.0.132", optional = true }
 
 # rerun support

examples/serde.rs

@@ -1,13 +1,15 @@
 use factrs::{
-    containers::{FactorBuilder, Graph, Values, X},
+    assign_symbols,
+    containers::{Graph, Values},
     fac,
-    factors::Factor,
     noise::GaussianNoise,
     residuals::{BetweenResidual, PriorResidual},
-    robust::{GemanMcClure, L2},
+    robust::GemanMcClure,
     variables::{SE2, SO2},
 };
 
+assign_symbols!(X: SO2, SE2);
+
 fn main() {
     // ------------------------- Try with values ------------------------- //
     let x = SO2::from_theta(0.6);
@@ -33,7 +35,7 @@ fn main() {
         GaussianNoise::from_scalar_cov(0.1),
         GemanMcClure::default()
     ];
-    let bet = fac![res, (X(0), X(1)), GaussianNoise::from_scalar_cov(10.0)];
+    let bet = fac![bet, (X(0), X(1)), GaussianNoise::from_scalar_cov(10.0)];
 
     let mut graph = Graph::new();
     graph.add_factor(prior);

factrs-proc/src/fac.rs

@@ -1,6 +1,7 @@
 use proc_macro2::Span;
 use proc_macro2::TokenStream as TokenStream2;
 use quote::quote;
+use syn::parse_quote;
 use syn::{parse::Parse, punctuated::Punctuated, Expr, Ident, Token};
 
 pub struct Factor {
@@ -29,7 +30,7 @@ impl Factor {
         let func = Ident::new(&format!("new{}", self.keys.len()), Span::call_site());
         let res = &self.residual;
         let keys = &self.keys;
-        return quote! { #func(#res, #keys) };
+        quote! { #func(#res, #keys) }
     }
 }
 
@@ -45,7 +46,7 @@ impl Parse for Factor {
             ));
         } else if input.len() > 4 {
             return Err(syn::Error::new_spanned(
-                &input.last(),
+                input.last(),
                 "Expected at most four items",
             ));
         }
@@ -60,7 +61,7 @@ impl Parse for Factor {
             // in parentheses
             Expr::Tuple(t) => t.elems.clone(),
             // a single key for unary factors
-            Expr::Path(_) => {
+            Expr::Path(_) | Expr::Call(_) => {
                 let mut p = Punctuated::<Expr, Token![,]>::new();
                 p.push(input[1].clone());
                 p
@@ -75,7 +76,12 @@ impl Parse for Factor {
 
         // Then the noise
         let noise = if input.len() >= 3 {
-            Some(input[2].clone())
+            match &input[2] {
+                Expr::Lit(l) => {
+                    Some(parse_quote!(factrs::noise::GaussianNoise::from_scalar_sigma(#l)))
+                }
+                _ => Some(input[2].clone()),
+            }
         } else {
             None
         };

factrs-proc/src/residual.rs

@@ -3,7 +3,7 @@ use quote::{format_ident, quote};
 use syn::{parse_quote, ItemImpl, Path};
 
 fn parse_residual_trait(item: &ItemImpl) -> syn::Result<(Path, u32)> {
-    let err = syn::Error::new_spanned(&item, "unable to parse residual number");
+    let err = syn::Error::new_spanned(item, "unable to parse residual number");
     let residual_trait = item.trait_.clone().ok_or(err.clone())?.1;
 
     let num = residual_trait
@@ -16,8 +16,8 @@ fn parse_residual_trait(item: &ItemImpl) -> syn::Result<(Path, u32)> {
         .parse::<u32>();
 
     match num {
-        Result::Err(_) => return Err(err),
-        Result::Ok(n) => return Ok((residual_trait, n)),
+        Result::Err(_) => Err(err),
+        Result::Ok(n) => Ok((residual_trait, n)),
     }
 }
 

factrs-proc/src/variable.rs

@@ -60,7 +60,7 @@ pub fn mark(item: ItemImpl) -> TokenStream2 {
         2 => {
             let first_generic = item.generics.params.first().unwrap();
             if let GenericParam::Const(ConstParam { ident, .. }) = first_generic {
-                let format = format!("{}<{{}}>", name.to_string());
+                let format = format!("{}<{{}}>", name);
                 // let format = quote! { #name<{}> }.to_string();
                 expanded.extend(quote! {
                     impl<const #ident: usize> typetag::Tagged for #name<#ident> {

src/optimizers/gauss_newton.rs

@@ -79,7 +79,11 @@ impl<S: LinearSolver> Optimizer for GaussNewton<S> {
 
         // Update the values
         let dx = LinearValues::from_order_and_vector(
-            self.graph_order.as_ref().expect("Missing graph order").order.clone(),
+            self.graph_order
+                .as_ref()
+                .expect("Missing graph order")
+                .order
+                .clone(),
             delta,
         );
         values.oplus_mut(&dx);

src/residuals/imu_preint/delta.rs

@@ -90,7 +90,7 @@ impl<T: Numeric> ImuDelta<T> {
         // Setup
         self.dt += dt;
         let accel_world = SO3::exp(self.xi_theta.as_view()).apply(accel.0.as_view());
-        let H = SO3::dexp(self.xi_theta.as_view());
+        let H = SO3::dexp_right(self.xi_theta.as_view());
         let Hinv = H.try_inverse().expect("Failed to invert H(theta)");
 
         // Integrate (make sure integration occurs in the correct order)
@@ -118,7 +118,7 @@ impl<T: Numeric> ImuDelta<T> {
     // that's faster
     #[allow(non_snake_case)]
     fn A(&self, gyro: &GyroUnbiased<T>, accel: &AccelUnbiased<T>, dt: T) -> Matrix<15, 15, T> {
-        let H = SO3::dexp(self.xi_theta.as_view());
+        let H = SO3::dexp_right(self.xi_theta.as_view());
         let Hinv = H.try_inverse().expect("Failed to invert H(theta)");
         let R: nalgebra::Matrix<
             T,
@@ -196,6 +196,11 @@ mod test {
         variables::VectorVar,
     };
 
+    #[cfg(not(feature = "f32"))]
+    const TOL: f64 = 1e-5;
+    #[cfg(feature = "f32")]
+    const TOL: f32 = 1e-3;
+
     // Helper function to integrate a constant gyro / accel for a given amount of
     // time
     fn integrate<T: Numeric>(
@@ -231,9 +236,9 @@ mod test {
         let delta = integrate(&gyro, &accel, &ImuBias::identity(), n, t);
 
         println!("Delta: {}", delta);
-        assert_scalar_eq!(delta.dt, t, comp = abs, tol = 1e-5);
-        assert_matrix_eq!(delta.xi_vel, accel.0 * t, comp = abs, tol = 1e-5);
-        assert_matrix_eq!(delta.xi_pos, accel.0 * t * t / 2.0, comp = abs, tol = 1e-5);
+        assert_scalar_eq!(delta.dt, t, comp = abs, tol = TOL);
+        assert_matrix_eq!(delta.xi_vel, accel.0 * t, comp = abs, tol = TOL);
+        assert_matrix_eq!(delta.xi_pos, accel.0 * t * t / 2.0, comp = abs, tol = TOL);
     }
 
     // Test constant angular velocity
@@ -249,16 +254,15 @@ mod test {
         let delta = integrate(&gyro, &accel, &ImuBias::identity(), n, t);
 
         println!("Delta: {}", delta);
-        assert_scalar_eq!(delta.dt, t, comp = abs, tol = 1e-5);
-        assert_matrix_eq!(delta.xi_theta, gyro.0 * t, comp = abs, tol = 1e-5);
+        assert_scalar_eq!(delta.dt, t, comp = abs, tol = TOL);
+        assert_matrix_eq!(delta.xi_theta, gyro.0 * t, comp = abs, tol = TOL);
     }
 
     // Test construction of state transtition matrix A
     #[test]
     fn make_a() {
         let dt = 0.1;
-        let v: nalgebra::Matrix<f64, Const<15>, Const<1>, nalgebra::ArrayStorage<f64, 15, 1>> =
-            Vector::<15>::from_fn(|i, _| i as dtype / 10.0);
+        let v = Vector::<15>::from_fn(|i, _| i as dtype / 10.0);
         let gyro = Gyro::new(3.0, 2.0, 1.0);
         let accel: Accel = Accel::new(1.0, 2.0, 3.0);
 
@@ -330,7 +334,7 @@ mod test {
             a_exp.fixed_view::<15, 12>(0, 3),
             a_got.fixed_view::<15, 12>(0, 3),
             comp = abs,
-            tol = 1e-5
+            tol = TOL
         );
     }
 
@@ -366,7 +370,7 @@ mod test {
 
         println!("H_accel_got: {:.4}", H_accel_got);
         println!("H_accel_exp: {:.4}", H_accel_exp);
-        assert_matrix_eq!(H_accel_got, H_accel_exp, comp = abs, tol = 1e-5);
+        assert_matrix_eq!(H_accel_got, H_accel_exp, comp = abs, tol = TOL);
 
         println!("H_gyro_got: {:.4}", H_gyro_got);
         println!("H_gyro_exp: {:.4}", H_gyro_exp);
@@ -375,7 +379,7 @@ mod test {
             H_gyro_got.fixed_view::<6, 3>(3, 0),
             H_gyro_exp.fixed_view::<6, 3>(3, 0),
             comp = abs,
-            tol = 1e-5
+            tol = TOL
         );
     }
 }

src/residuals/imu_preint/residual.rs

@@ -16,7 +16,7 @@ use crate::{
 
 /// Covariance parameters for the IMU preintegration
 ///
-/// Trys to come with semi-resonable defaults for the covariance parameters
+/// Tries to come with semi-reasonable defaults for the covariance parameters
 /// ```
 /// use factrs::residuals::ImuCovariance;
 /// let cov = ImuCovariance::default();
@@ -363,6 +363,7 @@ mod test {
     use crate::{
         assert_variable_eq, assign_symbols,
         containers::{Graph, Values},
+        fac,
         linalg::Vector3,
         optimizers::GaussNewton,
         residuals::{Accel, Gyro, PriorResidual},
@@ -395,9 +396,9 @@ mod test {
         // Build factor and graph
         let mut graph = Graph::new();
         let factor = preint.build(X(0), V(0), B(0), X(1), V(1), B(1));
-        let prior_x0 = FactorBuilder::new1(PriorResidual::new(x0.clone()), X(0)).build();
-        let prior_v0 = FactorBuilder::new1(PriorResidual::new(v0.clone()), V(0)).build();
-        let prior_b0 = FactorBuilder::new1(PriorResidual::new(b0.clone()), B(0)).build();
+        let prior_x0 = fac!(PriorResidual::new(x0.clone()), X(0), 1e-3);
+        let prior_v0 = fac!(PriorResidual::new(v0.clone()), V(0), 1e-3);
+        let prior_b0 = fac!(PriorResidual::new(b0.clone()), B(0), 1e-3);
         graph.add_factor(factor);
         graph.add_factor(prior_x0);
         graph.add_factor(prior_v0);

src/residuals/prior.rs

@@ -75,9 +75,9 @@ mod test {
     const TOL: f64 = 1e-6;
 
     #[cfg(feature = "f32")]
-    const PWR: i32 = 3;
+    const PWR: i32 = 4;
     #[cfg(feature = "f32")]
-    const TOL: f32 = 1e-3;
+    const TOL: f32 = 1e-2;
 
     fn test_prior_jacobian<
         #[cfg(feature = "serde")] P: VariableUmbrella + 'static + typetag::Tagged,