Implement SE23 matrix Lie group for extended poses

[gsorensen]

Hi! As a researcher who works with factor graph optimisation applied to autonomous systems, and who wants to see Rust applied more in the robotics domain, this project seems very promising. Therefore I wanted to contribute!

This pull request implements the SE_2(3) matrix Lie group from e.g., [1] into fact-rs.

Notes about implementation

• I tried to match the layout of SE(3), e.g., by naming the velocity component uvw to match the translational velocity symbols found in e.g., Thor I. Fossen' Handbook of Marine Craft Hydrodynamics and Motion Control, but it might be more descriptive to name this component (and its getter) vel. I can change this if desired
• Since definitions for Vector9 etc. are missing from the deps, these had to be manually defined. In the vee-implementation, I have created rotation and velocity as a Vector6 and position as Vector3 as a result, before transferring them to a Vector9 since Vector9::new is not a defined macro. Probably not optimal, so pointing it out if you have suggestions on how to do it instead.
• I believe hat_swap should apply for a 3D rigid body transformation, i.e., it should behave as SE(3) in this instance, so the implementation of that (and that of apply) should be correct.

[1] Axel Barrau, Silvere Bonnabel. A Mathematical Framework for IMU Error Propagation with Appli-
cations to Preintegration. IEEE International Conference on Robotics and Automation (ICRA), May
2020, Paris, France. 10.1109/ICRA40945.2020.9197492. hal-02394774v3

[contagon]

This looks awesome! I appreciate the PR and work done - it's great to see others interested in fact-rs:)

I made a few comments, should mostly be minor and easy changes. My only big picture thought is if there's a better name - SE23 is close to "SE twenty-three". I generally pronounce it "SE - 2 - 3". SE_2_3 is a bit verbose, SE_23 has the same twenty-three issue, and I worry SE2_3 is too close to SE2. So I suppose SE23 is fine unless you can think of any alternatives.

Did you have any long-term plans for using it for any residuals or anything?

[contagon]

This is great - would you mind adding type aliases for 7 and 8 as well? And for all of the types you added 9 to in this matrix as well?

[gsorensen]

I added them to the types 9 has been added to, as well as the matrices with one and two rows so that they are there for 1D or 2D residuals.

[contagon]

Try using Vector9<T>::from_column_slice to initialize from a slice instead. Not sure if it reduces copies at all, but the code will at least be a bit cleaner

[gsorensen]

Yeah, that looks way nicer

[gsorensen]

This looks awesome! I appreciate the PR and work done - it's great to see others interested in fact-rs:)

I made a few comments, should mostly be minor and easy changes. My only big picture thought is if there's a better name - SE23 is close to "SE twenty-three". I generally pronounce it "SE - 2 - 3". SE_2_3 is a bit verbose, SE_23 has the same twenty-three issue, and I worry SE2_3 is too close to SE2. So I suppose SE23 is fine unless you can think of any alternatives.

Did you have any long-term plans for using it for any residuals or anything?

Thanks for the review! I will look into the requested changes and accommodate then :)

I agree the name doesn't necessarily lend itself well to code. In the old GTSAM fork by Brossard, he used the name ExtendedPose3 as an alternative to Pose3, though this has now been embedded into NavState as of release 4.3 (though with position and velocity order switched for now).

We informally talk about "Extended poses", but I guess it also doesn't match all that well when the rest of the Lie groups have names like se3, so3 and so on. However, given fact-rs does have ImuBias as a variable, I guess ExtendedPose would be descriptive enough, whilst avoiding the potential 23 misunderstanding. If this seems preferable, I can rename the file to extended_pose.rs and the variable to ExtendedPose.

As for long-term plans, right now this is mostly a hobby project with my research being focused on phased-array radio systems-aided INS using FGO, where I use C++ with GTSAM.

I would however like to demonstrate feasibility of Rust-based FGO on my data just as a proof-of-concept, so residuals for range, azimuth, and elevation are ones I envision using fact-rs with, though I have mostly used fixed-lag smoothed iSAM2.

[contagon]

Hmm, I'll keep pondering the name - I think I might end up with ExtendedSE3 or VelSE3 in the long run.

Awesome! I'm mostly a gtsam user myself. Let me know if you hit any bugs or issues with fact-rs, I'd love to battle test it more fully. I've had a busy couple of weeks, but should be more responsive moving forward.

Thanks again!