Fix doTransform with Eigen Quaternion

[gleichdick]

I noticed that doTransform() for Eigen::Quaterniond seems to be broken while making unit tests.
The same testcase adapted for tf2_geometry_msgs works.

The math:

Let T be the transform rotation matrix (rotates around the x Axis by 180 degrees) and Q a given rotation matrix (-90 degrees around y). So when transforming Q (T times Q) the resulting quaternion should be [x, y, z, w]^T = [0.7071, 0, -0.7071, 0].

I re-enabled the other conversion tests and formatted the file with uncrustify

[gleichdick]

ROS1 has a unit test for that (which fails with my changes), but in my opinion it's not correct. @cwecht could you point me in the right direction?

[cwecht]

@gleichdick I thin, the unit test you mentioned is just plain wrong. I've used the rotated Unity-Y-Vector for visualization in my mind. Turns out, that this is a case, where the current implementation works. So: you are right and the test is wrong.

[clalancette]

@gleichdick I thin, the unit test you mentioned is just plain wrong. I've used the rotated Unity-Y-Vector for visualization in my mind. Turns out, that this is a case, where the current implementation works. So: you are right and the test is wrong.

@cwecht It's clear that you think the unit test is wrong. But given that you wrote the code originally, do you also agree that the code in doTransform is wrong? And that @gleichdick 's solution is correct?

[cwecht]

@clalancette I'm afraid it is so. I have no idea what I was thinking back in the day. I'd still suggest to use Eigen's quaterinon multiplication instead of converting the quaternions to matrices in order to do the multiplication.

[tfoote]

I agree that it would be better to leverage the quaternion multiplication w/o converting to matrices. I'd like to undertand what we did wrong with the quaternion math. Did the previous entry just have the order wrong. t * t_in * t.inverse() ? Where else does this fail? If we're in doubt about the correctness we should potentially write out a few more test cases to verify. And possibly test them against the matrix math for easier validation.

[clalancette]

@clalancette I'm afraid it is so. I have no idea what I was thinking back in the day. I'd still suggest to use Eigen's quaterinon multiplication instead of converting the quaternions to matrices in order to do the multiplication.

That's no problem, just making sure we were all on the same page. Thanks for the feedback.

[gleichdick]

In my understanding, a (homogenous) transform is just applied to the left. If the tanslational part is zero, it's just a rotation. So t.inverse() * t_in * t didn't make any sense to me (why do we need the inverse?) and I started creating/adapting some unit tests...

Now I added two more rotation tests (based on three-finger rule). I had to convert the quaternions to rotation matrices to compare them (because [0, 0, -1, 0] is the same rotation as [0, 0, 1, 0] and I defined the expected result in terms of angle-axis). doTransform() now simply multiplies the quaternions.

EDIT: Ah, Now I got it: According to Wiki, Rotations of quaternions are chained by multiplication only. The formula mentioned by Tully (q * x * q.inverse()) is for actually rotating a vector x, not a quaternion.

[tfoote]

Thanks for looking up the difference between the quaternion and vector operations. That makes more sense now. The notation can be quite ambiguous on that.

@cwecht could you take one more look?

[clalancette]

Here is CI in the meantime:

• Linux
• Linux-aarch64
• macOS
• Windows

[gleichdick]

Thanks for running the CI, math.h was missing on Windows...

[cwecht]

LFTM. Finally doTransform for Quaterniond should now be consistent with doTransform for e.g. Affined3d.

[clalancette]

Here's another shot at CI:

• Linux
• Linux-aarch64
• macOS
• Windows

[gleichdick]

The conversion tests turned out to be disabled for a reason, so I don't enable them anymore. Should now finally work...

18:06:19 C:\ci\ws\install\include\tf2/impl/convert.h(67,1): error C2668: 'Eigen::toMsg': ambiguous call to overloaded function [C:\ci\ws\build\tf2_eigen\tf2_eigen-test.vcxproj]

18:06:19 C:\ci\ws\src\ros2\geometry2\tf2_eigen\include\tf2_eigen/tf2_eigen.h(557,27): message : could be 'geometry_msgs::msg::Point Eigen::toMsg(const Eigen::Vector3d &)' [found using argument-dependent lookup] [C:\ci\ws\build\tf2_eigen\tf2_eigen-test.vcxproj]

18:06:19 C:\ci\ws\src\ros2\geometry2\tf2_eigen\include\tf2_eigen/tf2_eigen.h(130,27): message : or       'geometry_msgs::msg::Point tf2::toMsg(const Eigen::Vector3d &)' [C:\ci\ws\build\tf2_eigen\tf2_eigen-test.vcxproj]

By the way, I expect #368 to fix that problem. Is there any chance for me to run the CI (the big one) on my own?

[clalancette]

Is there any chance for me to run the CI (the big one) on my own?

Unfortunately, not really. It's a weakness in the current system, but it is also hard to fix. I'll run it for now. Here's another round of CI:

• Linux
• Linux-aarch64
• macOS
• Windows

[clalancette]

CI looks good now. Thanks @gleichdick for the fix and thanks @cwecht for the review. Merging.