Trouble with custom factor

[dwuertz]

I'm trying to port the Localization example [1] from GTSAM over to factrs and ran into problems with the custom factor which is a translation of the UnaryFactor in the original code:

#[derive(Clone, Debug)]
pub struct GpsResidual {
    meas: Vector2,
}

impl GpsResidual {
    pub fn new(x: f64, y: f64) -> Self {
        Self { meas: Vector2::new(x, y) }
    }
}

impl fmt::Display for GpsResidual {
    fn fmt(&self, f: &mut fmt::Formatter) -> fmt::Result {
        write!(f, "GpsResidual({})", self.meas)
    }
}

impl Residual1 for GpsResidual
{
    type Differ = ForwardProp<Const<2>>;
    type V1 = SE2;
    type DimIn = Const<3>;
    type DimOut = Const<2>;

    fn residual1<D: Numeric>(&self, v: SE2<D>) -> VectorX<D> {
        let p_meas = Vector2::<D>::dual_convert(&self.meas);
        let p: VectorVar2<D> = v.xy().into_owned().into();
        let p_meas: VectorVar2<D> = p_meas.into();
        let r = p_meas.ominus_right(&p);
        let mut residual = VectorX::zeros(2);
        residual.fixed_rows_mut::<2>(0).copy_from(&r);
        residual
    }
}

factrs::impl_residual!(1, GpsResidual);

fn main() {
    // ...
    let mut graph = Graph::new();
    // ...
    let noise = GaussianNoise::from_scalar_sigma(1.0);
    let res = GpsResidual::new(0.5, 0.5);
    let factor = FactorBuilder::new1(res, X(0))
        .noise(noise.clone())
        .build();
    graph.add_factor(factor);
    // ...
    let mut opt: GaussNewton = GaussNewton::new(graph);
    let result = opt.optimize(values).expect("Optimization failed"); // Out-of-bounds panic here
}

The code compiles but panics due to a "Matrix index out of bounds" error during optimization.

thread 'main' panicked at /home/dude/.cargo/registry/src/index.crates.io-6f17d22bba15001f/num-dual-0.9.1/src/derivative
.rs:117:10:
Matrix index out of bounds.
stack backtrace:
   0: std::panicking::begin_panic
             at /rustc/051478957371ee0084a7c0913941d2a8c4757bb9/library/std/src/panicking.rs:693:12
   1: nalgebra::base::ops::<impl core::ops::index::IndexMut<(usize,usize)> for nalgebra::base::matrix::Matrix<T,R,C,S>>
::index_mut
             at /home/dude/.cargo/registry/src/index.crates.io-6f17d22bba15001f/nalgebra-0.32.6/src/base/ops.rs:66:9
   2: nalgebra::base::ops::<impl core::ops::index::IndexMut<usize> for nalgebra::base::matrix::Matrix<T,R,C,S>>::index_
mut
             at /home/dude/.cargo/registry/src/index.crates.io-6f17d22bba15001f/nalgebra-0.32.6/src/base/ops.rs:58:18
   3: num_dual::derivative::Derivative<T,F,R,C>::derivative_generic
             at /home/dude/.cargo/registry/src/index.crates.io-6f17d22bba15001f/num-dual-0.9.1/src/derivative.rs:117:10
   4: <factrs::variables::vector::VectorVar<_,D> as factrs::variables::traits::Variable<D>>::dual_setup
             at /home/dude/.cargo/registry/src/index.crates.io-6f17d22bba15001f/factrs-0.1.0/src/variables/vector.rs:83
:23
   5: <factrs::variables::se2::SE2<D> as factrs::variables::traits::Variable<D>>::dual_setup
             at /home/dude/.cargo/registry/src/index.crates.io-6f17d22bba15001f/factrs-0.1.0/src/variables/se2.rs:166:1
7
   6: factrs::variables::traits::Variable::dual
             at /home/dude/.cargo/registry/src/index.crates.io-6f17d22bba15001f/factrs-0.1.0/src/variables/traits.rs:15
9:21
   7: <factrs::linalg::forward_prop::ForwardProp<N> as factrs::linalg::Diff>::jacobian_1
             at /home/dude/.cargo/registry/src/index.crates.io-6f17d22bba15001f/factrs-0.1.0/src/linalg/forward_prop.rs
:56:55
   8: factrs::residuals::traits::Residual1::residual1_jacobian
             at /home/dude/.cargo/registry/src/index.crates.io-6f17d22bba15001f/factrs-0.1.0/src/residuals/traits.rs:13
8:21
   9: <factrs_test::GpsResidual as factrs::residuals::traits::Residual>::residual_jacobian
             at /home/dude/.cargo/registry/src/index.crates.io-6f17d22bba15001f/factrs-0.1.0/src/residuals/macros.rs:24
:21
  10: <T as factrs::residuals::traits::ResidualSafe>::residual_jacobian
             at /home/dude/.cargo/registry/src/index.crates.io-6f17d22bba15001f/factrs-0.1.0/src/residuals/traits.rs:68
:9
  11: factrs::containers::factor::Factor::linearize
             at /home/dude/.cargo/registry/src/index.crates.io-6f17d22bba15001f/factrs-0.1.0/src/containers/factor.rs:7
7:48
  12: factrs::containers::graph::Graph::linearize::{{closure}}
             at /home/dude/.cargo/registry/src/index.crates.io-6f17d22bba15001f/factrs-0.1.0/src/containers/graph.rs:64
:51
  13: core::iter::adapters::map::map_fold::{{closure}}
             at /rustc/051478957371ee0084a7c0913941d2a8c4757bb9/library/core/src/iter/adapters/map.rs:89:28
  14: <core::slice::iter::Iter<T> as core::iter::traits::iterator::Iterator>::fold
             at /rustc/051478957371ee0084a7c0913941d2a8c4757bb9/library/core/src/slice/iter/macros.rs:230:27
  15: <core::iter::adapters::map::Map<I,F> as core::iter::traits::iterator::Iterator>::fold
             at /rustc/051478957371ee0084a7c0913941d2a8c4757bb9/library/core/src/iter/adapters/map.rs:129:9
  16: core::iter::traits::iterator::Iterator::for_each
             at /rustc/051478957371ee0084a7c0913941d2a8c4757bb9/library/core/src/iter/traits/iterator.rs:818:9
  17: alloc::vec::Vec<T,A>::extend_trusted
             at /rustc/051478957371ee0084a7c0913941d2a8c4757bb9/library/alloc/src/vec/mod.rs:3096:17
  18: <alloc::vec::Vec<T,A> as alloc::vec::spec_extend::SpecExtend<T,I>>::spec_extend
             at /rustc/051478957371ee0084a7c0913941d2a8c4757bb9/library/alloc/src/vec/spec_extend.rs:26:9
  19: <alloc::vec::Vec<T> as alloc::vec::spec_from_iter_nested::SpecFromIterNested<T,I>>::from_iter
             at /rustc/051478957371ee0084a7c0913941d2a8c4757bb9/library/alloc/src/vec/spec_from_iter_nested.rs:62:9
  20: <alloc::vec::Vec<T> as alloc::vec::spec_from_iter::SpecFromIter<T,I>>::from_iter
             at /rustc/051478957371ee0084a7c0913941d2a8c4757bb9/library/alloc/src/vec/spec_from_iter.rs:33:9
  21: <alloc::vec::Vec<T> as core::iter::traits::collect::FromIterator<T>>::from_iter
             at /rustc/051478957371ee0084a7c0913941d2a8c4757bb9/library/alloc/src/vec/mod.rs:2970:9
  22: core::iter::traits::iterator::Iterator::collect
             at /rustc/051478957371ee0084a7c0913941d2a8c4757bb9/library/core/src/iter/traits/iterator.rs:2005:9
  23: factrs::containers::graph::Graph::linearize
             at /home/dude/.cargo/registry/src/index.crates.io-6f17d22bba15001f/factrs-0.1.0/src/containers/graph.rs:64
:23
  24: <factrs::optimizers::gauss_newton::GaussNewton<S> as factrs::optimizers::traits::Optimizer>::step
             at /home/dude/.cargo/registry/src/index.crates.io-6f17d22bba15001f/factrs-0.1.0/src/optimizers/gauss_newto
n.rs:67:28
  25: factrs::optimizers::traits::Optimizer::optimize
             at /home/dude/.cargo/registry/src/index.crates.io-6f17d22bba15001f/factrs-0.1.0/src/optimizers/traits.rs:1
34:22
  26: factrs_test::main
             at ./src/main.rs:106:18
  27: core::ops::function::FnOnce::call_once
             at /rustc/051478957371ee0084a7c0913941d2a8c4757bb9/library/core/src/ops/function.rs:250:5
note: Some details are omitted, run with `RUST_BACKTRACE=full` for a verbose backtrace.

This sounds like in issue with my code, but I'm a bit lost at this point. Any clues about this?

[1] https://github.com/borglab/gtsam/blob/develop/examples/LocalizationExample.cpp

[contagon]

Thanks for reaching out! Easy fix, ForwardProp is generic on the dimension of the input, so it should be ForwardProp<Const<3>> instead of ForwardProp<Const<2>>. I'm going to do some thinking as well, as it would be nice if we caught this earlier on rather than just barfing like this...

Alternatively to make sure it doesn't happen again, you can also do type Differ = ForwardProp<<Self as Residual1>::DimIn>;

Let me know if that fixes it, or if you hit any other snags! factrs is under pretty heavy development still, so if you see anything you'd like/need let us know

[contagon]

Also ominus_right returns a VectorX<D> so you should be able to return the result from it directly if I'm not mistaken:)

[dwuertz]

Success! Fixing the dimension did the trick. I should point out that I originally followed the implementation of imu_preint which uses DimOut for the differ [2]. May there be an issue as well?

Anyway, once I got it working, I added a manual implementation of the Jacobian, just for the heck of it. No problems here. Listing the final code below, should anybody find it useful/instructional:

impl Residual1 for GpsResidual
{
    type Differ = ForwardProp<<Self as Residual1>::DimIn>;
    type V1 = SE2;
    type DimIn = Const<3>;
    type DimOut = Const<2>;

    fn residual1<D: Numeric>(&self, v: SE2<D>) -> VectorX<D> {
        let p_meas = Vector2::<D>::dual_convert(&self.meas);
        let p: VectorVar2<D> = v.xy().into_owned().into();
        let p_meas: VectorVar2<D> = p_meas.into();
        p.ominus_right(&p_meas)
    }

    fn residual1_jacobian(&self, values: &Values,keys: &[Key]) -> DiffResult<VectorX, MatrixX> {
        let p: &SE2 = values.get_unchecked(*keys.first().unwrap()).unwrap();
        let s = p.theta().sin();
        let c = p.theta().cos();
        let diff = MatrixX::from_row_slice(2, 3, &[0.0, c, -s,
                                                   0.0, s,  c]);
        DiffResult { value: self.residual1(p.clone()), diff }
    }
}

Should you be interested, I could open a PR to add the complete example to examples/. Otherwise, feel free to ignore it.
Thanks for the support, looking forward to the project evolving!

[2] https://github.com/rpl-cmu/factrs/blob/master/src/residuals/imu_preint/residual.rs#L299

[contagon]

Glad that worked for you! And yeah, thanks for catching that bug, it slipped through our testing. Fixed it in #17.

A PR would be great! We could use some custom residual examples. It's something I'm actually working on adding to the docs as well