Fix errors while orientation covariance visualization in CovarianceVi…

[UniBwTAS]

Hello,
this PR fixes some problems when visualizing the orientation uncertainty/covariance.
Steps to reproduce:

Execute in bash/zsh: rostopic pub "/test" geometry_msgs/PoseWithCovarianceStamped '{header: {stamp: 0, frame_id: "map"}, pose: {pose: {position: {x: 0, y: 0, z: 0}, orientation: {x: 0, y: 0, z: 0, w: 1}}, covariance: [1, 0, 0, 0, 0, 0, 0, 0.1, 0, 0, 0, 0, 0, 0, 0.1, 0, 0, 0, 0, 0, 0, 0.01, 0, 0, 0, 0, 0, 0, 0.2, 0, 0, 0, 0, 0, 0, 0.01]}}'

This publishes a pose with following orientation covarariance (roll, pitch, yaw):

0.01 0 0
0 0.2 0
0 0 0.01

This leads to the following visualization in RVIZ:

The problem is that the covariance ellipse of the z-axis (blue) has the wrong orientation. I think it should be rotated by 90 degrees. The variance is large only for the y-axis (green) while the other two are very small. When one imagines it rotates around the green y-axis then the blue z-axis goes back and forth and not left and right.

With my PR it looks like this:

Another example that it works as intended now:

Execute in bash/zsh: rostopic pub "/test" geometry_msgs/PoseWithCovarianceStamped '{header: {stamp: 0, frame_id: "map"}, pose: {pose: {position: {x: 0, y: 0, z: 0}, orientation: {x: 0, y: 0, z: 0, w: 1}}, covariance: [1, 0, 0, 0, 0, 0, 0, 0.1, 0, 0, 0, 0, 0, 0, 0.1, 0, 0, 0, 0, 0, 0, 0.01, 0, 0, 0, 0, 0, 0, 0.2, 0.1, 0, 0, 0, 0, 0.1, 0.2]}}'

This publishes a pose with following orientation covarariance (roll, pitch, yaw):

0.01 0 0
0 0.2 0.1
0 0.1 0.2

This leads to the following visualization in RVIZ:

Again the the covariance ellipse of the blue z-axis should be rotated by 90 degrees. Additionally we now have high variances of y- and z-axis as well as non-diagonal entries in the covariance matrix, which means that y and z rotations are highly correlated.

With my PR it looks like this:

[UniBwTAS]

Further examples:
rostopic pub "/test" geometry_msgs/PoseWithCovarianceStamped '{header: {stamp: 0, frame_id: "map"}, pose: {pose: {position: {x: 0, y: 0, z: 0}, orientation: {x: 0, y: 0, z: 0, w: 1}}, covariance: [1, 0, 0, 0, 0, 0, 0, 0.1, 0, 0, 0, 0, 0, 0, 0.1, 0, 0, 0, 0, 0, 0, 0.2, 0, 0.1, 0, 0, 0, 0, 0.01, 0, 0, 0, 0, 0.1, 0, 0.2]}}'

This publishes a pose with following orientation covarariance (roll, pitch, yaw):

0.2 0 0.1
0 0.01 0
0.1 0 0.2

This leads to the following visualization in RVIZ:

After PR looks like:

rostopic pub "/test" geometry_msgs/PoseWithCovarianceStamped '{header: {stamp: 0, frame_id: "map"}, pose: {pose: {position: {x: 0, y: 0, z: 0}, orientation: {x: 0, y: 0, z: 0, w: 1}}, covariance: [1, 0, 0, 0, 0, 0, 0, 0.1, 0, 0, 0, 0, 0, 0, 0.1, 0, 0, 0, 0, 0, 0, 0.2, 0.1, 0, 0, 0, 0, 0.1, 0.2, 0, 0, 0, 0, 0, 0, 0.01]}}'

This publishes a pose with following orientation covarariance (roll, pitch, yaw):

0.2 0.1 0
0.1 0.2 0
0 0 0.01

This leads to the following visualization in RVIZ:

After PR looks the same:

I think everything looks good now.

[UniBwTAS]

Maybe related to: #1191

[rhaschke]

@Ellon, could you please review this, as you initially implemented covariance visualization?
I'm not sure what's the intended visualization. But I agree with @UniBwTAS that it seems to be inconsistent with the present orientation.

[Ellon]

I've completely forgot to address this. I'm sorry about that. I'll answer as soon as I come back from holidays.

…

On Tue, Aug 25, 2020, 22:31 Robert Haschke ***@***.***> wrote: @Ellon <https://github.com/Ellon>, could you please review this, as you initially implemented covariance visualization? I'm not sure what's the intended visualization. But I agree with @UniBwTAS <https://github.com/UniBwTAS> that it seems to be inconsistent with the present orientation. — You are receiving this because you were mentioned. Reply to this email directly, view it on GitHub <#1540 (comment)>, or unsubscribe <https://github.com/notifications/unsubscribe-auth/AAQ2VD3CAG3GA5R2P5DIJY3SCQNQXANCNFSM4QJV5I5Q> .

[rhaschke]

@Ellon: Ping.

[Ellon]

Hi @UniBwTAS and @rhaschke, sorry for the late reply.

@UniBwTAS I debugged all your examples "by hand" and it seems there's indeed a bug in the current code, at least for the cylinder on the z-axis. Could you show images "before PR" for your further examples? I'm having problems to compile and test noetic-devel on my current system and I cannot see how they were behaving before the PR. I imagine this would explain why you are proposing a change to orientation_offset_node_[kRoll] as well ?

While I'm at it, I'll investigate the relationship with #1191 .

[UniBwTAS]

Hello @Ellon

I updated the above "further examples" post. As far as I remember for actual visualization the change at the X Axis makes no difference. I just changed it to make it consistent such that the surface normal of the top of the cylinder points away from the center, I think. However this is not visible as the top and bottom look the same. I can remove the change for the X axis, if you prefer this.

[rhaschke]

@Ellon, @UniBwTAS: What's the status of this PR?

[rhaschke]

@Ellon, @UniBwTAS, and @ViktorWalter: Could please review this PR (#1540) and #1576? If you all agree with the proposed solution(s), I'will be happy to merge them. Thanks a lot in advance.

[ViktorWalter]

I have looked at #1540 - I can replicate the issue described here - the visualization of the "shape" attached to "Z" axis is statically rotated by 90 degrees and this commit fixes it. However, the change in line 276 as far as I can tell does nothing, as the "Covariance ellipses" are symmetrical. Still, I suggest merging #1540. This should be fully compatible with also merging #1576.
Edit: I would also suggest renaming kYaw kRoll and kPitch to better reflect their function - each of these indices refers to an object that in fact represents the pair of the other two values than its name suggests.

[rhaschke]

Thanks, @ViktorWalter for your feedback. I will await the feedback.

[Ellon]

@Ellon, @UniBwTAS: What's the status of this PR?

Hi @rhaschke ! My apologies for being silent about this PR.

By looking at the commit and the examples @UniBwTAS gave it seems to be correcting the problem I didn't see at the time, but today it's not straight forward for me to test this PR in my current system (I'm using an hybrid of old ROS indigo with some other in-house things) so I couldn't run some tests with this PR by myself. I'll give myself this week evenings to have noetic running in a docker and test this PR.

[rhaschke]

I'll give myself this week evenings to have noetic running in a docker and test this PR.

Thanks @Ellon for helping to review this. Please also consider #1576.

[Ellon]

Please also consider #1576.

OK.

Small update: I managed to have rviz from noetic-devel compiled inside a docker from ubuntu focal, but I'm unable to run rviz due to something related with GLX/nvidia/X11/cuda. I'll continue trying tomorrow, but if anyone knows what could be the problem please let me know.

[rhaschke]

Running rviz from a docker container requires some extra work as described in section "Running MoveIt container" of https://moveit.ros.org/install/docker. We do have a script gui-docker to facilitate the task.

[Ellon]

We do have a script gui-docker to facilitate the task.

Thanks for the info. I'll take a look at it tonight.

[rhaschke]

@Ellon, did you manage to review this PR and #1576?