fix: preserve rotation quadrants in trlog2

[vera-rubin]

trlog2 loses the quadrant for rotations beyond 90°, so its logarithms don't exponentiate back to the input.
We use atan2 in the SO(2) and SE(2) branches to recover the correct angle.
We tested the reported round trips and added a regression covering matrix and twist returns alongside the existing 2D pose and twist tests.

Fixes #252

— ChipLab

[codecov-commenter]

⚠️ Please install the to ensure uploads and comments are reliably processed by Codecov.

Codecov Report

✅ All modified and coverable lines are covered by tests.

📢 Thoughts on this report? Let us know!

[petercorke]

Thank you for your interest in SMTB, for the report in #252, and for the fix. It's great to have a first PR from you.

I confirmed that 120°, 180° and −120° fail all three round trips (SO2.log, SE2.log, Twist2(SE2).exp) before the change and pass after it, with no new singularity at θ = π in the SE(2) branch. The atan2 change and the regression test are what I'd have written. Thanks for the clear reproducer.