ethz-asl/minkindr

multiplication of rotations can fail due to reduced precision of output

ZacharyTaylor opened this issue · 0 comments

A minimal piece of code that will cause this issue is as follows:

kindr::minimal::RotationQuaternion q1(0.014005508273839951, 0, 0, 0.99987637996673584);
kindr::minimal::RotationQuaternion q2(0.018121428787708282, 0, 0,-0.99981027841567993);
q1*q2;

The first two lines create quaternions and are sufficiently close to 1 that no errors occur.
However, when the third line is run the following error is encountered

WARNING: Logging before InitGoogleLogging() is written to STDERR
F0111 13:23:33.874274 17251 rotation-quaternion-inl.h:77] Check failed: (squaredNorm()) >= (static_cast(1.0))-(static_cast(1e-4)) (0.999898 vs. 0.9999)
*** Check failure stack trace: ***

This is because the result of the multiplication has reduced the precision enough to fail the check.

One possible solution to this would be to renormalize the result of quaternion multiplications, though I am unsure if this would be the best approach. Does anyone have any thoughts as how to prevent this issue occurring? It seems like quite a problem as currently the wrong inputs will crash any program that uses minkindr multiplication.