-
|
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 #[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. 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 |
Beta Was this translation helpful? Give feedback.
Replies: 2 comments 3 replies
-
|
Thanks for reaching out! Easy fix, Alternatively to make sure it doesn't happen again, you can also do 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 |
Beta Was this translation helpful? Give feedback.
-
|
Also |
Beta Was this translation helpful? Give feedback.
-
|
Success! Fixing the dimension did the trick. I should point out that I originally followed the implementation of 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 [2] https://github.com/rpl-cmu/factrs/blob/master/src/residuals/imu_preint/residual.rs#L299 |
Beta Was this translation helpful? Give feedback.
-
|
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 |
Beta Was this translation helpful? Give feedback.
-
|
I had played around with this when trying to find the bug, so just added it in PR #22 :) |
Beta Was this translation helpful? Give feedback.
Thanks for reaching out! Easy fix,
ForwardPropis generic on the dimension of the input, so it should beForwardProp<Const<3>>instead ofForwardProp<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