Imu Preintegration

README.md

@@ -16,7 +16,16 @@ We recommend you checkout the [docs](https://docs.rs/factrs/latest/factrs/) (WIP
 ## Example
 
 ```rust
-use factrs::prelude::*;
+use factrs::{
+   assign_symbols,
+   containers::{FactorBuilder, Graph, Values},
+   noise::GaussianNoise,
+   optimizers::GaussNewton,
+   residuals::{BetweenResidual, PriorResidual},
+   robust::Huber,
+   traits::*,
+   variables::SO2,
+};
 
 // Assign symbols to variable types
 assign_symbols!(X: SO2);

examples/g2o-rerun.rs

@@ -13,7 +13,7 @@ use factrs::{
 use rerun::{Arrows2D, Arrows3D, Points2D, Points3D};
 
 fn main() -> Result<(), Box<dyn std::error::Error>> {
-    // ------------------------- Parse Arguments & Load data ------------------------- //
+    // ---------------------- Parse Arguments & Load data ---------------------- //
     let args: Vec<String> = env::args().collect();
     if args.len() < 2 {
         println!("Usage: {} <g2o file> <points/arrows>", args[0]);

examples/serde.rs

@@ -1,8 +1,7 @@
 use factrs::{
-    containers::{Graph, Values, X},
+    containers::{FactorBuilder, Graph, Values, X},
     factors::Factor,
     noise::GaussianNoise,
-    prelude::FactorBuilder,
     residuals::{BetweenResidual, PriorResidual},
     robust::{GemanMcClure, L2},
     variables::{SE2, SO2},

src/containers/factor.rs

@@ -31,7 +31,15 @@ use crate::{
 /// [LinearFactor].
 ///
 ///  ```
-/// # use factrs::prelude::*;
+/// # use factrs::{
+///    assign_symbols,
+///    containers::FactorBuilder,
+///    noise::GaussianNoise,
+///    optimizers::GaussNewton,
+///    residuals::{PriorResidual},
+///    robust::GemanMcClure,
+///    variables::VectorVar3,
+/// };
 /// # assign_symbols!(X: VectorVar3);
 /// let prior = VectorVar3::new(1.0, 2.0, 3.0);
 /// let residual = PriorResidual::new(prior);

src/containers/graph.rs

@@ -14,7 +14,14 @@ use crate::{containers::Factor, dtype, linear::LinearGraph};
 /// solved iteratively.
 ///
 /// ```
-/// # use factrs::prelude::*;
+/// # use factrs::{
+///    assign_symbols,
+///    containers::{Graph, FactorBuilder},
+///    residuals::PriorResidual,
+///    robust::GemanMcClure,
+///    traits::*,
+///    variables::SO2,
+/// };
 /// # assign_symbols!(X: SO2);
 /// # let factor = FactorBuilder::new1(PriorResidual::new(SO2::identity()), X(0)).build();
 /// let mut graph = Graph::new();

src/containers/symbol.rs

@@ -4,7 +4,7 @@ use std::{
     mem::size_of,
 };
 
-use crate::prelude::VariableUmbrella;
+use crate::variables::VariableUmbrella;
 
 // Char is stored in last CHR_BITS
 // Value is stored in the first IDX_BITS
@@ -83,7 +83,7 @@ pub trait TypedSymbol<V: VariableUmbrella>: Symbol {}
 /// with the type they will be used with. This macro will create a new symbol
 /// and implement all the necessary traits for it to be used as a symbol.
 /// ```
-/// use factrs::prelude::*;
+/// use factrs::{assign_symbols, variables::{SO2, SE2}};
 /// assign_symbols!(X: SO2; Y: SE2);
 /// ```
 #[macro_export]

src/containers/values.rs

@@ -2,11 +2,10 @@ use std::{collections::hash_map::Entry, default::Default, fmt, iter::IntoIterato
 
 use ahash::AHashMap;
 
-use super::{Key, Symbol, TypedSymbol};
+use super::{DefaultSymbol, Key, Symbol, TypedSymbol};
 use crate::{
     linear::LinearValues,
-    prelude::{DefaultSymbol, VariableUmbrella},
-    variables::VariableSafe,
+    variables::{VariableSafe, VariableUmbrella},
 };
 
 // Since we won't be passing dual numbers through any of this,
@@ -19,7 +18,11 @@ use crate::{
 /// Values, it must implement [Variable](crate::variables::Variable), and then
 /// will implement [VariableSafe] via a blanket implementation.
 /// ```
-/// # use factrs::prelude::*;
+/// # use factrs::{
+///    assign_symbols,
+///    containers::Values,
+///    variables::SO2,
+/// };
 /// # assign_symbols!(X: SO2);
 /// let x = SO2::from_theta(0.1);
 /// let mut values = Values::new();
@@ -81,7 +84,11 @@ impl Values {
     /// symbol and as such is guaranteed to return the correct type. Returns
     /// None if key isn't found.
     /// ```
-    /// # use factrs::prelude::*;
+    /// # use factrs::{
+    ///    assign_symbols,
+    ///    containers::Values,
+    ///    variables::SO2,
+    /// };
     /// # assign_symbols!(X: SO2);
     /// # let x = SO2::from_theta(0.1);
     /// # let mut values = Values::new();
@@ -150,7 +157,12 @@ impl Values {
     /// the values.
     ///
     /// ```
-    /// # use factrs::prelude::*;
+    /// # use factrs::{
+    ///    assign_symbols,
+    ///    containers::Values,
+    ///    traits::*,
+    ///    variables::SO2,
+    /// };
     /// # assign_symbols!(X: SO2);
     /// # let mut values = Values::new();
     /// # (0..10).for_each(|i| {values.insert(X(0), SO2::identity());} );

src/lib.rs

@@ -36,7 +36,16 @@
 //!
 //! # Example
 //! ```
-//! use factrs::prelude::*;
+//! use factrs::{
+//!    assign_symbols,
+//!    containers::{FactorBuilder, Graph, Values},
+//!    noise::GaussianNoise,
+//!    optimizers::GaussNewton,
+//!    residuals::{BetweenResidual, PriorResidual},
+//!    robust::Huber,
+//!    traits::*,
+//!    variables::SO2,
+//! };
 //!
 //! // Assign symbols to variable types
 //! assign_symbols!(X: SO2);
@@ -89,34 +98,31 @@ pub mod robust;
 pub mod utils;
 pub mod variables;
 
-/// Untagged symnbols if `unchecked` API is desired.
+/// Untagged symbols if `unchecked` API is desired.
 ///
 /// We strongly recommend using [assign_symbols](crate::assign_symbols) to
 /// create and tag symbols with the appropriate types. However, we provide a
-/// number of pre-defined symbols if desired. Note this objects can't be tagged
+/// number of pre-defined symbols if desired. Note these objects can't be tagged
 /// due to the orphan rules.
 pub mod symbols {
     crate::assign_symbols!(
         A, B, C, D, E, F, G, H, I, J, K, L, M, N, O, P, Q, R, S, T, U, V, W, X, Y, Z
     );
 }
 
-/// Helper module to import common types
+/// Helper module to import common traits
 ///
-/// This module is meant to be glob imported to make it easier to use the
-/// library.
+/// This module is meant to be glob imported to make it easier to use the traits
+/// and functionality in the library.
 /// ```
-/// use factrs::prelude::*;
+/// use factrs::traits::*;
 /// ```
-pub mod prelude {
+pub mod traits {
     pub use crate::{
-        assign_symbols,
-        containers::*,
-        noise::*,
-        optimizers::*,
-        residuals::*,
-        robust::*,
-        variables::*,
+        linalg::{Diff, DualConvert},
+        optimizers::{GraphOptimizer, Optimizer},
+        residuals::Residual,
+        variables::Variable,
     };
 }
 

src/linalg/dual.rs

@@ -1,6 +1,6 @@
 use nalgebra::{allocator::Allocator, Const};
 
-use super::{Dim, RealField};
+use super::{Dim, Matrix, RealField};
 use crate::dtype;
 
 /// Wrapper for all properties needed for dual numbers
@@ -28,3 +28,16 @@ impl<
     > DualAllocator<N> for T
 {
 }
+
+// TODO: Expand on this instead of including in Variable??
+pub trait DualConvert {
+    type Alias<D: Numeric>;
+    fn dual_convert<D: Numeric>(other: &Self::Alias<dtype>) -> Self::Alias<D>;
+}
+
+impl<const R: usize, const C: usize, T: Numeric> DualConvert for Matrix<R, C, T> {
+    type Alias<D: Numeric> = Matrix<R, C, D>;
+    fn dual_convert<D: Numeric>(other: &Self::Alias<dtype>) -> Self::Alias<D> {
+        other.map(|x| x.into())
+    }
+}

src/linalg/forward_prop.rs

@@ -23,7 +23,11 @@ use crate::{
 /// inputs and with vector-valued outputs.
 ///
 /// ```
-/// use factrs::{linalg::*, prelude::*};
+/// use factrs::{
+///     linalg::{vectorx, Const, DiffResult, ForwardProp, Numeric, VectorX},
+///     traits::*,
+///     variables::SO2,
+/// };
 ///
 /// fn f<D: Numeric>(x: SO2<D>, y: SO2<D>) -> VectorX<D> {
 ///     x.ominus(&y)

src/linalg/mod.rs

@@ -11,7 +11,7 @@
 use crate::dtype;
 
 mod dual;
-pub use dual::{DualAllocator, DualScalar, DualVector, Numeric};
+pub use dual::{DualAllocator, DualConvert, DualScalar, DualVector, Numeric};
 // Dual numbers
 pub use num_dual::Derivative;
 

src/linalg/numerical_diff.rs

@@ -17,7 +17,11 @@ use crate::{
 /// inputs and with vector-valued outputs.
 ///
 /// ```
-/// use factrs::{linalg::*, prelude::*};
+/// use factrs::{
+///     linalg::{vectorx, DiffResult, NumericalDiff, VectorX},
+///     traits::*,
+///     variables::SO2,
+/// };
 ///
 /// // We can also be generic over Numeric as in [ForwardProp] as well if desired
 /// fn f(x: SO2, y: SO2) -> VectorX {

src/noise/gaussian.rs

@@ -20,6 +20,9 @@ tag_noise!(
     GaussianNoise<10>,
     GaussianNoise<11>,
     GaussianNoise<12>,
+    GaussianNoise<13>,
+    GaussianNoise<14>,
+    GaussianNoise<15>,
 );
 
 /// A Gaussian noise model.

src/noise/unit.rs

@@ -17,6 +17,9 @@ tag_noise!(
     UnitNoise<10>,
     UnitNoise<11>,
     UnitNoise<12>,
+    UnitNoise<13>,
+    UnitNoise<14>,
+    UnitNoise<15>,
 );
 
 /// A unit noise model.

src/optimizers/mod.rs

@@ -61,11 +61,10 @@ pub mod test {
 
     use super::*;
     use crate::{
-        containers::{Graph, Values},
+        containers::{FactorBuilder, Graph, Values},
         dtype,
         linalg::{AllocatorBuffer, Const, DualAllocator, DualVector, VectorX},
         noise::{NoiseModelSafe, UnitNoise},
-        prelude::FactorBuilder,
         residuals::{BetweenResidual, PriorResidual, Residual, ResidualSafe},
         symbols::X,
         variables::VariableUmbrella,