Make core module to pair with trait module

examples/g2o.rs

@@ -1,17 +1,16 @@
+#[cfg(feature = "rerun")]
+use std::net::{SocketAddr, SocketAddrV4};
 use std::{env, time::Instant};
 
+#[cfg(feature = "rerun")]
+use factrs::rerun::RerunObserver;
 use factrs::{
-    optimizers::{GaussNewton, GraphOptimizer, Optimizer},
+    core::{GaussNewton, SE2, SE3},
+    traits::Optimizer,
     utils::load_g20,
-    variables::*,
 };
-
-#[cfg(feature = "rerun")]
-use factrs::rerun::RerunObserver;
 #[cfg(feature = "rerun")]
 use rerun::{Arrows2D, Arrows3D, Points2D, Points3D};
-#[cfg(feature = "rerun")]
-use std::net::{SocketAddr, SocketAddrV4};
 
 // Setups rerun and a callback for iteratively sending to rerun
 // Must run with --features rerun for it to work

examples/gps.rs

@@ -10,21 +10,18 @@ A simple 2D pose slam example with "GPS" measurements
 */
 
 #![allow(unused_imports)]
+// Our state will be represented by SE2 -> theta, x, y
+// VectorVar2 is a newtype around Vector2 for optimization purposes
+use factrs::variables::{VectorVar2, SE2};
 use factrs::{
     assign_symbols,
-    containers::{Graph, Values},
+    core::{BetweenResidual, GaussNewton, GaussianNoise, Graph, Values},
     dtype, fac,
     linalg::{Const, ForwardProp, Numeric, NumericalDiff, VectorX},
-    noise::GaussianNoise,
-    optimizers::GaussNewton,
-    residuals::{BetweenResidual, Residual1},
-    traits::{GraphOptimizer, Optimizer, Variable},
+    residuals::Residual1,
+    traits::*,
 };
 
-// Our state will be represented by SE2 -> theta, x, y
-// VectorVar2 is a newtype around Vector2 for optimization purposes
-use factrs::variables::{VectorVar2, SE2};
-
 #[derive(Clone, Debug)]
 // Enable serialization if it's desired
 #[cfg_attr(feature = "serde", derive(serde::Serialize, serde::Deserialize))]
@@ -45,10 +42,11 @@ impl GpsResidual {
 impl Residual1 for GpsResidual {
     // Use forward propagation for differentiation
     type Differ = ForwardProp<<Self as Residual1>::DimIn>;
-    // Alternatively, could use numerical differentiation (6 => 10^-6 as denominator)
-    // type Differ = NumericalDiff<6>;
+    // Alternatively, could use numerical differentiation (6 => 10^-6 as
+    // denominator) type Differ = NumericalDiff<6>;
 
-    // The input variable type, input dimension of variable(s), and output dimension of residual
+    // The input variable type, input dimension of variable(s), and output dimension
+    // of residual
     type V1 = SE2;
     type DimIn = Const<3>;
     type DimOut = Const<2>;
@@ -64,8 +62,8 @@ impl Residual1 for GpsResidual {
     }
 
     // You can also hand-code the jacobian by hand if extra efficiency is desired.
-    // fn residual1_jacobian(&self, values: &Values, keys: &[Key]) -> DiffResult<VectorX, MatrixX> {
-    //     let p: &SE2 = values
+    // fn residual1_jacobian(&self, values: &Values, keys: &[Key]) ->
+    // DiffResult<VectorX, MatrixX> {     let p: &SE2 = values
     //         .get_unchecked(keys[0])
     //         .expect("got wrong variable type");
     //     let s = p.theta().sin();
@@ -76,8 +74,9 @@ impl Residual1 for GpsResidual {
     //         diff,
     //     }
     // }
-    // As a note - the above jacobian is only valid if running with the "left" feature disabled
-    // Switching to the left feature will change the jacobian used
+    // As a note - the above jacobian is only valid if running with the "left"
+    // feature disabled Switching to the left feature will change the jacobian
+    // used
 }
 
 // Here we assign X to always represent SE2 variables
@@ -111,11 +110,12 @@ fn main() {
 
     // These will all compile-time error
     // values.insert(X(5), VectorVar2::identity()); // wrong variable type
-    // let f = fac![GpsResidual::new(0.0, 0.0), (X(0), X(1))]; // wrong number of keys
-    // let n = GaussianNoise::<5>::from_scalar_sigma(0.1);
-    // let f = fac![GpsResidual::new(0.0, 0.0), X(0), n]; // wrong noise-model dimension
-    // assign_symbols!(Y : VectorVar2);
-    // let f = fac![GpsResidual::new(0.0, 0.0), Y(0), 0.1 as std]; // wrong variable type
+    // let f = fac![GpsResidual::new(0.0, 0.0), (X(0), X(1))]; // wrong number of
+    // keys let n = GaussianNoise::<5>::from_scalar_sigma(0.1);
+    // let f = fac![GpsResidual::new(0.0, 0.0), X(0), n]; // wrong noise-model
+    // dimension assign_symbols!(Y : VectorVar2);
+    // let f = fac![GpsResidual::new(0.0, 0.0), Y(0), 0.1 as std]; // wrong variable
+    // type
 
     // optimize
     let mut opt: GaussNewton = GaussNewton::new(graph);

rustfmt.toml

@@ -0,0 +1,6 @@
+imports_granularity = "Crate"
+group_imports = "StdExternalCrate"
+# imports_layout = "HorizontalVertical"
+
+format_code_in_doc_comments = true
+wrap_comments = true

src/containers/factor.rs

@@ -136,7 +136,7 @@ impl<'f, KF> FactorFormatter<'f, KF> {
     }
 }
 
-impl<'f, KF: KeyFormatter> fmt::Debug for FactorFormatter<'f, KF> {
+impl<KF: KeyFormatter> fmt::Debug for FactorFormatter<'_, KF> {
     fn fmt(&self, f: &mut fmt::Formatter<'_>) -> fmt::Result {
         if f.alternate() {
             f.write_str("Factor {\n")?;

src/containers/graph.rs

@@ -131,7 +131,7 @@ impl<'g, KF> GraphFormatter<'g, KF> {
     }
 }
 
-impl<'g, KF: KeyFormatter> Debug for GraphFormatter<'g, KF> {
+impl<KF: KeyFormatter> Debug for GraphFormatter<'_, KF> {
     fn fmt(&self, f: &mut std::fmt::Formatter<'_>) -> std::fmt::Result {
         if f.alternate() {
             f.write_str("Graph [\n")?;

src/containers/values.rs

@@ -225,7 +225,7 @@ impl<'v, KF> ValuesFormatter<'v, KF> {
     }
 }
 
-impl<'v, KF: KeyFormatter> fmt::Display for ValuesFormatter<'v, KF> {
+impl<KF: KeyFormatter> fmt::Display for ValuesFormatter<'_, KF> {
     fn fmt(&self, f: &mut fmt::Formatter) -> fmt::Result {
         let precision = f.precision().unwrap_or(3);
         if f.alternate() {
@@ -246,7 +246,7 @@ impl<'v, KF: KeyFormatter> fmt::Display for ValuesFormatter<'v, KF> {
     }
 }
 
-impl<'v, KF: KeyFormatter> fmt::Debug for ValuesFormatter<'v, KF> {
+impl<KF: KeyFormatter> fmt::Debug for ValuesFormatter<'_, KF> {
     fn fmt(&self, f: &mut fmt::Formatter) -> fmt::Result {
         let precision = f.precision().unwrap_or(3);
         if f.alternate() {

src/lib.rs

@@ -127,10 +127,26 @@ pub mod symbols {
 /// ```
 pub mod traits {
     pub use crate::{
-        linalg::Diff,
-        optimizers::{GraphOptimizer, Optimizer},
-        residuals::Residual,
-        variables::Variable,
+        linalg::Diff, noise::NoiseModel, optimizers::Optimizer, residuals::Residual,
+        robust::RobustCost, variables::Variable,
+    };
+}
+
+/// Helper module to group together common types
+///
+/// Specifically, this contains everything that would be needed to implement a
+/// simple pose graph. While we recommend against it, it can be all imported
+/// using
+/// ```
+/// use factrs::core::*;
+/// ```
+pub mod core {
+    pub use crate::{
+        containers::{Factor, Graph, Values},
+        noise::{GaussianNoise, UnitNoise},
+        optimizers::{GaussNewton, LevenMarquardt},
+        residuals::{BetweenResidual, PriorResidual},
+        variables::{VectorVar, VectorVar1, VectorVar2, VectorVar3, SE2, SE3, SO2, SO3},
     };
 }
 
@@ -139,8 +155,7 @@ pub mod rerun;
 
 #[cfg(feature = "serde")]
 pub mod serde {
-    pub use crate::noise::tag_noise;
-    pub use crate::residuals::tag_residual;
-    pub use crate::robust::tag_robust;
-    pub use crate::variables::tag_variable;
+    pub use crate::{
+        noise::tag_noise, residuals::tag_residual, robust::tag_robust, variables::tag_variable,
+    };
 }

src/optimizers/gauss_newton.rs

@@ -1,6 +1,6 @@
 use faer_ext::IntoNalgebra;
 
-use super::{GraphOptimizer, OptObserverVec, OptParams, OptResult, Optimizer};
+use super::{OptObserverVec, OptParams, OptResult, Optimizer};
 use crate::{
     containers::{Graph, GraphOrder, Values, ValuesOrder},
     linalg::DiffResult,
@@ -26,8 +26,8 @@ pub struct GaussNewton<S: LinearSolver = CholeskySolver> {
     graph_order: Option<GraphOrder>,
 }
 
-impl<S: LinearSolver> GraphOptimizer for GaussNewton<S> {
-    fn new(graph: Graph) -> Self {
+impl<S: LinearSolver> GaussNewton<S> {
+    pub fn new(graph: Graph) -> Self {
         Self {
             graph,
             solver: S::default(),
@@ -37,7 +37,7 @@ impl<S: LinearSolver> GraphOptimizer for GaussNewton<S> {
         }
     }
 
-    fn graph(&self) -> &Graph {
+    pub fn graph(&self) -> &Graph {
         &self.graph
     }
 }

src/optimizers/levenberg_marquardt.rs

@@ -3,7 +3,7 @@ use std::ops::Mul;
 use faer::{scale, sparse::SparseColMat};
 use faer_ext::IntoNalgebra;
 
-use super::{GraphOptimizer, OptError, OptObserverVec, OptParams, OptResult, Optimizer};
+use super::{OptError, OptObserverVec, OptParams, OptResult, Optimizer};
 use crate::{
     containers::{Graph, GraphOrder, Values, ValuesOrder},
     dtype,
@@ -51,8 +51,8 @@ pub struct LevenMarquardt<S: LinearSolver = CholeskySolver> {
     graph_order: Option<GraphOrder>,
 }
 
-impl<S: LinearSolver> GraphOptimizer for LevenMarquardt<S> {
-    fn new(graph: Graph) -> Self {
+impl<S: LinearSolver> LevenMarquardt<S> {
+    pub fn new(graph: Graph) -> Self {
         Self {
             graph,
             solver: S::default(),
@@ -64,7 +64,7 @@ impl<S: LinearSolver> GraphOptimizer for LevenMarquardt<S> {
         }
     }
 
-    fn graph(&self) -> &Graph {
+    pub fn graph(&self) -> &Graph {
         &self.graph
     }
 }

src/optimizers/macros.rs

@@ -5,35 +5,43 @@
 /// - Between optimization for VectorVar3, SO3, and SE3
 #[macro_export]
 macro_rules! test_optimizer {
-    ($optimizer:ty) => {
+    ($o:ty) => {
         #[test]
         fn priorvector3() {
-            $crate::optimizers::test::optimize_prior::<$optimizer, 3, $crate::variables::VectorVar3>()
+            let f = |graph| <$o>::new(graph);
+            $crate::optimizers::test::optimize_prior::<$o, 3, $crate::variables::VectorVar3>(&f)
         }
 
         #[test]
         fn priorso3() {
-            $crate::optimizers::test::optimize_prior::<$optimizer, 3, $crate::variables::SO3>();
+            let f = |graph| <$o>::new(graph);
+            $crate::optimizers::test::optimize_prior::<$o, 3, $crate::variables::SO3>(&f);
         }
 
         #[test]
         fn priorse3() {
-            $crate::optimizers::test::optimize_prior::<$optimizer, 6, $crate::variables::SE3>();
+            let f = |graph| <$o>::new(graph);
+            $crate::optimizers::test::optimize_prior::<$o, 6, $crate::variables::SE3>(&f);
         }
 
         #[test]
         fn betweenvector3() {
-            $crate::optimizers::test::optimize_between::<$optimizer, 3, 6, $crate::variables::VectorVar3>();
+            let f = |graph| <$o>::new(graph);
+            $crate::optimizers::test::optimize_between::<$o, 3, 6, $crate::variables::VectorVar3>(
+                &f,
+            );
         }
 
         #[test]
         fn betweenso3() {
-            $crate::optimizers::test::optimize_between::<$optimizer, 3, 6, $crate::variables::SO3>();
+            let f = |graph| <$o>::new(graph);
+            $crate::optimizers::test::optimize_between::<$o, 3, 6, $crate::variables::SO3>(&f);
         }
 
         #[test]
         fn betweense3() {
-            $crate::optimizers::test::optimize_between::<$optimizer, 6, 12, $crate::variables::SE3>();
+            let f = |graph| <$o>::new(graph);
+            $crate::optimizers::test::optimize_between::<$o, 6, 12, $crate::variables::SE3>(&f);
         }
     };
 }

src/optimizers/mod.rs

@@ -35,9 +35,7 @@
 //! using the [test_optimizer](crate::test_optimizer) macro to run a handful of
 //! simple tests over a few different variable types to ensure correctness.
 mod traits;
-pub use traits::{
-    GraphOptimizer, OptError, OptObserver, OptObserverVec, OptParams, OptResult, Optimizer,
-};
+pub use traits::{OptError, OptObserver, OptObserverVec, OptParams, OptResult, Optimizer};
 
 mod macros;
 
@@ -58,7 +56,6 @@ pub mod test {
         containers::{FactorBuilder, Graph, Values},
         dtype,
         linalg::{AllocatorBuffer, Const, DualAllocator, DualVector, VectorX},
-        noise::{NoiseModel, UnitNoise},
         residuals::{BetweenResidual, PriorResidual, Residual},
         symbols::X,
         variables::VariableDtype,
@@ -69,11 +66,11 @@ pub mod test {
         const DIM: usize,
         #[cfg(feature = "serde")] T: VariableDtype<Dim = nalgebra::Const<DIM>> + 'static + typetag::Tagged,
         #[cfg(not(feature = "serde"))] T: VariableDtype<Dim = nalgebra::Const<DIM>> + 'static,
-    >()
-    where
-        UnitNoise<DIM>: NoiseModel,
+    >(
+        new: &dyn Fn(Graph) -> O,
+    ) where
         PriorResidual<T>: Residual,
-        O: Optimizer<Input = Values> + GraphOptimizer,
+        O: Optimizer<Input = Values>,
     {
         let t = VectorX::from_fn(T::DIM, |_, i| ((i + 1) as dtype) / 10.0);
         let p = T::exp(t.as_view());
@@ -86,7 +83,7 @@ pub mod test {
         let factor = FactorBuilder::new1_unchecked(res, X(0)).build();
         graph.add_factor(factor);
 
-        let mut opt = O::new(graph);
+        let mut opt = new(graph);
         values = opt.optimize(values).expect("Optimization failed");
 
         let out: &T = values.get_unchecked(X(0)).expect("Missing X(0)");
@@ -104,12 +101,12 @@ pub mod test {
         const DIM_DOUBLE: usize,
         #[cfg(feature = "serde")] T: VariableDtype<Dim = nalgebra::Const<DIM>> + 'static + typetag::Tagged,
         #[cfg(not(feature = "serde"))] T: VariableDtype<Dim = nalgebra::Const<DIM>> + 'static,
-    >()
-    where
-        UnitNoise<DIM>: NoiseModel,
+    >(
+        new: &dyn Fn(Graph) -> O,
+    ) where
         PriorResidual<T>: Residual,
         BetweenResidual<T>: Residual,
-        O: Optimizer<Input = Values> + GraphOptimizer,
+        O: Optimizer<Input = Values>,
         Const<DIM>: ToTypenum,
         AllocatorBuffer<DimNameSum<Const<DIM>, Const<DIM>>>: Sync + Send,
         DefaultAllocator: DualAllocator<DimNameSum<Const<DIM>, Const<DIM>>>,
@@ -136,7 +133,7 @@ pub mod test {
         let factor = FactorBuilder::new2_unchecked(res, X(0), X(1)).build();
         graph.add_factor(factor);
 
-        let mut opt = O::new(graph);
+        let mut opt = new(graph);
         values = opt.optimize(values).expect("Optimization failed");
 
         let out1: &T = values.get_unchecked(X(0)).expect("Missing X(0)");

src/optimizers/traits.rs

@@ -1,4 +1,4 @@
-use crate::{containers::Graph, dtype};
+use crate::dtype;
 
 /// Error types for optimizers
 #[derive(Debug)]
@@ -165,10 +165,3 @@ pub trait Optimizer {
         Err(OptError::MaxIterations(values))
     }
 }
-
-/// Small trait for optimizers that work on a graph
-pub trait GraphOptimizer: Optimizer {
-    fn new(graph: Graph) -> Self;
-
-    fn graph(&self) -> &Graph;
-}