Fix slerp at coincident/antipodal endpoints, and SO2/SE2.interp1()#193
Open
gaoflow wants to merge 2 commits into
Open
Fix slerp at coincident/antipodal endpoints, and SO2/SE2.interp1()#193gaoflow wants to merge 2 commits into
gaoflow wants to merge 2 commits into
Conversation
The slerp weights sin((1-s)t)/sin(t) and sin(s.t)/sin(t) are singular wherever
sin(t) vanishes, ie. at t=0 (coincident endpoints) and t=pi (antipodal endpoints,
which are the same rotation under the double cover). qslerp() guarded only t=0;
UnitQuaternion.interp() and .interp1() re-derived the weights inline and guarded
neither, so they raised on valid input:
q = UnitQuaternion.Rx(0.3)
q.interp(q, 0.5) # ZeroDivisionError
UnitQuaternion().interp1(0.5) # ZeroDivisionError
UnitQuaternion.Rx(pi).interp(UnitQuaternion.Rx(-pi), 0.5, shortest=True)
# ZeroDivisionError
UnitQuaternion.Rx(pi).interp(UnitQuaternion.Rx(-pi), 5)
# TypeError, non-unit result
qslerp() itself returned non-unit quaternions near t=pi: qslerp(q, -q, 0.5)
gave [0 0 0 0], and for endpoints 1e-9 from antipodal the norm reached 5.8e6.
Root of that: acos(dotprod) loses the small angle to rounding at both ends, so
sin(acos(dotprod)) is a bad denominator. Take sin(t) directly as the length of
the component of q1 orthogonal to q0, which keeps full relative precision, and
get t from atan2. The only remaining degenerate case is sin(t) == 0, where q0
and q1 are the same rotation and so is every interpolate.
Measured against a 50-digit q0*exp(s*log(q0^-1.q1)) reference over the whole
range of t and s, worst error for t within 1e-6 of pi drops from 4.4e-5 to
2.7e-10 rad, and the largest deviation from unit norm anywhere from 8.2e6 to
2.8e-4. Ordinary angles are unchanged to within 1 ulp, and the whole surface
still agrees with scipy's Slerp to 1.4e-15 rad over 10000 random pairs.
The two UnitQuaternion methods now call qslerp(), which their own :seealso:
already pointed at, so the formula lives in one place.
The fix for rai-opensource#33 dropped the `start` local from interp1() but only replaced its two uses in the N == 3 branch, so the SO(2)/SE(2) branch has referenced an undefined name ever since: SE2(1, 2, 0.3).interp1(0.5) # NameError: name 'start' is not defined rai-opensource#33 did report it for both SE2 and SE3. Pass None like the N == 3 branch does; trinterp2() already treats a None start as the identity. interp1() had no test coverage for either dimension, hence the four years.
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment
Add this suggestion to a batch that can be applied as a single commit.This suggestion is invalid because no changes were made to the code.Suggestions cannot be applied while the pull request is closed.Suggestions cannot be applied while viewing a subset of changes.Only one suggestion per line can be applied in a batch.Add this suggestion to a batch that can be applied as a single commit.Applying suggestions on deleted lines is not supported.You must change the existing code in this line in order to create a valid suggestion.Outdated suggestions cannot be applied.This suggestion has been applied or marked resolved.Suggestions cannot be applied from pending reviews.Suggestions cannot be applied on multi-line comments.Suggestions cannot be applied while the pull request is queued to merge.Suggestion cannot be applied right now. Please check back later.
Two crashes in the interpolation methods, found sweeping the whole slerp surface (angle between the endpoints from 0 to pi across both singular ends,
sover [0,1],shortesteither way).slerp singularities
The slerp weights
sin((1-s)t)/sin(t)andsin(s.t)/sin(t)are singular whereversin(t)vanishes: att = 0(coincident endpoints) and att = pi(antipodal endpoints, which are the same rotation under the double cover).qslerp()guards onlyt = 0;UnitQuaternion.interp()and.interp1()re-derive the weights inline and guard neither.acos(dotprod)loses the small angle to rounding at both ends, sosin(acos(dotprod))is a poor denominator. This takessin(t)directly as the length of the component ofq1orthogonal toq0, which keeps full relative precision, and getstfromatan2. The only degenerate case left issin(t) == 0, where the endpoints are the same rotation and so is every interpolate. The twoUnitQuaternionmethods now callqslerp(), which their own:seealso:already pointed at, so the formula lives in one place.Checked against a 50-digit
q0 exp(s log(q0^-1 q1))reference — Lie-group form, so it shares no algebra with the sin-weight formula — over the full range oftands:qslerpwas returning quaternions with a norm in the millions)scipy.spatial.transform.Slerpto 1.4e-15 rad over 10000 random pairsExactly antipodal with
shortest=Falsehas no unique great circle, so there is no correct answer there; it now returns a unit quaternion for the rotation both endpoints share instead of a norm-1e6 vector or[0 0 0 0].SO2/SE2.interp1()
The fix for #33 dropped the
startlocal but replaced its uses only in theN == 3branch, soSE2(1, 2, 0.3).interp1(0.5)has raisedNameError: name 'start' is not definedever since — #33 did report it for both SE2 and SE3.interp1()had no test coverage in either dimension.4 new test cases, all red on master. The existing
test_slerpandtest_interpvalues were already correct and pass unchanged. Full suite green,black23.10.0 clean.