Merge pull request #10 from contagon/easton/preint

Imu Preintegration

Cargo.toml

@@ -31,7 +31,7 @@ typetag = { version = "0.2.16", optional = true }
 serde_json = { version = "1.0.120", optional = true }
 
 # rerun support
-rerun = { version = "0.17", optional = true, default-features = false, features = [
+rerun = { version = "0.18", optional = true, default-features = false, features = [
     "sdk",
 ] }
 

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
+/// We strongly recommend using [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/nalgebra_wrap.rs

@@ -5,6 +5,7 @@ pub use nalgebra::{
     allocator::Allocator,
     dmatrix as matrixx,
     dvector as vectorx,
+    ComplexField,
     Const,
     DefaultAllocator,
     Dim,
@@ -113,7 +114,7 @@ pub type VectorView5<'a, D = dtype> = na::VectorView<'a, D, Const<5>>;
 pub type VectorView6<'a, D = dtype> = na::VectorView<'a, D, Const<6>>;
 
 // Generic, taking in sizes with Const
-pub type VectorDim<N> = OVector<dtype, N>;
+pub type VectorDim<N, D = dtype> = OVector<D, N>;
 pub type MatrixDim<R, C = Const<1>, D = dtype> =
     na::Matrix<D, R, C, <na::DefaultAllocator as Allocator<D, R, C>>::Buffer>;
 pub type MatrixViewDim<'a, R, C = Const<1>, D = dtype> = na::MatrixView<'a, D, R, C>;