Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Proposal to fix inconsitency in jacobian(::Type{RotMatrix}, ::QuatRotation) #291

Open
wants to merge 1 commit into
base: master
Choose a base branch
from
Open

Proposal to fix inconsitency in jacobian(::Type{RotMatrix}, ::QuatRotation) #291

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
68 changes: 29 additions & 39 deletions src/derivatives.jl
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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


Expand Down
17 changes: 15 additions & 2 deletions test/derivative_tests.jl
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down