Drastic decrease in quality after #499

[ojura]

This is using the same dataset as in #453, which was another regression that caused a drastic decrease in quality on that dataset.

I did a git bisect:

3948943b6491ad07dd4c6ba8e71cb4d03b27a5bf is the first bad commit
commit 3948943b6491ad07dd4c6ba8e71cb4d03b27a5bf
Author: Wolfgang Hess <whess@google.com>
Date:   Mon Sep 4 11:14:51 2017 +0200

    Use EstimateGravityOrientation() in 2D SLAM. (#499)

Here's with e3b6f0a:

...and with 3948943:

Here is the updated pioneer-cartographer.lua (please use this one instead of the one in #453). Also, please use cartographer_ros with rangefinder decimation (if it doesn't get merged by the time you are reading this): googlecartographer/cartographer_ros@e8962a7.

[jihoonl]

We are also investigating the same problem at the moment. The orientation becomes very strange when use_odometry=true.

[ojura]

BTW, bisecting is really made difficult by the fact that there are two separate repos. I know it's a long shot, but would you be willing to consider moving everything to one repo? In the very probable case that you won't, in order to facilitate bisecting, I was thinking of making a script that would prepare a "zipped" linear repo with commits from both cartographer and cartographer_ros ordered by time of commiting.

Edit: I did exactly that, a mirror repo is located at larics/cartographer_combined. Here is the post on Google Groups.

[ojura]

I have also observed a minor decrease in quality also after 0ef372d (before, after). But I guess that's a tuning question, and I can open another issue for that after resolve this one, which is far more serious.

[ojura]

@jihoonl have you reached any conclusions? I still haven't looked into it in detail, but I have one clue which might be useful. In the InsertionResult returned from AddAccumulatedRangeData, I have replaced the gravity_alignment.rotation() with an identity quaternion (Eigen::Quaterniond::Identity()).

The reasoning for trying this out was that I am doing flat 2D vehicle slam, so I don't really need the estimation of gravity. And, in my case, changing this single line of code immediately stopped this from happening.

The conclusion is that the passed gravity alignment wrecks chaos in the sparse pose graph for some reason. That's all I know at this moment.

[ojura]

Here is a log of the gravity alignment quaternion. It should be close to the identity quaternion the whole time. It is oscillating in yaw: https://gist.github.com/ojura/6fc7dd0f2fad8ae46b5c9fab03e89c74

[wohe]

Gravity only constrains 2 degrees of freedom, not yaw. The quaternions all seem to be rotations around the z axis. This part seems okay to me.

Obviously the question is whether the yaw component of this quaternion is handled correctly, i.e. odometry is taken into account once in the correct direction.

[ojura]

Why would the gravity alignment quaternion have a yaw component at all? I am a bit confused about its role.

[wohe]

Gravity orientation is updated by integrating angular velocities and there is nothing that would remove the yaw component and also no reason to.

[Mofef]

@ojura I'm not sure if i got you right, but in commit https://github.com/googlecartographer/cartographer/commit/3948943b6491ad07dd4c6ba8e71cb4d03b27a5bf, the one of #499 gravity_alignment.rotation() isn't part of the insertion result...? Or do you mean you replaced gravity_alignment with sth. like transform::Rigid3d(gravity_alignment.translation(), Eigen::Quaterniond::Identity()) this did not improve the result in our case. Other than the only thing we found out so far was that if use_odometry=false the problem does not occur.

[ojura]

@Mofef, @wohe changed this a bit in #500, where he passes gravity_alignment.rotation() instead of gravity_alignment. I have been experimenting with the current head.

[ojura]

Gravity orientation is updated by integrating angular velocities and there is nothing that would remove the yaw component and also no reason to.

@wohe this is where you have me confused about what the role of gravity_alignment is. Previously, we had tracking_to_tracking_2d, which was accompanied with the following comment: // Computes the rotation without yaw, as defined by GetYaw().

In #500, you wrote: In 2D this [gravity_alignment] replaces the 'tracking_to_tracking_2d' transform.

So that's why I thought that this quaternion should not contain any yaw. And the fact that setting gravity_alignment to an identity quaternion (effectively removing that yaw) unbroke my SLAM reinforced that interpretation. Can you please clarify?

[jihoonl]

The breakage can be prevented when you set odometry_available in optimization_problem.cc to false. So, I assume that we have to double check the logic of TransformInterpolationBuffer::Lookup or logic that computes relative_pose when odometry_available=true.

Too sad that I will be in the train and cannot attend the open house meeting.

[wohe]

There were a couple of changes:

1. tracking_to_tracking_2d was only a rotation, even though stored as a rigid transform. We now store gravity_alignment (which has replaced it) as a quaternion since we know there is no translational part.
2. The GetYaw() part was possible because we earlier had the assumption that the tracking frame is approximately gravity aligned. Now we don't make this assumption anymore, so removing the yaw is not easily possible and the code that was there would not work if the tracking frame is oriented orthogonal to the gravity aligned frame. In the absence of bugs (entirely possible of course) this should not matter, the gravity aligned frame just has an arbitrary rotation around the z axis now.
3. We no longer separate the pose in two separate components. The pose stored in each trajectory node is now the complete pose, even in 2D.

[ojura]

I see, thanks on the explanation. Well, the good news is that I have published pose_estimate directly from the local trajectory builder and it looks okay (it follows the right trajectory). It's just that something later in the SPG makes the optimized poses (and the optimized submap origins) go crazy.

So @jihoonl I disagree that the problem is in the local trajectory builder, or the pose extrapolator / interpolation buffer. Something is broken in SPG optimization, and passing just an identity gravity alignment quaternion to the SPG in the insertion result prevents this from happening (@Mofef oddly this "fix" doesn't work in #499, but it works in #500). @wohe do you have an idea where the problem could lie? My head gets a little dizzy when I try to follow what you're doing with the pose and the gravity alignment in the sparse pose graph and constraint builder :-)

[Mofef]

@ojura I reproduced, that the "fix" improves the result after #500, but for us the result is still way worse than before #499

[wohe]

@ojura Thanks for pointing out that the issue could be in the pose graph optimization, I have an idea what is wrong: we have 2D poses in this optimization problem which are from the gravity_alignment frame to the map frame. Odometry poses are in tracking frame and optimization_problem.cc is not correctly transforming them into the gravity_alignment frame.

[ojura]

Thanks on the suggestion, @wohe. Based on it, I followed how the local trajectory builder applies the gravity alignment to rangefinder data and did a similar thing with odometry data passed to SPG. From your description, I thought that I would need to multiply the odometry pose with gravity_alignment in order to achieve what we need, but it turned out to be that it needed to be multiplied with its inverse! Go figure. At least on my data, this restores pre-#499 behaviour, so @Mofef @jihoonl you might want to try it out (check out PR #532).

[wohe]

Fixing this should actually improve quality over the behavior before #499. The bug already existed before and only was exposed by #499. Earlier, the missing transformations just happened to be approximately identity if the tracking frame was kept approximately gravity aligned.

[wohe]

Thanks everyone for helping to get this resolved!

[Mofef]

For us its solved. Thank you :)