Lie algebra se3

Hi again :slight_smile:
In a current project of mine, representations of transformations are often needed to be converted between the Lie group SE3 and it’s Lie algebra se3. In a C++ code base, I’ve seen Sophus used for those cases. I didn’t find anything mentionned about this in /r/rust. I was wondering if some of the people in this forum knew about a rust project already taking care of this? Otherwise, I don’t mind recoding the functions I will need based on nalgebra, but we often make small mistakes along the road so that’s why I’m asking.

Hi! I am not aware of any crate that deal specifically with lie groups/lie algebra. So if you end up writing functions you need and don’t mind open-sourcing them, maybe some could be integrated to nalgebra or may result in a new crate!

I definitely cannot do something nalgebra-quality-like with my basic rust skills ^^ but I don’t mind at all opensourcing my experiments to serve as a concrete example use case. This use case is basically minimizing a non linear optimization problem with a Gauss-Newton approach for tracking of a camera (visual odometry). Transformation steps are expressed as small Lie algebra elements. I’ll link here more details (or problems, who knows XD) when I’m further in the code.

I’m trying to setup tests to compare rotations before and after a round trip to and from Lie algebra. I need to have approximate comparisons. I’ve seen that the Rotation type has trait AbsDiffEq, so I have added the approx crate to my dev dependencies. Yet when I’m trying to use it, I get the error message

the trait approx::AbsDiffEq is not implemented for nalgebra::Unit<nalgebra::Quaternion<f32>>.

The function tested:

fn log_exp_round_trip() {
    let rotation = UnitQuaternion::from_euler_angles(0.0, 0.0, 1.0);
        epsilon = EPSILON_ERROR

Is there something I’m doing wrong? (my basic so3 and se3 modules are on github but I didn’t commit the approximate tests yet, still a work in progress).

EDIT: turns out I had put approx 0.3 in my Cargo.toml and I just realized that nalgebra is based on version 0.2 of approx and they are not compatible apparently. Switched back to 0.2 and it works :slight_smile: I miss Elm semantic versioning ^^

Thanks for mentioning this, I’ll update the approx dependency to 0.3 tomorrow!

You’re welcome ^^

So I have basically finished rewriting the so3 and se3 transformations I’ll need for my optimization problem. They are inspired by the implementation in Sophus but have few differences that I think are better. I’ve added quicktests to make sure round trips give the initial value back. Nothing is generic (I’ve used f32) but I think you can have a look at the implementation now if you are interested. I’m also open to any feedback.

Module so3:

Now I’m gonna try to use them for my next rust challenge :crossed_fingers:

1 Like

That looks pretty good! While that line is fine, you could just use fields of Matrix3 instead of indexing here, and use Vector3::new instead of Vector3::from_colum_slice:

Vector3::new(mat.m32, mat.m12, mat.m21);

You can also construct a Matrix3 with ::new so that, e.g., this becomes:

    0.0, -w3, w2,
    w3, 0.0, -w1,
    -w2, w1, 0.0