Merge pull request #24 from rpl-cmu/easton/variable-clean

Clean up variable trait

Cargo.toml

@@ -46,6 +46,7 @@ typetag = { version = "0.2.18", optional = true, path = "./factrs-typetag" }
 rerun = { version = "0.20", optional = true, default-features = false, features = [
     "sdk",
 ] }
+simba = { version = "0.9.0", default-features = false }
 
 [features]
 # Run everything with f32 instead of the default f64

examples/gps.rs

@@ -56,7 +56,7 @@ impl Residual1 for GpsResidual {
     // D is a custom numeric type that can be leveraged for autodiff
     fn residual1<T: Numeric>(&self, v: SE2<T>) -> VectorX<T> {
         // Convert measurement from dtype to T
-        let p_meas = VectorVar2::<T>::dual_convert(&self.meas);
+        let p_meas = self.meas.cast();
         // Convert p to VectorVar2 as well
         let p = VectorVar2::from(v.xy().into_owned());
 

src/containers/symbol.rs

@@ -4,7 +4,7 @@ use std::{
     mem::size_of,
 };
 
-use crate::variables::VariableUmbrella;
+use crate::variables::VariableDtype;
 
 // ------------------------- Symbol Basics ------------------------- //
 
@@ -27,7 +27,7 @@ pub trait Symbol: fmt::Debug + Into<Key> {}
 /// Adds type information to a symbol
 ///
 /// Will almost always be generated by [assign_symbols]
-pub trait TypedSymbol<V: VariableUmbrella>: Symbol {}
+pub trait TypedSymbol<V: VariableDtype>: Symbol {}
 
 /// Custom formatting for keys in [Values](factrs::containers::Values) or [Graph](factrs::containers::Graph)
 ///

src/containers/values.rs

@@ -12,7 +12,7 @@ use super::{
 };
 use crate::{
     linear::LinearValues,
-    variables::{VariableSafe, VariableUmbrella},
+    variables::{VariableDtype, VariableSafe},
 };
 
 // Since we won't be passing dual numbers through any of this,
@@ -63,7 +63,7 @@ impl Values {
     pub fn insert<S, V>(&mut self, symbol: S, value: V) -> Option<Box<dyn VariableSafe>>
     where
         S: TypedSymbol<V>,
-        V: VariableUmbrella,
+        V: VariableDtype,
     {
         self.values.insert(symbol.into(), Box::new(value))
     }
@@ -72,7 +72,7 @@ impl Values {
     pub fn insert_unchecked<S, V>(&mut self, symbol: S, value: V) -> Option<Box<dyn VariableSafe>>
     where
         S: Symbol,
-        V: VariableUmbrella,
+        V: VariableDtype,
     {
         self.values.insert(symbol.into(), Box::new(value))
     }
@@ -104,7 +104,7 @@ impl Values {
     pub fn get<S, V>(&self, symbol: S) -> Option<&V>
     where
         S: TypedSymbol<V>,
-        V: VariableUmbrella,
+        V: VariableDtype,
     {
         self.values
             .get(&symbol.into())
@@ -115,7 +115,7 @@ impl Values {
     pub fn get_unchecked<S, V>(&self, symbol: S) -> Option<&V>
     where
         S: Symbol,
-        V: VariableUmbrella,
+        V: VariableDtype,
     {
         self.values
             .get(&symbol.into())
@@ -126,7 +126,7 @@ impl Values {
     pub fn get_mut<S, V>(&mut self, symbol: S) -> Option<&mut V>
     where
         S: TypedSymbol<V>,
-        V: VariableUmbrella,
+        V: VariableDtype,
     {
         self.values
             .get_mut(&symbol.into())
@@ -137,7 +137,7 @@ impl Values {
     pub fn get_unchecked_mut<S, V>(&mut self, symbol: S) -> Option<&mut V>
     where
         S: Symbol,
-        V: VariableUmbrella,
+        V: VariableDtype,
     {
         self.values
             .get_mut(&symbol.into())
@@ -147,7 +147,7 @@ impl Values {
     pub fn remove<S, V>(&mut self, symbol: S) -> Option<V>
     where
         S: TypedSymbol<V>,
-        V: VariableUmbrella,
+        V: VariableDtype,
     {
         self.values
             .remove(&symbol.into())

src/lib.rs

@@ -127,7 +127,7 @@ pub mod symbols {
 /// ```
 pub mod traits {
     pub use crate::{
-        linalg::{Diff, DualConvert},
+        linalg::Diff,
         optimizers::{GraphOptimizer, Optimizer},
         residuals::Residual,
         variables::Variable,

src/linalg/dual.rs

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

src/linalg/forward_prop.rs

@@ -5,9 +5,8 @@ use super::{
     AllocatorBuffer, Diff, MatrixDim,
 };
 use crate::{
-    dtype,
     linalg::{Const, DefaultAllocator, DiffResult, DimName, Dyn, MatrixX, VectorDim, VectorX},
-    variables::Variable,
+    variables::{Variable, VariableDtype},
 };
 
 /// Forward mode differentiator
@@ -46,12 +45,12 @@ macro_rules! forward_maker {
     ($num:expr, $( ($name:ident: $var:ident) ),*) => {
         paste! {
             #[allow(unused_assignments)]
-            fn [<jacobian_ $num>]<$( $var: Variable<Alias<dtype> = $var>, )* F: Fn($($var::Alias<Self::T>,)*) -> VectorX<Self::T>>
+            fn [<jacobian_ $num>]<$( $var: VariableDtype, )* F: Fn($($var::Alias<Self::T>,)*) -> VectorX<Self::T>>
                     (f: F, $($name: &$var,)*) -> DiffResult<VectorX, MatrixX>{
                 // Prepare variables
                 let mut curr_dim = 0;
                 $(
-                    let $name: $var::Alias<Self::T> = $var::dual($name, curr_dim);
+                    let $name: $var::Alias<Self::T> = $name.dual(curr_dim);
                     curr_dim += $name.dim();
                 )*
 

src/linalg/mod.rs

@@ -11,7 +11,7 @@
 use crate::dtype;
 
 mod dual;
-pub use dual::{DualAllocator, DualConvert, DualScalar, DualVector, Numeric};
+pub use dual::{DualAllocator, DualScalar, DualVector, Numeric};
 // Dual numbers
 pub use num_dual::Derivative;
 
@@ -62,7 +62,7 @@ impl MatrixBlock {
 // ------------------------- Derivatives ------------------------- //
 use paste::paste;
 
-use crate::variables::Variable;
+use crate::variables::VariableDtype;
 
 /// A struct to hold the result of a differentiation operation
 #[derive(Debug, Clone)]
@@ -74,7 +74,7 @@ pub struct DiffResult<V, G> {
 macro_rules! fn_maker {
     (grad, $num:expr, $( ($name:ident: $var:ident) ),*) => {
         paste! {
-            fn [<gradient_ $num>]<$( $var: Variable<Alias<dtype> = $var>, )* F: Fn($($var::Alias<Self::T>,)*) -> Self::T>
+            fn [<gradient_ $num>]<$( $var: VariableDtype, )* F: Fn($($var::Alias<Self::T>,)*) -> Self::T>
                     (f: F, $($name: &$var,)*) -> DiffResult<dtype, VectorX>{
                     let f_wrapped = |$($name: $var::Alias<Self::T>,)*| vectorx![f($($name.clone(),)*)];
                     let DiffResult { value, diff } = Self::[<jacobian_ $num>](f_wrapped, $($name,)*);
@@ -86,7 +86,7 @@ macro_rules! fn_maker {
 
     (jac, $num:expr, $( ($name:ident: $var:ident) ),*) => {
         paste! {
-            fn [<jacobian_ $num>]<$( $var: Variable<Alias<$crate::dtype>=$var>, )* F: Fn($($var::Alias<Self::T>,)*) -> VectorX<Self::T>>
+            fn [<jacobian_ $num>]<$( $var: VariableDtype, )* F: Fn($($var::Alias<Self::T>,)*) -> VectorX<Self::T>>
                     (f: F, $($name: &$var,)*) -> DiffResult<VectorX, MatrixX>;
         }
     };

src/linalg/nalgebra_wrap.rs

@@ -5,6 +5,7 @@ pub use nalgebra::{
     allocator::Allocator, dmatrix as matrixx, dvector as vectorx, ComplexField, Const,
     DefaultAllocator, Dim, DimName, Dyn, RealField,
 };
+pub use simba::scalar::SupersetOf;
 
 use crate::dtype;
 

src/linalg/numerical_diff.rs

@@ -3,7 +3,7 @@ use paste::paste;
 use crate::{
     dtype,
     linalg::{Diff, DiffResult, MatrixX, VectorX},
-    variables::Variable,
+    variables::{Variable, VariableDtype},
 };
 
 /// Forward mode differentiator
@@ -41,19 +41,19 @@ macro_rules! numerical_maker {
     ($num:expr, $( ($idx:expr, $name:ident, $var:ident) ),*) => {
         paste! {
             #[allow(unused_assignments)]
-            fn [<jacobian_$num>]<$( $var: Variable, )* F: Fn($($var,)*) -> VectorX>
+            fn [<jacobian_$num>]<$( $var: VariableDtype, )* F: Fn($($var,)*) -> VectorX>
                     (f: F, $($name: &$var,)*) -> DiffResult<VectorX, MatrixX> {
                 let eps = dtype::powi(10.0, -PWR);
 
                 // Get Dimension
                 let mut dim = 0;
-                $(dim += $name.dim();)*
+                $(dim += Variable::dim($name);)*
 
                 let res = f($( $name.clone(), )*);
 
                 // Compute gradient
                 let mut jac: MatrixX = MatrixX::zeros(res.len(), dim);
-                let mut tvs = [$( VectorX::zeros($name.dim()), )*];
+                let mut tvs = [$( VectorX::zeros(Variable::dim($name)), )*];
 
                 for i in 0..$num {
                     let mut curr_dim = 0;
@@ -113,19 +113,19 @@ macro_rules! numerical_variable_maker {
     ($num:expr, $( ($idx:expr, $name:ident, $var:ident) ),*) => {
         paste! {
             #[allow(unused_assignments)]
-            pub fn [<jacobian_variable_$num>]<$( $var: Variable, )* VOut: Variable, F: Fn($($var,)*) -> VOut>
+            pub fn [<jacobian_variable_$num>]<$( $var: VariableDtype, )* VOut: VariableDtype, F: Fn($($var,)*) -> VOut>
                     (f: F, $($name: &$var,)*) -> DiffResult<VOut, MatrixX> {
                 let eps = dtype::powi(10.0, -PWR);
 
                 // Get Dimension
                 let mut dim = 0;
-                $(dim += $name.dim();)*
+                $(dim += Variable::dim($name);)*
 
                 let res = f($( $name.clone(), )*);
 
                 // Compute gradient
                 let mut jac: MatrixX = MatrixX::zeros(VOut::DIM, dim);
-                let mut tvs = [$( VectorX::zeros($name.dim()), )*];
+                let mut tvs = [$( VectorX::zeros(Variable::dim($name)), )*];
 
                 for i in 0..$num {
                     let mut curr_dim = 0;

src/optimizers/mod.rs

@@ -61,14 +61,14 @@ pub mod test {
         noise::{NoiseModel, UnitNoise},
         residuals::{BetweenResidual, PriorResidual, Residual},
         symbols::X,
-        variables::VariableUmbrella,
+        variables::VariableDtype,
     };
 
     pub fn optimize_prior<
         O,
         const DIM: usize,
-        #[cfg(feature = "serde")] T: VariableUmbrella<Dim = nalgebra::Const<DIM>> + 'static + typetag::Tagged,
-        #[cfg(not(feature = "serde"))] T: VariableUmbrella<Dim = nalgebra::Const<DIM>> + 'static,
+        #[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,
@@ -102,8 +102,8 @@ pub mod test {
         O,
         const DIM: usize,
         const DIM_DOUBLE: usize,
-        #[cfg(feature = "serde")] T: VariableUmbrella<Dim = nalgebra::Const<DIM>> + 'static + typetag::Tagged,
-        #[cfg(not(feature = "serde"))] T: VariableUmbrella<Dim = nalgebra::Const<DIM>> + 'static,
+        #[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,

src/rerun.rs

@@ -6,7 +6,7 @@ use rerun::{
 use crate::{
     containers::Values,
     optimizers::OptObserver,
-    variables::{MatrixLieGroup, VariableUmbrella, VectorVar2, VectorVar3, SE2, SE3, SO2, SO3},
+    variables::{MatrixLieGroup, VariableDtype, VectorVar2, VectorVar3, SE2, SE3, SO2, SO3},
 };
 /*
 Each of our fact.rs types can be turned into a handful of rerun types. These include,
@@ -384,7 +384,7 @@ impl<'a> FromIterator<&'a SE3> for Points3D {
 // ------------------------- Streamer ------------------------- //
 pub struct RerunObserver<V, R>
 where
-    V: VariableUmbrella + 'static,
+    V: VariableDtype + 'static,
     R: AsComponents,
     for<'a> R: FromIterator<&'a V>,
 {
@@ -396,7 +396,7 @@ where
 
 impl<V, R> RerunObserver<V, R>
 where
-    V: VariableUmbrella + 'static,
+    V: VariableDtype + 'static,
     R: AsComponents,
     for<'a> R: FromIterator<&'a V>,
 {
@@ -412,7 +412,7 @@ where
 
 impl<V, R> OptObserver for RerunObserver<V, R>
 where
-    V: VariableUmbrella + 'static,
+    V: VariableDtype + 'static,
     R: AsComponents,
     for<'a> R: FromIterator<&'a V>,
 {

src/residuals/between.rs

@@ -5,7 +5,7 @@ use crate::{
         AllocatorBuffer, DefaultAllocator, DualAllocator, DualVector, ForwardProp, Numeric, VectorX,
     },
     residuals::Residual2,
-    variables::{Variable, VariableUmbrella},
+    variables::{Variable, VariableDtype},
 };
 
 /// Binary factor between variables.
@@ -31,7 +31,7 @@ impl<P: Variable> BetweenResidual<P> {
 }
 
 #[factrs::mark]
-impl<P: VariableUmbrella + 'static> Residual2 for BetweenResidual<P>
+impl<P: VariableDtype + 'static> Residual2 for BetweenResidual<P>
 where
     AllocatorBuffer<DimNameSum<P::Dim, P::Dim>>: Sync + Send,
     DefaultAllocator: DualAllocator<DimNameSum<P::Dim, P::Dim>>,
@@ -45,7 +45,7 @@ where
     type DimIn = DimNameSum<P::Dim, P::Dim>;
 
     fn residual2<T: Numeric>(&self, v1: P::Alias<T>, v2: P::Alias<T>) -> VectorX<T> {
-        let delta = P::dual_convert::<T>(&self.delta);
+        let delta = self.delta.cast::<T>();
         v1.compose(&delta).ominus(&v2)
     }
 }