Partial Pose3 relative factor LocalLevelXYRotZ

[dehann]

Before the Manifolds.jl upgrade, the factor PartialPose3XYYaw was able to get away with coordinates stored as Tait-Bryant / Euler angels (RotZYX). This made partial calculations around yaw only easier in the old system. Now that the whole system is working towards more general manifolds, the mechanism by which partial relative factors work needs to be upgraded too. In the new system, coords are Lie exponentials (or any other representation that pops up), so a simple partial yaw factor could now influence more than just the previous "3rd rotation coordinate". Coords in short term likely to be vee(so(3)) so all three Lie exponentials might be affected by a partial rotation factor (as example).

Intention is to rename the old factor to something like Pose3LevelXYRotZ, which lets the user define whether Z points up or down in their application. Other factors will be needed if nav convention is not NED or NWU.
Hoof issue met dit is coords vir Pose3 is nie meer Euler hoeke RotZYX nie, dis nou Lie exponentials -- maw Yaw kan rotasie kan al 3 coords (p,q,r)^ affekteer.

[dehann]

Following from current status at JuliaRobotics/RoME.jl#509, I just want to make sure the residual for Pose3LevelXYRotZ does in fact calculate on the local level.

We assume previous pose precedence to provide wTi -- i.e. AbstractRelative factor goes between two Pose3s p_i en p_j, each with affine matrices wTi en wTj (just for explanation). Where wTi is the transform of p_i in the world-frame. So we assume "local level" is 'given' by current estimate of wTi. Obviously this is non-linear. For this we also assume the user knows that this factor can be used with PriorPose3ZRotYX (previously the partial prior on Z Pitch Roll).

[Affie]

There is a sign issue with the current factor in JuliaRobotics/RoME.jl#509
It can be shown by this test:

# To compare signs to Point2 case 
mu = [20.0, 5.0]
xyy = Point2Point2(MvNormal( mu, diagm([0.01, 0.01].^2)))
xi = [10;0]
xj = [20;10]
res = calcFactorResidualTemporary(xyy, (Point2, Point2), [], (xi, xj))
@test isapprox(res, [10;-5], atol=0.15)

# Comparing to Point2 for sign only
mu = [20.0, 5.0, 0.0]
xyy = Pose3Pose3XYYaw(MvNormal( mu, diagm([0.01, 0.01, 0.001].^2)))
xi = ProductRepr([10;0; 10.], collect(RotZ(0.0)))
xj = ProductRepr([20;10;-10.], collect(RotZ(0.0)*RotY(0.2)*RotX(-0.1)))
res = calcFactorResidualTemporary(xyy, (Pose3, Pose3), [], (xi, xj))
@test isapprox(res, [10;-5;0], atol=0.15)

I'll add what I have so far in a twig.

[Affie]

Here is the twig for comparison:
JuliaRobotics/RoME.jl#512

[Affie]

BTW, I'm not sold on the name LocalLevelXYRotZ yet, the RotZ part is confusing, I prefer making the external reference frame more explicit such as using heading or yaw (but yaw can perhaps still be confusing)