Inconsistent `z_up` transform — applied to rotation but not translation in pose computation

[D-1shu]

z_up Pose Consistency Issue

Summary

The z_up option permuted ONLY the rotation (row remap (X,Y,Z)->(Y,Z,X)) while leaving the translation vector in the original camera optical frame. Additionally, a secondary quaternion component shuffle attempted to compensate during publication. This produced a pose whose orientation and position were expressed in different coordinate frames.

Inconsistencies

• Function getRelativeTransform(...): Applied row permutation to rotation matrix when z_up==true, but left tvec unpermuted.
• Function makeTagPose(...): Applied a second permutation by shuffling quaternion components (x,y,z,w)->(z,x,y,w) attempting to align frames; translation still unmodified.

Net effect: orientation was effectively permuted twice (matrix + quaternion shuffle) while translation not permuted at all, yielding a mismatched 6D pose.

Why This Is a Problem

Consumers (TF, rviz, downstream planners) assume rotation R and translation t in a homogeneous transform share the same coordinate basis. Mixing frames causes:

• Apparent tag drift or incorrect spatial alignment in visualization.
• Incorrect relative transforms when chaining through TF.
• Subtle errors in control loops if using pose derivatives.

Correct Frame Mapping

Define permutation matrix S for z_up:
S = [[0,1,0],[0,0,1],[1,0,0]] so that (X,Y,Z)_camera -> (Y,Z,X)_zup.
Apply uniformly:
R' = S R
t' = S t
No further quaternion component reordering is needed; quaternion derived from R' is already in the target frame.

Fix Suggested

1. Applied the same permutation to the translation vector:
   • getRelativeTransform
2. Removed quaternion component shuffle in:
   • makeTagPose
3. Quaternions now created directly from already-permuted rotation matrices.

Code Touch Points

• getRelativeTransform: permute t when z_up.
• makeTagPose: remove manual quaternion field reordering.

[QQting]

Pull Requests are welcome.