Trait Variable

pub trait Variable:
    Clone
    + Sized
    + Display
    + Debug {
    type T: Numeric;
    type Dim: DimName;
    type Alias<TT: Numeric>: Variable<T = TT>;

    const DIM: usize = <Self::Dim>::USIZE;

    // Required methods
    fn identity() -> Self;
    fn inverse(&self) -> Self;
    fn compose(&self, other: &Self) -> Self;
    fn exp(delta: VectorViewX<'_, Self::T>) -> Self;
    fn log(&self) -> VectorX<Self::T>;
    fn cast<TT: Numeric + SupersetOf<Self::T>>(&self) -> Self::Alias<TT>;

    // Provided methods
    fn dim(&self) -> usize { ... }
    fn oplus(&self, xi: VectorViewX<'_, Self::T>) -> Self { ... }
    fn oplus_right(&self, xi: VectorViewX<'_, Self::T>) -> Self { ... }
    fn oplus_left(&self, xi: VectorViewX<'_, Self::T>) -> Self { ... }
    fn ominus(&self, y: &Self) -> VectorX<Self::T> { ... }
    fn ominus_right(&self, y: &Self) -> VectorX<Self::T> { ... }
    fn ominus_left(&self, y: &Self) -> VectorX<Self::T> { ... }
    fn minus(&self, other: &Self) -> Self { ... }
    fn dual_exp<N: DimName>(idx: usize) -> Self::Alias<DualVector<N>>
       where AllocatorBuffer<N>: Sync + Send,
             DefaultAllocator: DualAllocator<N>,
             DualVector<N>: Copy { ... }
    fn dual<N: DimName>(&self, idx: usize) -> Self::Alias<DualVector<N>>
       where AllocatorBuffer<N>: Sync + Send,
             DefaultAllocator: DualAllocator<N>,
             DualVector<N>: Copy + SupersetOf<Self::T> { ... }
}

Variable trait for Lie groups

All variables must implement this trait to be used in the optimization algorithms. See module level documentation for more details.

Provided Associated Constants

const DIM: usize = <Self::Dim>::USIZE

Required Associated Types

type T: Numeric

Datatype of the variable

type Dim: DimName

Dimension of the Lie group / Tangent space

type Alias<TT: Numeric>: Variable<T = TT>

Alias for the type for dual conversion

Required Methods

fn identity() -> Self

Identity element of the group

fn inverse(&self) -> Self

Inverse of the group element

fn compose(&self, other: &Self) -> Self

Composition of two group elements

fn exp(delta: VectorViewX<'_, Self::T>) -> Self

Exponential map (trivial if a vector space)

fn log(&self) -> VectorX<Self::T>

Logarithm map (trivial if a vector space)

fn cast<TT: Numeric + SupersetOf<Self::T>>(&self) -> Self::Alias<TT>

Conversion to dual space

Simply convert all interior values of dtype to DD.

Provided Methods

fn dim(&self) -> usize

Dimension helper

fn oplus(&self, xi: VectorViewX<'_, Self::T>) -> Self

Adds value from the tangent space to the group element

By default this uses the “right” version ¹ $$ x \oplus \xi = x \cdot \exp(\xi) $$ If the “left” feature is enabled, instead this turns to $$ x \oplus \xi = \exp(\xi) \cdot x $$

---

1. Solà, Joan, et al. “A Micro Lie Theory for State Estimation in Robotics.” Arxiv:1812.01537, Dec. 2021 ↩

fn oplus_right(&self, xi: VectorViewX<'_, Self::T>) -> Self

fn oplus_left(&self, xi: VectorViewX<'_, Self::T>) -> Self

fn ominus(&self, y: &Self) -> VectorX<Self::T>

Compares two group elements in the tangent space

By default this uses the “right” version ¹ $$ x \ominus y = \log(y^{-1} \cdot x) $$ If the “left” feature is enabled, instead this turns to $$ x \ominus y = \log(x \cdot y^{-1}) $$

---

1. Solà, Joan, et al. “A Micro Lie Theory for State Estimation in Robotics.” Arxiv:1812.01537, Dec. 2021 ↩

Examples found in repository

examples/gps.rs (line 62)

    fn residual1<T: Numeric>(&self, v: SE2<T>) -> VectorX<T> {
        // Convert measurement from dtype to T
        let p_meas = self.meas.cast();
        // Convert p to VectorVar2 as well
        let p = VectorVar2::from(v.xy().into_owned());

        p.ominus(&p_meas)
    }

fn ominus_right(&self, y: &Self) -> VectorX<Self::T>

fn ominus_left(&self, y: &Self) -> VectorX<Self::T>

fn minus(&self, other: &Self) -> Self

Subtract out portion from other variable.

This can be seen as a “tip-to-tail” computation. IE it computes the transformation between two poses. I like to think of it as “taking away” the portion subtracted out, for example given a chain of poses $a, b, c$, the following “removes” the portion from $a$ to $b$.

$$ {}_a T_c \boxminus {}_a T_b = ({}_a T_b)^{-1} {}_a T_c = {}_b T_c $$

This operation is NOT effected by the left/right feature.

Examples found in repository

examples/readme.rs (line 26)

fn main() {
    // Make all the values
    let mut values = Values::new();

    let x = SO2::from_theta(1.0);
    let y = SO2::from_theta(2.0);
    values.insert(X(0), SO2::identity());
    values.insert(X(1), SO2::identity());

    // Make the factors & insert into graph
    let mut graph = Graph::new();
    let res = PriorResidual::new(x.clone());
    let factor = fac![res, X(0)];
    graph.add_factor(factor);

    let res = BetweenResidual::new(y.minus(&x));
    let factor = fac![res, (X(0), X(1)), 0.1 as std, Huber::default()];
    graph.add_factor(factor);

    // Optimize!
    let mut opt: GaussNewton = GaussNewton::new(graph);
    let result = opt.optimize(values).unwrap();
    println!("Results {:#}", result);
}

fn dual_exp<N: DimName>(idx: usize) -> Self::Alias<DualVector<N>>

where AllocatorBuffer<N>: Sync + Send, DefaultAllocator: DualAllocator<N>, DualVector<N>: Copy,

Setup group element correctly using the tangent space

By default this uses the exponential map to propagate dual numbers to the variable to setup the jacobian properly. Can be hardcoded to avoid the repeated computation.

fn dual<N: DimName>(&self, idx: usize) -> Self::Alias<DualVector<N>>

where AllocatorBuffer<N>: Sync + Send, DefaultAllocator: DualAllocator<N>, DualVector<N>: Copy + SupersetOf<Self::T>,

Applies the tangent vector in dual space

Takes the results from dual_exp and applies the tangent vector using the right/left oplus operator.

Dyn Compatibility

This trait is not dyn compatible.

In older versions of Rust, dyn compatibility was called "object safety", so this trait is not object safe.

Implementors

impl<T: Numeric> Variable for ImuBias<T>

type T = T

type Dim = Const<6>

type Alias<TT: Numeric> = ImuBias<TT>

impl<T: Numeric> Variable for SE2<T>

type T = T

type Dim = Const<3>

type Alias<TT: Numeric> = SE2<TT>

impl<T: Numeric> Variable for SE3<T>

type T = T

type Dim = Const<6>

type Alias<TT: Numeric> = SE3<TT>

impl<T: Numeric> Variable for SO2<T>

type T = T

type Dim = Const<1>

type Alias<TT: Numeric> = SO2<TT>

impl<T: Numeric> Variable for SO3<T>

type T = T

type Dim = Const<3>

type Alias<TT: Numeric> = SO3<TT>

impl<const N: usize, T: Numeric> Variable for VectorVar<N, T>

type T = T

type Dim = Const<N>

type Alias<TT: Numeric> = VectorVar<N, TT>