diff --git a/src/derivatives.jl b/src/derivatives.jl index c8000a7..7adec68 100644 --- a/src/derivatives.jl +++ b/src/derivatives.jl @@ -26,53 +26,43 @@ ith_partial{N}(X::SMatrix{9,N}, i) = @SMatrix([X[1,i] X[4,i] X[7,i]; jacobian(::Type{output_param}, R::input_param) Returns the jacobian for transforming from the input rotation parameterization to the output parameterization, centered at the value of R. +Note that +* if `input_param <: QuatRotation`, a unit quaternion is assumed and + the jacobian is with respect to the four parameters of a unit quaternion. +* if `input_param <: Quaternion`, a general (with arbitrary norm) + quaternion is assumed and the jacobian is with respect to a general quaternion jacobian(R::rotation_type, X::AbstractVector) Returns the jacobian for rotating the vector X by R. """ -function jacobian(::Type{RotMatrix}, q::QuatRotation) - w = real(q.q) - x, y, z = imag_part(q.q) - - # let q = s * qhat where qhat is a unit quaternion and s is a scalar, - # then R = RotMatrix(q) = RotMatrix(s * qhat) = s * RotMatrix(qhat) - - # get R(q) - # R = q[:] # FIXME: broken with StaticArrays 0.4.0 due to https://github.com/JuliaArrays/StaticArrays.jl/issues/128 - R = SVector(Tuple(q)) - - # solve d(s*R)/dQ (because its easy) - dsRdQ = @SMatrix [ 2*w 2*x -2*y -2*z ; - 2*z 2*y 2*x 2*w ; - -2*y 2*z -2*w 2*x ; - - -2*z 2*y 2*x -2*w ; - 2*w -2*x 2*y -2*z ; - 2*x 2*w 2*z 2*y ; - - 2*y 2*z 2*w 2*x ; - -2*x -2*w 2*z 2*y ; - 2*w -2*x -2*y 2*z ] - - # get s and dsdQ - # s = 1 - dsdQ = @SVector [2*w, 2*x, 2*y, 2*z] - - # now R(q) = (s*R) / s - # so dR/dQ = (s * d(s*R)/dQ - (s*R) * ds/dQ) / s^2 - # = (d(s*R)/dQ - R*ds/dQ) / s +function jacobian(::Type{RotMatrix}, q::Quaternion) + s = abs(q) + w = real(q) + x, y, z = imag_part(q) + qhat = SVector{4}(w/s, x/s, y/s, z/s) # unit quaternion + + dRdqhat = _jacobian_unit_quat(qhat) + + dqhatdq = (I(4) - qhat * qhat') / s # jacobian of normalization + dRdqhat * dqhatdq # total jacobian +end - # now R(q) = (R(s*q)) / s for scalar s, because RotMatrix(s * q) = s * RotMatrix(q) - # - # so dR/dQ = (dR(s*q)/dQ*s - R(s*q) * ds/dQ) / s^2 - # = (dR(s*q)/dQ*s - s*R(q) * ds/dQ) / s^2 - # = (dR(s*q)/dQ - R(q) * ds/dQ) / s - jac = dsRdQ - R * transpose(dsdQ) +jacobian(::Type{RotMatrix}, q::QuatRotation) = _jacobian_unit_quat(params(q)) - # now reformat for output. TODO: is the the best expression? - # return Vec{4,Mat{3,3,T}}(ith_partial(jac, 1), ith_partial(jac, 2), ith_partial(jac, 3), ith_partial(jac, 4)) +@inline function _jacobian_unit_quat(qhat::SVector{4}) + # jacobian of vec(RotMatrix(QuatRotation(qhat, false))) + w, x, y, z = qhat + return @SMatrix [ 2*w 2*x -2*y -2*z ; + 2*z 2*y 2*x 2*w ; + -2*y 2*z -2*w 2*x ; + -2*z 2*y 2*x -2*w ; + 2*w -2*x 2*y -2*z ; + 2*x 2*w 2*z 2*y ; + 2*y 2*z 2*w 2*x ; + -2*x -2*w 2*z 2*y ; + 2*w -2*x -2*y 2*z ] end diff --git a/test/derivative_tests.jl b/test/derivative_tests.jl index 084e8e4..b89d396 100644 --- a/test/derivative_tests.jl +++ b/test/derivative_tests.jl @@ -11,13 +11,26 @@ using ForwardDiff @testset "Jacobian checks" begin # Quaternion to rotation matrix + @testset "Jacobian (Quaternion -> RotMatrix)" begin + for i in 1:10 # do some repeats + q = rand(QuaternionF64) # a random quaternion, **not** normalized + + # test jacobian to a Rotation matrix + R_jac = Rotations.jacobian(RotMatrix, q) + FD_jac = ForwardDiff.jacobian(x -> SVector{9}(QuatRotation(x[1], x[2], x[3], x[4])), # normalize + SVector(q.s, q.v1, q.v2, q.v3)) + + # compare + @test FD_jac ≈ R_jac + end + end @testset "Jacobian (QuatRotation -> RotMatrix)" begin for i in 1:10 # do some repeats - q = rand(QuatRotation) # a random quaternion + q = rand(QuatRotation) # a random quaternion (normalized) # test jacobian to a Rotation matrix R_jac = Rotations.jacobian(RotMatrix, q) - FD_jac = ForwardDiff.jacobian(x -> SVector{9}(QuatRotation(x[1], x[2], x[3], x[4])), + FD_jac = ForwardDiff.jacobian(x -> SVector{9}(QuatRotation(x[1], x[2], x[3], x[4], false)), # don't normalize Rotations.params(q)) # compare