Fixes and tests on PartialPose3, Bearing, and BearingRange Factors

[dehann]

Probably easiest to use Rotations.RotXY or something similar? Issue is the test script was written assuming the coordinates for rotation are Euler angles -- this is probably no longer true, as more general SE(n) objects are now being used.

[dehann]

ah, think the issue with the new factor residual function is that R[1:2,1:2] rotation matrix parts contain more than just the yaw angle. That 2x2 segment also contains information from pitch and roll.

[Affie]

You might be correct. I just followed the previous factor, but can have another look.

[dehann]

Hi Johan, this is probably still not 100%, but hopefully closer:

wR2 = wR1*1R2 = wR1*(1Rψ*Rθ*Rϕ)
wRz = wR1*1Rz
zRz = wRz \ wR(Δψ)

M_R = SO(3)
δ(α,β,γ) = vee(M_R, R_0, log(M_R, R_0, zRz))

M = SE(3)
p0 = identity_element(M)
δ(x,y,z,α,β,γ) = vee(M, p0, log(M, p0, zRz))

EDIT, best to run the residual in SE(2), however, note that all three Lie exp rotation coordinates will be affected, so the partial needs to involve for XYYaw = (1,2,4,5,6)

EDIT2: the main issue is which reference frame should XYYaw work in? The assumption is this XYYaw<:AbstractRelative will be used in combination with ZRP priors. The combo of two factors replace a regular Pose3Pose3 odo. Therefore, XYYaw is assumed (from the users perspective) as a factor being in the "world frame", but as a wander-azimuth relative to wT1. So by extension, XY should be considered as a local level XY also relative to wander-azimuth wT1. Again, users needs to know that this XYYaw factor is used in combination with a Pitch-Roll-Z prior (which creates the assumed local level).

[Affie]

Hi @dehann. As far as I can tell, I missed the projection part to make sure the SE(2) points are on the manifold.
We should also maybe assert that the pitch is not 90 degrees?

[Affie]

These looks outdated as pts no longer exists

[Affie]

Should have probably done BearingRange fix in a new branch, but it just reverts back to when it last worked. It should be correct now and we can update and make sure it still passes the tests.

[Affie]

Potential bug remains in partials

[Affie]

I'm happy to merge this and do further improvements in new PRs. CI failures are intermittent numerical failures.

[dehann]

cool thanks! I was looking at bearing tests yesterday and trying to see if there is a bug or shortcoming in IIF, but wasn't yet able to pin down either way yet. Think we're close to tagging IIF.