Fix transform issue in odometry display plugin #1631

UniBwTAS · 2021-06-07T17:06:08Z

Description

This pull request fixes a transform issue with odometry plugin for rviz. The previous implementation only shows meaningful Arrows/Axes when the "fixed frame" of rviz is equal to the parent frame of the Odometry message. However, when the fixed frame is different, then the pose of the visuals have to be constantly re-computed because they moved w.r.t. the fixed frame. Currently the transform is calculated just once when the message arrives. We re-compute the transform of the scene node (which corresponds to the parent frame of the Odometry message and contains all the Arrows/Axes) whenever the update (=redraw) callback is triggered.

Additionally we introduced a ref_pose_ which is useful for poses with very large positional values. E.g. the Pose in the UTM frame. Without this ref_pose_ this causes glitches because Ogre only works with single point precision (float). So Ogre has problems to calculate the transform fixed_frame (often base_link or frame close to base_link) -> utm -> base_link. By introducing this ref_pose_ Ogre has to handle much smaller positional values and the offset to the ref_pose_ can be calculated in double point precision.

Image before:

Image after:

Image for better understanding. In the attached rosbag we compare ground truth odometry against dead-reckoning. It contains the transform utm -> odom -> base_link, one odom message for dead reckoning and odom message for ground truth (gps + imu).

Files to reproduce (rosbag & rviz config):
https://www.dropbox.com/sh/mo6zo6akakiap6e/AAAShCSywQxV4sWC5JtjF5TEa?dl=0

rhaschke

I understand that you want to see the past path of your robot/car when using a fixed frame that is attached to the robot.
In principle, I support this PR. However, there are several shortcomings that need to be fixed:
The PR combines several changes, which should be split into individual commits, each providing a concise commit message:

Smoothly updating the scene node's position + orientation
Introducing ref_pose to be more robust w.r.t. huge positions
Cosmetic changes, e.g. renaming length() ➡️ norm()

Renaming and introducing new members breaks ABI, which should be strictly avoided. Can you think of alternative implementations? If not, try to restrict incompatible API/ABI changes to a minimum!

Removing last_used_message_ and introducing two new last*message members. Is that really needed? It breaks ABI.
As far as I can see, you need last_message_ for smoothly updating the scene node's position + orientation.
Why not moving this update into processMessage()? Then you don't need to remember last_message_?
There is a significant drift of the shown trajectory when the fixed transform is set to odom. Could be numerical issues, but could be a conceptual problem as well. Using odom as fixed frame, I expect to see exactly the old behavior.
Instead of using the last message's frame as the reference frame, shouldn't you use the same frame_id as reference, e.g. the one from the first seen message? Why happens if odometry messages are expressed w.r.t. a different parent frame in between?

src/rviz/default_plugin/odometry_display.cpp

src/rviz/default_plugin/odometry_display.h

src/rviz/default_plugin/odometry_display.cpp

UniBwTAS · 2021-06-22T10:54:51Z

I split the original commit into 3 distinct ones.

Why not moving this update into processMessage()? Then you don't need to remember last_message_?

The transform has to be calculated continuously when the fixed_frame (e.g. base_link) is moving w.r.t. to the parent frame of the Odometry message (e.g. odom). When the Odometry message comes in not very frequently then the movement of the trajectory does not look very smooth. So I decided to put it into the update() callback. Nevertheless I was able to get rid of last_message_ by simply using the most recent transform available. I think its ok in most scenarios.

There is a significant drift of the shown trajectory when the fixed transform is set to odom. Could be numerical issues, but could be a conceptual problem as well. Using odom as fixed frame, I expect to see exactly the old behavior.

I think the drift you mean only comes from the inaccuracies of 2D (no pitch or roll) dead reckoning (red trajectory) compared to that of inertial navigation system (blue trajectory). For me everything looks fine w.r.t. to visualization. The red trajectory looks the same with and without my commits when setting the fixed_frame to odom. Without my commits it is meaningless to compare red and blue trajectory since the latter has utm as its parent frame and hence for it the fixed frame should be set to utm. (However, UTM origin is quite far away from trajecory)

UniBwTAS · 2021-06-22T11:01:08Z

By the way, using the attached rosbag one can see the importance of ref_pose_ (introduced in the second commit). The blue trajectory looks very ugly due to large positional values in the UTM frame which causes numerical issues:

rhaschke

Thanks for cleaning up this PR.

Without my commits it is meaningless to compare red and blue trajectory since the latter has utm as its parent frame and hence for it the fixed frame should be set to utm. (However, UTM origin is quite far away from trajecory)

I don't get this statement. Why shouldn't it be possible use odom as the fixed frame for the blue trajectory? Without your changes (left), the blue trajectory is perfectly displayed on top of the red one, implying that the frame conversion works perfectly fine.
With your changes (right), the blue trajectory drifts over time (i.e. history changes):

comaprison.mp4

It looks like the order of orientation and translation is somehow screwed.

rhaschke · 2021-06-22T12:16:17Z

src/rviz/default_plugin/odometry_display.cpp

+    ROS_ERROR("Error transforming odometry '%s' from frame '%s' to frame '%s'", qPrintable(getName()),
+              last_used_message_->header.frame_id.c_str(), qPrintable(fixed_frame_));


If you are at it: Could you replace this console error output with a display status error (Display::setStatus)?
Remember to reset the status to OK if there is no error.

Even though the left/old one looks better, the right one is correct. Our dead reckoning (red) is really that bad ;) It is only based on wheel turn rates and a yaw gyroscope from the ESP sensor. Therefore it only works in the 2D plane as it cannot detect roll and pitch of the vehicle. The inertial navigation system, on the other hand, is able to measure roll and pitch. Thats why the blue trajectory pitches and rolls w.r.t. to the fixed frame (odom, which is based on our dead reckoning).

That the left one looks meaningful is a coincidence because the trajectories are "similar". New Odometry messages, or more precisely the arrows, are inserted at the right position of the scene but old ones are not updated when the fixed frame moves w.r.t. these arrows.

If you are at it: Could you replace this console error output with a display status error (Display::setStatus)?
Remember to reset the status to OK if there is no error.

I added this in the latest commit. I borrowed the code from grid_display.cpp.

I finally removed the setStatus commit again, as this part is already handled by the MessageFilterDisplay Super-Class. Otherwise there would be duplicate Transform items:

Sure. Sorry for the noise.

rhaschke · 2021-06-22T16:41:10Z

That the left one looks meaningful is a coincidence because the trajectories are "similar". New Odometry messages, or more precisely the arrows, are inserted at the right position of the scene but old ones are not updated when the fixed frame moves w.r.t. these arrows.

My main concern is that the history of the blue trajectory changes, which shouldn't be the case in my opinion.

rhaschke · 2021-06-22T17:42:30Z

tf2_eigen package is missing (see buildfarm job)

UniBwTAS · 2021-06-22T18:17:48Z

My main concern is that the history of the blue trajectory changes, which shouldn't be the case in my opinion.

No the history of blue arrows is not changing. Just its parent frame utm moved with respect to fixed_frame odom.

UniBwTAS · 2021-06-22T18:42:39Z

tf2_eigen package is missing (see buildfarm job)

I added the tf2_eigen package into the CMakeList.txt and package.xml

rhaschke

My main concern is that the history of the blue trajectory changes, which shouldn't be the case in my opinion.

No the history of blue arrows is not changing. Just its parent frame utm moved with respect to fixed_frame odom.

Sure, but in effect, the past trajectory moves in space, which is wrong in my opinion.

CMakeLists.txt

src/rviz/default_plugin/odometry_display.cpp

…ry of arrows/axes This commit ensures to keep history of arrows/axes even if fixed_frame is different from parent frame_id of Odometry.msg

… message These large values can occur when transforming GPS to UTM coordinates.

…en current and last position These large values can occur when transforming GPS to UTM coordinates.

UniBwTAS · 2021-06-22T19:26:13Z

My main concern is that the history of the blue trajectory changes, which shouldn't be the case in my opinion.

No the history of blue arrows is not changing. Just its parent frame utm moved with respect to fixed_frame odom.

Sure, but in effect, the past trajectory moves in space, which is wrong in my opinion.

In my opinion this is absolutely mandatory since otherwise the visualization makes no sense any more as soon you switch to another fixed_frame than the parent frame of the Odometry message. Compare the first image of this pull request with second one. The first visualization is useless.

UniBwTAS · 2021-06-22T20:01:01Z

I added a video. Maybe you will better see why the history of arrows is important to us and why it should move w.r.t. to the fixed_frame. Here the fixed_frame is set to utm. Now the aerial image and the blue trajectory are fixed. The red trajectory pitches and rolls together with the car (as it is just 2D). Now we can compare how much the origin of the red trajectory drifts over time compared to ground truth blue trajectory:

https://www.dropbox.com/s/p13n2i6f46zsqup/odometry_display_with_utm_as_fixed_frame.mp4?dl=0

UniBwTAS · 2021-06-23T05:58:44Z

The arrows are still fixed as before when fixed_frame is parent frame of odometry messages. In this case the plugin behaves the same as before. However, when the fixed_frame is different from parent frame it now shows meaningful trajectories whereas in previous implementation the visual output was unusable.

UniBwTAS · 2021-06-24T12:21:23Z

We created a synthetic example which shows the problem with the current implementation:
Current Implementation:

odom-2021-06-24_14.14.12.mp4

New Implementation:

odom-2021-06-24_14.10.51.mp4

The code generating the synthetic example can be found here.

rhaschke · 2021-06-24T16:44:38Z

Thanks for providing this example. If I now understand correctly, the observed drift comes from slightly different odometries published in the two topics of the rosbag file. Depending on which frame is used as the fixed frame, one or the other curve stays fixed, right?

AndreasR30 · 2021-06-24T17:15:09Z

Thanks for providing this example. If I now understand correctly, the observed drift comes from slightly different odometries published in the two topics of the rosbag file. Depending on which frame is used as the fixed frame, one or the other curve stays fixed, right?

Yes

rhaschke · 2021-06-24T17:31:17Z

I think this is fine to merge as soon as UniBwTAS#1 is merged into this PR.

UniBwTAS · 2021-06-24T19:01:23Z

I think this is fine to merge as soon as UniBwTAS#1 is merged into this PR.

Done

UniBwTAS · 2021-06-30T06:56:06Z

@rhaschke Is there still a reason not to merge this pull request?

AndreasR30 · 2021-07-20T09:14:34Z

@rhaschke ping

rhaschke · 2021-08-19T22:38:42Z

Sorry for the delay merging this.

…ry of arrows/axes This ensures correct history display even if rviz' fixed frame is different from the parent frame_id of Odometry.msg. Partial backport of #1631.

In order to correctly display the path/history of Odometry messages when the message's frame_id is moving relative to rviz' fixed frame, we need to update the pose of the corresponding scene node in each update() cycle. This is a partial backport of ros-visualization#1631.

…-visualization#1631)" This reverts commit a6e58df.

This reverts commit 5b27a0c.

rhaschke · 2021-09-27T05:57:05Z

I reverted this PR for the same reason as #1655, namely updating the node's transform doesn't consider the message's timestamp anymore.

rhaschke requested changes Jun 19, 2021

View reviewed changes

UniBwTAS force-pushed the fix-odometry-plugin branch 2 times, most recently from 7a865e2 to 86067c5 Compare June 22, 2021 10:45

rhaschke requested changes Jun 22, 2021

View reviewed changes

UniBwTAS force-pushed the fix-odometry-plugin branch from 86067c5 to dab9066 Compare June 22, 2021 18:38

rhaschke requested changes Jun 22, 2021

View reviewed changes

CMakeLists.txt Show resolved Hide resolved

src/rviz/default_plugin/odometry_display.cpp Outdated Show resolved Hide resolved

AndreasR30 added 3 commits June 22, 2021 21:11

Smoothly update the scene node's position + orientation to keep histo…

2c2c2b2

…ry of arrows/axes This commit ensures to keep history of arrows/axes even if fixed_frame is different from parent frame_id of Odometry.msg

Intruduce ref_pose to handle poses with very large values in Odometry…

def9a67

… message These large values can occur when transforming GPS to UTM coordinates.

Use double precision for large values when calculating distance betwe…

f880b1e

…en current and last position These large values can occur when transforming GPS to UTM coordinates.

UniBwTAS force-pushed the fix-odometry-plugin branch from dab9066 to f880b1e Compare June 22, 2021 19:13

rhaschke approved these changes Jun 24, 2021

View reviewed changes

add odometry test example (#1)

85a6036

UniBwTAS mentioned this pull request Aug 5, 2021

Fix transform issue in point cloud display plugin #1652

Closed

rhaschke merged commit 5b27a0c into ros-visualization:noetic-devel Aug 19, 2021

UniBwTAS deleted the fix-odometry-plugin branch August 20, 2021 16:41

rhaschke added a commit to rhaschke/rviz that referenced this pull request Sep 24, 2021

Revert "Smoothly move an Odometry's path given a moving frame_id (ros…

723a387

…-visualization#1631)" This reverts commit a6e58df.

rhaschke added a commit that referenced this pull request Sep 24, 2021

Revert "Fix transform issue in odometry display plugin (#1631)"

84057af

This reverts commit 5b27a0c.

AndreasR30 mentioned this pull request Dec 15, 2021

Add option to continuously transform visuals in odometry plugin #1686

Open

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

Fix transform issue in odometry display plugin #1631

Fix transform issue in odometry display plugin #1631

UniBwTAS commented Jun 7, 2021 •

edited

Loading

rhaschke left a comment

UniBwTAS commented Jun 22, 2021 •

edited

Loading

UniBwTAS commented Jun 22, 2021

rhaschke left a comment

rhaschke Jun 22, 2021

UniBwTAS Jun 22, 2021

UniBwTAS Jun 22, 2021

UniBwTAS Jun 22, 2021 •

edited

Loading

rhaschke Jun 22, 2021

rhaschke commented Jun 22, 2021

rhaschke commented Jun 22, 2021

UniBwTAS commented Jun 22, 2021

UniBwTAS commented Jun 22, 2021 •

edited

Loading

rhaschke left a comment

UniBwTAS commented Jun 22, 2021 •

edited

Loading

UniBwTAS commented Jun 22, 2021

UniBwTAS commented Jun 23, 2021

UniBwTAS commented Jun 24, 2021 •

edited

Loading

rhaschke commented Jun 24, 2021

AndreasR30 commented Jun 24, 2021

rhaschke commented Jun 24, 2021

UniBwTAS commented Jun 24, 2021

UniBwTAS commented Jun 30, 2021

AndreasR30 commented Jul 20, 2021

rhaschke commented Aug 19, 2021

rhaschke commented Sep 27, 2021

		ROS_ERROR("Error transforming odometry '%s' from frame '%s' to frame '%s'", qPrintable(getName()),
		last_used_message_->header.frame_id.c_str(), qPrintable(fixed_frame_));

Fix transform issue in odometry display plugin #1631

Fix transform issue in odometry display plugin #1631

Conversation

UniBwTAS commented Jun 7, 2021 • edited Loading

Description

rhaschke left a comment

Choose a reason for hiding this comment

UniBwTAS commented Jun 22, 2021 • edited Loading

UniBwTAS commented Jun 22, 2021

rhaschke left a comment

Choose a reason for hiding this comment

rhaschke Jun 22, 2021

Choose a reason for hiding this comment

UniBwTAS Jun 22, 2021

Choose a reason for hiding this comment

UniBwTAS Jun 22, 2021

Choose a reason for hiding this comment

UniBwTAS Jun 22, 2021 • edited Loading

Choose a reason for hiding this comment

rhaschke Jun 22, 2021

Choose a reason for hiding this comment

rhaschke commented Jun 22, 2021

rhaschke commented Jun 22, 2021

UniBwTAS commented Jun 22, 2021

UniBwTAS commented Jun 22, 2021 • edited Loading

rhaschke left a comment

Choose a reason for hiding this comment

UniBwTAS commented Jun 22, 2021 • edited Loading

UniBwTAS commented Jun 22, 2021

UniBwTAS commented Jun 23, 2021

UniBwTAS commented Jun 24, 2021 • edited Loading

rhaschke commented Jun 24, 2021

AndreasR30 commented Jun 24, 2021

rhaschke commented Jun 24, 2021

UniBwTAS commented Jun 24, 2021

UniBwTAS commented Jun 30, 2021

AndreasR30 commented Jul 20, 2021

rhaschke commented Aug 19, 2021

rhaschke commented Sep 27, 2021

UniBwTAS commented Jun 7, 2021 •

edited

Loading

UniBwTAS commented Jun 22, 2021 •

edited

Loading

UniBwTAS Jun 22, 2021 •

edited

Loading

UniBwTAS commented Jun 22, 2021 •

edited

Loading

UniBwTAS commented Jun 22, 2021 •

edited

Loading

UniBwTAS commented Jun 24, 2021 •

edited

Loading