fix(state_representation): remove error in orientation scaling

[domire8]

Description

Bug detected by @eeberhard, scaling a pose didn't give the correct orientation. The fix I propose is to use slerp instead of the exponential calculation from before. As a side note, this solution is equivalent to keeping the exp but removing the division by 2 that already seemed suspicious.

Review guidelines

Estimated Time of Review: 5 minutes

Checklist before merging:

• Confirm that the relevant changelog(s) are up-to-date in case of any user-facing changes

[eeberhard]

I believe the division by two probably was mistakenly carried over from mapping equations involving the half-angle. For example, when converting from axis-angle to quaternion, the angle is divided by two before being used in a similar exponential function.

Using Eigen's slerp is definitely simpler. Just to be sure, does the resulting quaternion respect the original hemisphere (i.e. does the real part remain positive / negative in all cases)? Would be good to have a small test case for that too.

[domire8]

Using Eigen's slerp is definitely simpler. Just to be sure, does the resulting quaternion respect the original hemisphere (i.e. does the real part remain positive / negative in all cases)? Would be good to have a small test case for that too.

Yeah, could you give me a hand with that? I don't think I would know what to test for exactly...

[buschbapti]

From what I see, the log function was in charge of putting the quaternion in the correct hemisphere:

if (q_tmp.w() < 0) {
    q_tmp = Eigen::Quaterniond(-q_tmp.coeffs());
  }

Question is to know if slerp is handling that or if you need to check for that. I see this question on slack overflow that is vey relevant https://stackoverflow.com/questions/62943083/interpolate-between-two-quaternions-the-long-way.

You need to check what is already handled by Eigen as we don't want to duplicate.

[buschbapti]

Coming back to that I am trying to find how it is implemented in Eigen. I see one problem already from the documentation of slerp (https://eigen.tuxfamily.org/dox/classEigen_1_1QuaternionBase.html#ac840bde67d22f2deca330561c65d144e):

the spherical linear interpolation between the two quaternions *this and other at the parameter t in [0;1].

This means that our lambda should be in [0;1] which is not what we need here. Have you tried with 2 for example?

[buschbapti]

I think I am getting my head around that. I am so rusty... It makes sense that lambda should be between 0 and 1. Twice the rotation would actually be the same rotation. According to the stack overflow page I sent -lambda would end up with the rotation on the opposite hemisphere and longer path. We probably don't want that anyway.

So, if the function does not behave correctly with lambda outside of [0;1] you can do:

slerp(lambda % 1, q)

If I recall a negative number modulo would end up positive. If not find the correct function one of them have that behavior in C++.

Gosh I miss that.

[domire8]

I can't understand why we wouldn't be able to 'double' the rotation though. Imagine just a small roational displacement from identity, and then I would like to double that. Why would I not be able to do that? It wouldn't change the hemisphere or anything, except if I really have a bad understanding of quaternions..

[buschbapti]

You can. We should not restrict that because for position it makes sense. But doubling the orientation means that you end up where you are. So either slerp already handle that (please do check) or you need to take the modulo of lambda before calling slerp.

[eeberhard]

Baptiste of course makes an excellent point about slerp (spherical linear interpolation). For context, in my original use case I wanted to interpolate between two poses according to an interpolation time factor t, which was explicitly bound between 0 and 1. That's why I chose slerp as the workaround in my application. When Dominic applied it here, I didn't consider the cases outside of my original use case of interpolation.

But, "clamping" the lambda of this scaling operation between 0 and 1 using the modulus is not correct either, since that defeats the purpose of arbitrarily scaling a rotation.

I suspect just removing the suspicious half operation from the exponentiation function would be enough to solve the original problem and preserve the intended scaling effect. The exp function essentially acts as an integrator for cumulatively applied rotations, and is similar to how angular velocity can be converted into a rotation. The main point is that the half-angle is normally used when converting between quaternion and axis-angle, and it slipped into this equation even though it shouldn't.

Concretely, I would suggest the following test cases:

• Scale q by arbitrary factor between 0 and 1 (say, 0.6)
  • Expect result to be equivalent to an identity slerp of the quaternion by the same factor (qI.slerp(q, 0.6))
• Scale q by arbitrary factor between 1 and 2 (let's say 1.6)
  • Expect result to be equal to q * (qI.slerp(q, 1.6 - 1))
• Scale q by any arbitrary positive factor n, it should be q * q * ... n times * qI.slerp(q, remainder of n)

Same for negative case, where the expected orientation is the conjugate of the above expectations

[eeberhard]

But doubling the orientation means that you end up where you are

Just to also clarify this, doubling an orientation represented by some quaternion q would be equivalent to the Hamilton product q * q. In other words, Dominic is correct to expect the total rotation to increase with a larger lambda. Of course it will eventually wrap around, but the number of multiplications necessary is acos(w) / π (since 2 * acos(qw) is the angular displacement of a unit quaternion and 2π is a full rotation, again why those 2 and 1/2 appear in some equations)

[buschbapti]

Very good explanation as always @eeberhard. I am definitely too rusty for this. I think we are getting around this. As discussed with @domire8 proper test cases are needed and it seems the one you propose are good.

[domire8]

Concretely, I would suggest the following test cases:

* Scale q by arbitrary factor between 0 and 1 (say, `0.6`)
  
  * Expect result to be equivalent to an identity slerp of the quaternion by the same factor (`qI.slerp(q, 0.6)`)

* Scale q by arbitrary factor between 1 and 2 (let's say `1.6`)
  
  * Expect result to be equal to `q * (qI.slerp(q, 1.6 - 1))`

Makes sense, I've tried it. However, I'm not sure how I can control on which hemisphere the quaternions should end up now. The axis angle representation of the resulting scaled quaternions are the same between slerp and exp but the signs of the quaternion coefficients don't seem to correspond in each case. Hence, the tests still fail...

[eeberhard]

The axis angle representation of the resulting scaled quaternions are the same between slerp and exp but the signs of the quaternion coefficients don't seem to correspond in each case. Hence, the tests still fail...

It would better to check angular distance against the Eigen methods (expect near zero), rather than the coefficients. Regarding the duality of quaternions, I would test that $w$ has the same sign before and after the operation. If that expectation is not met, then you can modify the operation to force this outcome.

Could you also test the inverse case to make sure negative scaling works as intended? (i.e., same tests but take the conjugate of the slerp-calculated result)

[domire8]

Thanks for helping me work this out. I think this is good to go now with all requested test cases.