Fixed the orientation of covariance view to match with the interpreta…

[ViktorWalter]

Fixed the orientation part of covariance view to match with the interpretation of the covariance in e.g. geometry_msgs::PoseWithCovariance used by tf2.
This means, that the "tangent covariance" on the tips of the axis vector visualizations will actually show the orientation uncertainties/noises in the frame of the world, not in the frame of the object/body as was the case previously.

[rhaschke]

Could you please check the overlap of your PR with #1540.

[ViktorWalter]

Could you please check the overlap of your PR with #1540.

I checked now - I would say their solution is incorrect - they are "manually" fixing the issues they discovered by swapping axes.
I have observed their issues myself before. The cause of this issue was - as far as I can tell - the incorrect interpretation of the orientation covariance in general in cases where the orientation mean is not aligned with the world (frame in which we are visualizing). How exactly this leads to some of the "tangent covariances" appearing perpendicular to what one would expect them is difficult to interpret, but I'd say that it is because they are created from submatrices of covariance matrix in the wrong frame. If the orientation itself changes - e.g. we rotate the mean about x by 90 deg. - the covariance visualization should change to reflect this.
Edit: I see that their pose is actually aligned. Then I don't know what the issue is there - I have not observed this happening in such cases.

[ViktorWalter]

Ok, I can replicate the issue in #1540 with both versions. This implies that this is a separate issue.

[ViktorWalter]

Here is a demo - the cyan visualizations are with the original implementation of the orientation covariance display, the purple are mine. On he left, they are displayed with the same mean as in #1540, on the right the orientation itself is different by 90 degrees, but the orientation covariance is unchanged. The left visualizations both exhibit the issue in #1540, but on the right you can see the effects of my fix. My assesment is that the author of #1540 was right - the 2D covariance on the "Z" (blue) axis is rotated by 90 degrees. This is separate from my issues.

[rhaschke]

I'm afraid that I cannot judge your code change, because I don't exactly know the definition of the Pose covariance. Given 6d-vectors x = (p, w) and their counterparts y = (Rp, Rw) represented in another frame to compute the covariance from, the corresponding covariance matrices should be defined as: CX = <(x-µ) (x-µ)^t> and CY = <(Rx-Rµ) (Rx-Rµ)^t> = RCXR^t.
However, your change only applies the orientation change to the orientation part, while according to above equation it should be applied to both, position and orientation, shouldn't it?

[ViktorWalter]

No. The position covariance is already expressed in the frame in the message header. Otherwise, changing the orientation would rotate the position covariance too, which it doesn't. It Does change the orientation covariance, which it should not - e.g if we have large noise in yaw, but set the orientation to 90 deg in pitch, the visualization will imply that there is a large noise in roll, which is wrong. My change makes the orientation covariance consistent with the position covariance interpretation - by recalculating the covariance matrix, we can extract the "tangent covariances" (I don't know what to call them) such that they will represent the noise in the viewing frame. The reason that this change is applied only to the orientation covariance is that as opposed to the position covariance this visualization is affixed to the axis gizmo's full pose. Edit.: Obviously, an alternative solution would be to leave the covariance as is, and affix it to the frame axes instead of the visualized body axes.