From 8d40a572a3c7feb4dce8d81b1214b062666d162b Mon Sep 17 00:00:00 2001 From: hyrodium Date: Mon, 8 Nov 2021 00:40:04 +0900 Subject: [PATCH 1/9] replace fields of UnitQuaternion --- src/unitquaternion.jl | 27 +++++++++++++++++++-------- 1 file changed, 19 insertions(+), 8 deletions(-) diff --git a/src/unitquaternion.jl b/src/unitquaternion.jl index 7df29551..bce64155 100644 --- a/src/unitquaternion.jl +++ b/src/unitquaternion.jl @@ -17,28 +17,25 @@ where `w` is the scalar (real) part, `x`,`y`, and `z` are the vector (imaginary) and `q = [w,x,y,z]`. """ struct UnitQuaternion{T} <: Rotation{3,T} - w::T - x::T - y::T - z::T + q::Quaternion{T} @inline function UnitQuaternion{T}(w, x, y, z, normalize::Bool = true) where T if normalize inorm = inv(sqrt(w*w + x*x + y*y + z*z)) - new{T}(w*inorm, x*inorm, y*inorm, z*inorm) + new{T}(Quaternion(w*inorm, x*inorm, y*inorm, z*inorm, true)) else - new{T}(w, x, y, z) + new{T}(Quaternion(w, x, y, z, true)) end end @inline function UnitQuaternion{T}(q::Quaternion) where T if q.norm - new{T}(q.s, q.v1, q.v2, q.v3) + new{T}(q) else throw(InexactError(nameof(T), T, q)) end end - UnitQuaternion{T}(q::UnitQuaternion) where T = new{T}(q.w, q.x, q.y, q.z) + UnitQuaternion{T}(q::UnitQuaternion) where T = new{T}(q.q) end # ~~~~~~~~~~~~~~~ Constructors ~~~~~~~~~~~~~~~ # @@ -60,6 +57,20 @@ function Quaternions.Quaternion(q::UnitQuaternion) Quaternion(q.w, q.x, q.y, q.z, true) end +function Base.getproperty(q::UnitQuaternion, s::Symbol) + if s == :q + return getfield(q,:q) + elseif s == :w + return getfield(q,:q).s + elseif s == :x + return getfield(q,:q).v1 + elseif s == :y + return getfield(q,:q).v2 + elseif s == :z + return getfield(q,:q).v3 + end +end + # Pass in Vectors @inline function (::Type{Q})(q::AbstractVector, normalize::Bool = true) where Q <: UnitQuaternion check_length(q, 4) From 1982a0f6ea7b1686a473842c4e3e7406cc7f8b88 Mon Sep 17 00:00:00 2001 From: hyrodium Date: Wed, 10 Nov 2021 22:27:01 +0900 Subject: [PATCH 2/9] wip, update around fields of UnitQuaternion --- src/angleaxis_types.jl | 10 ++- src/derivatives.jl | 48 +++++++----- src/error_maps.jl | 44 +++++++---- src/mrps.jl | 9 ++- src/principal_value.jl | 2 +- src/rodrigues_params.jl | 10 ++- src/unitquaternion.jl | 158 ++++++++++++++++++++++++---------------- 7 files changed, 178 insertions(+), 103 deletions(-) diff --git a/src/angleaxis_types.jl b/src/angleaxis_types.jl index ac14c84d..86702dac 100644 --- a/src/angleaxis_types.jl +++ b/src/angleaxis_types.jl @@ -78,12 +78,16 @@ end end @inline function (::Type{AA})(q::UnitQuaternion) where AA <: AngleAxis - s2 = q.x * q.x + q.y * q.y + q.z * q.z + w = q.q.s + x = q.q.v1 + y = q.q.v2 + z = q.q.v3 + s2 = x * x + y * y + z * z sin_t2 = sqrt(s2) - theta = 2 * atan(sin_t2, q.w) + theta = 2 * atan(sin_t2, w) num_pert = eps(typeof(theta))^4 inv_sin_t2 = 1 / (sin_t2 + num_pert) - return principal_value(AA(theta, inv_sin_t2 * (q.x + num_pert), inv_sin_t2 * q.y, inv_sin_t2 * q.z, false)) + return principal_value(AA(theta, inv_sin_t2 * (x + num_pert), inv_sin_t2 * y, inv_sin_t2 * z, false)) end # Trivial type conversions for RotX, RotY and RotZ diff --git a/src/derivatives.jl b/src/derivatives.jl index 4d35af6f..da33d638 100644 --- a/src/derivatives.jl +++ b/src/derivatives.jl @@ -31,6 +31,10 @@ to the output parameterization, centered at the value of R. Returns the jacobian for rotating the vector X by R. """ function jacobian(::Type{RotMatrix}, q::UnitQuaternion) + w = q.q.s + x = q.q.v1 + y = q.q.v2 + z = q.q.v3 # 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) @@ -40,21 +44,21 @@ function jacobian(::Type{RotMatrix}, q::UnitQuaternion) R = SVector(Tuple(q)) # solve d(s*R)/dQ (because its easy) - dsRdQ = @SMatrix [ 2*q.w 2*q.x -2*q.y -2*q.z ; - 2*q.z 2*q.y 2*q.x 2*q.w ; - -2*q.y 2*q.z -2*q.w 2*q.x ; + 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*q.z 2*q.y 2*q.x -2*q.w ; - 2*q.w -2*q.x 2*q.y -2*q.z ; - 2*q.x 2*q.w 2*q.z 2*q.y ; + -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*q.y 2*q.z 2*q.w 2*q.x ; - -2*q.x -2*q.w 2*q.z 2*q.y ; - 2*q.w -2*q.x -2*q.y 2*q.z ] + 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*q.w, 2*q.x, 2*q.y, 2*q.z] + 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 @@ -134,16 +138,21 @@ end # Jacobian converting from a Quaternion to an MRP # function jacobian(::Type{MRP}, q::UnitQuaternion{T}) where T - den = 1 + q.w + w = q.q.s + x = q.q.v1 + y = q.q.v2 + z = q.q.v3 + + den = 1 + w scale = 1 / den dscaledQw = -(scale * scale) - dSpqdQw = SVector(q.x, q.y, q.z) * dscaledQw + dSpqdQw = SVector(x, y, z) * dscaledQw J0 = @SMatrix [ dSpqdQw[1] scale zero(T) zero(T) ; dSpqdQw[2] zero(T) scale zero(T) ; dSpqdQw[3] zero(T) zero(T) scale ] # Need to project out norm component of Quat - dQ = @SVector [q.w, q.x, q.y, q.z] + dQ = @SVector [w, x, y, z] return J0 - (J0*dQ)*dQ' end @@ -175,18 +184,23 @@ end # TODO: should this be jacobian(:rotate, q, X) # or something? function jacobian(q::UnitQuaternion, X::AbstractVector) + w = q.q.s + x = q.q.v1 + y = q.q.v2 + z = q.q.v3 + @assert length(X) === 3 T = eltype(q) # derivatives ignoring the scaling - q_im = SVector(2*q.x, 2*q.y, 2*q.z) - dRdQr = SVector{3}(2 * q.w * X + cross(q_im, X)) - dRdQim = -X * q_im' + dot(X, q_im) * one(SMatrix{3,3,T}) + q_im * X' - 2*q.w * d_cross(X) + q_im = SVector(2*x, 2*y, 2*z) + dRdQr = SVector{3}(2 * w * X + cross(q_im, X)) + dRdQim = -X * q_im' + dot(X, q_im) * one(SMatrix{3,3,T}) + q_im * X' - 2*w * d_cross(X) dRdQs = hcat(dRdQr, dRdQim) # include normalization (S, s = norm of quaternion) - dSdQ = SVector(2*q.w, 2*q.x, 2*q.y, 2*q.z) # h(x) + dSdQ = SVector(2*w, 2*x, 2*y, 2*z) # h(x) # and finalize with the quotient rule Xo = q * X # N.B. g(x) = s * Xo, with dG/dx = dRdQs diff --git a/src/error_maps.jl b/src/error_maps.jl index 99b4bbde..20e55e6a 100644 --- a/src/error_maps.jl +++ b/src/error_maps.jl @@ -173,9 +173,9 @@ for imap in (InvExponentialMap,InvCayleyMap,InvMRPMap,InvQuatVecMap) end (::InvExponentialMap)(q::UnitQuaternion) = scaling(ExponentialMap)*logm(q) -(::InvCayleyMap)(q::UnitQuaternion) = scaling(CayleyMap) * vector(q)/q.w -(::InvMRPMap)(q::UnitQuaternion) = scaling(MRPMap)*vector(q)/(1+q.w) -(::InvQuatVecMap)(q::UnitQuaternion) = scaling(QuatVecMap)*vector(q) * sign(q.w) +(::InvCayleyMap)(q::UnitQuaternion) = scaling(CayleyMap) * vector(q)/real(q.q) +(::InvMRPMap)(q::UnitQuaternion) = scaling(MRPMap)*vector(q)/(1+real(q.q)) +(::InvQuatVecMap)(q::UnitQuaternion) = scaling(QuatVecMap)*vector(q) * sign(real(q.q)) # ~~~~~~~~~~~~~~~ Inverse map Jacobians ~~~~~~~~~~~~~~~ # @@ -209,8 +209,10 @@ end function jacobian(::InvQuatVecMap, q::UnitQuaternion) + w = q.q.s + μ = scaling(QuatVecMap) - return sign(q.w) * SA[ + return sign(w) * SA[ 0. μ 0 0; 0. 0 μ 0; 0. 0 0 μ] @@ -218,20 +220,30 @@ end function jacobian(::InvCayleyMap, q::UnitQuaternion) + w = q.q.s + x = q.q.v1 + y = q.q.v2 + z = q.q.v3 + μ = scaling(CayleyMap) - si = 1/q.w - return μ*@SMatrix [-si^2*q.x si 0 0; - -si^2*q.y 0 si 0; - -si^2*q.z 0 0 si] + si = 1/w + return μ*@SMatrix [-si^2*x si 0 0; + -si^2*y 0 si 0; + -si^2*z 0 0 si] end function jacobian(::InvMRPMap, q::UnitQuaternion) + w = q.q.s + x = q.q.v1 + y = q.q.v2 + z = q.q.v3 + μ = scaling(MRPMap) - si = 1/(1+q.w) - return μ*@SMatrix [-si^2*q.x si 0 0; - -si^2*q.y 0 si 0; - -si^2*q.z 0 0 si] + si = 1/(1+w) + return μ*@SMatrix [-si^2*x si 0 0; + -si^2*y 0 si 0; + -si^2*z 0 0 si] end @@ -285,8 +297,10 @@ function ∇jacobian(::InvExponentialMap, q::UnitQuaternion, b::SVector{3}, eps= end function ∇jacobian(::InvCayleyMap, q::UnitQuaternion, b::SVector{3}) + w = q.q.s + μ = scaling(CayleyMap) - si = 1/q.w + si = 1/w v = vector(q) μ*@SMatrix [ 2*si^3*(v'b) -si^2*b[1] -si^2*b[2] -si^2*b[3]; @@ -297,8 +311,10 @@ function ∇jacobian(::InvCayleyMap, q::UnitQuaternion, b::SVector{3}) end function ∇jacobian(::InvMRPMap, q::UnitQuaternion, b::SVector{3}) + w = q.q.s + μ = scaling(MRPMap) - si = 1/(1+q.w) + si = 1/(1+w) v = vector(q) μ * @SMatrix [ 2*si^3*(v'b) -si^2*b[1] -si^2*b[2] -si^2*b[3]; diff --git a/src/mrps.jl b/src/mrps.jl index 8fcd7a52..c731f528 100644 --- a/src/mrps.jl +++ b/src/mrps.jl @@ -38,8 +38,13 @@ Base.one(::Type{RP}) where RP <: MRP = RP(0.0, 0.0, 0.0) end @inline function (::Type{RP})(q::UnitQuaternion) where RP<:MRP - M = 1/(1+q.w) - RP(q.x*M, q.y*M, q.z*M) + w = q.q.s + x = q.q.v1 + y = q.q.v2 + z = q.q.v3 + + M = 1/(1+w) + RP(x*M, y*M, z*M) end # ~~~~~~~~~~~~~~~ StaticArrays Interface ~~~~~~~~~~~~~~~ # diff --git a/src/principal_value.jl b/src/principal_value.jl index fd051f17..ba820632 100644 --- a/src/principal_value.jl +++ b/src/principal_value.jl @@ -17,7 +17,7 @@ the following properties: """ principal_value(r::Rotation) = r -principal_value(q::Q) where Q <: UnitQuaternion = q.w < zero(eltype(q)) ? Q(-q.w, -q.x, -q.y, -q.z, false) : q +principal_value(q::Q) where Q <: UnitQuaternion = real(q.q) < zero(eltype(q)) ? Q(-q.q) : q function principal_value(spq::MRP{T}) where {T} # A quat with positive real part: UnitQuaternion( qw, qx, qy, qz) # diff --git a/src/rodrigues_params.jl b/src/rodrigues_params.jl index a708a253..656aec87 100644 --- a/src/rodrigues_params.jl +++ b/src/rodrigues_params.jl @@ -31,12 +31,16 @@ end # ~~~~~~~~~~~~~~~ Quaternion <=> RP ~~~~~~~~~~~~~~~~~~ # @inline function (::Type{Q})(g::RodriguesParam) where Q<:UnitQuaternion M = 1/sqrt(1 + g.x*g.x + g.y*g.y + g.z*g.z) - q = Q(M, M*g.x, M*g.y, M*g.z, false) + return Q(M, M*g.x, M*g.y, M*g.z, false) end @inline function (::Type{G})(q::UnitQuaternion) where G<:RodriguesParam - M = 1/q.w - G(q.x*M, q.y*M, q.z*M) + w = q.q.s + x = q.q.v1 + y = q.q.v2 + z = q.q.v3 + + return G(x/w, y/w, z/w) end # ~~~~~~~~~~~~~~~ StaticArrays Interface ~~~~~~~~~~~~~~~ # diff --git a/src/unitquaternion.jl b/src/unitquaternion.jl index bce64155..cfb41d81 100644 --- a/src/unitquaternion.jl +++ b/src/unitquaternion.jl @@ -1,4 +1,4 @@ -import Base: *, /, \, exp, ≈, ==, inv +import Base: *, /, \, exp, ≈, == """ UnitQuaternion{T} <: Rotation @@ -54,7 +54,7 @@ function UnitQuaternion(q::T) where T<:Quaternion end function Quaternions.Quaternion(q::UnitQuaternion) - Quaternion(q.w, q.x, q.y, q.z, true) + return q.q end function Base.getproperty(q::UnitQuaternion, s::Symbol) @@ -125,55 +125,60 @@ end function Base.getindex(q::UnitQuaternion, i::Int) + w = q.q.s + x = q.q.v1 + y = q.q.v2 + z = q.q.v3 + if i == 1 - ww = (q.w * q.w) - xx = (q.x * q.x) - yy = (q.y * q.y) - zz = (q.z * q.z) + ww = (w * w) + xx = (x * x) + yy = (y * y) + zz = (z * z) ww + xx - yy - zz elseif i == 2 - xy = (q.x * q.y) - zw = (q.w * q.z) + xy = (x * y) + zw = (w * z) 2 * (xy + zw) elseif i == 3 - xz = (q.x * q.z) - yw = (q.y * q.w) + xz = (x * z) + yw = (y * w) 2 * (xz - yw) elseif i == 4 - xy = (q.x * q.y) - zw = (q.w * q.z) + xy = (x * y) + zw = (w * z) 2 * (xy - zw) elseif i == 5 - ww = (q.w * q.w) - xx = (q.x * q.x) - yy = (q.y * q.y) - zz = (q.z * q.z) + ww = (w * w) + xx = (x * x) + yy = (y * y) + zz = (z * z) ww - xx + yy - zz elseif i == 6 - yz = (q.y * q.z) - xw = (q.w * q.x) + yz = (y * z) + xw = (w * x) 2 * (yz + xw) elseif i == 7 - xz = (q.x * q.z) - yw = (q.y * q.w) + xz = (x * z) + yw = (y * w) 2 * (xz + yw) elseif i == 8 - yz = (q.y * q.z) - xw = (q.w * q.x) + yz = (y * z) + xw = (w * x) 2 * (yz - xw) elseif i == 9 - ww = (q.w * q.w) - xx = (q.x * q.x) - yy = (q.y * q.y) - zz = (q.z * q.z) + ww = (w * w) + xx = (x * x) + yy = (y * y) + zz = (z * z) ww - xx - yy + zz else @@ -182,16 +187,21 @@ function Base.getindex(q::UnitQuaternion, i::Int) end function Base.Tuple(q::UnitQuaternion) - ww = (q.w * q.w) - xx = (q.x * q.x) - yy = (q.y * q.y) - zz = (q.z * q.z) - xy = (q.x * q.y) - zw = (q.w * q.z) - xz = (q.x * q.z) - yw = (q.y * q.w) - yz = (q.y * q.z) - xw = (q.w * q.x) + w = q.q.s + x = q.q.v1 + y = q.q.v2 + z = q.q.v3 + + ww = (w * w) + xx = (x * x) + yy = (y * y) + zz = (z * z) + xy = (x * y) + zw = (w * z) + xz = (x * z) + yw = (y * w) + yz = (y * z) + xw = (w * x) # initialize rotation part return (ww + xx - yy - zz, @@ -206,8 +216,8 @@ function Base.Tuple(q::UnitQuaternion) end # ~~~~~~~~~~~~~~~ Getters ~~~~~~~~~~~~~~~ # -@inline scalar(q::UnitQuaternion) = q.w -@inline vector(q::UnitQuaternion) = SVector{3}(q.x, q.y, q.z) +@inline scalar(q::UnitQuaternion) = real(q.q) +@inline vector(q::UnitQuaternion) = vector(q.q) @inline vector(q::Quaternion) = SVector{3}(q.v1, q.v2, q.v3) """ @@ -221,7 +231,7 @@ p = MRP(1.0, 2.0, 3.0) Rotations.params(p) == @SVector [1.0, 2.0, 3.0] # true ``` """ -@inline params(q::UnitQuaternion) = SVector{4}(q.w, q.x, q.y, q.z) +@inline params(q::UnitQuaternion) = SVector{4}(q.q.s, q.q.v1, q.q.v2, q.q.v3) # TODO: this will be removed, because Quaternion is not a subtype of Rotation @inline params(q::Quaternion) = SVector{4}(q.s, q.v1, q.v2, q.v3) @@ -236,11 +246,16 @@ end # ~~~~~~~~~~~~~~~ Math Operations ~~~~~~~~~~~~~~~ # # Inverses -inv(q::Q) where Q <: UnitQuaternion = Q(q.w, -q.x, -q.y, -q.z, false) +Base.inv(q::Q) where Q <: UnitQuaternion = Q(conj(q.q)) function _normalize(q::Q) where Q <: UnitQuaternion + w = q.q.s + x = q.q.v1 + y = q.q.v2 + z = q.q.v3 + n = norm(params(q)) - Q(q.w/n, q.x/n, q.y/n, q.z/n) + Q(w/n, x/n, y/n, z/n) end # Identity @@ -270,12 +285,17 @@ function expm(ϕ::AbstractVector) end function _log_as_quat(q::Q, eps=1e-6) where Q <: UnitQuaternion + w = q.q.s + x = q.q.v1 + y = q.q.v2 + z = q.q.v3 + # Assumes unit quaternion - θ = sqrt(q.x^2 + q.y^2 + q.z^2) + θ = sqrt(x^2 + y^2 + z^2) if θ > eps - M = atan(θ, q.w)/θ + M = atan(θ, w)/θ else - M = (1-(θ^2/(3q.w^2)))/q.w + M = (1-(θ^2/(3w^2)))/w end _pure_quaternion(M*vector(q)) end @@ -299,11 +319,8 @@ rmult(w) * SVector(q) Sets the output mapping equal to the mapping of `w` """ -function (*)(q::UnitQuaternion, w::UnitQuaternion) - UnitQuaternion(q.w * w.w - q.x * w.x - q.y * w.y - q.z * w.z, - q.w * w.x + q.x * w.w + q.y * w.z - q.z * w.y, - q.w * w.y - q.x * w.z + q.y * w.w + q.z * w.x, - q.w * w.z + q.x * w.y - q.y * w.x + q.z * w.w, false) +function (*)(q1::UnitQuaternion, q2::UnitQuaternion) + return UnitQuaternion(q1.q*q2.q) end """ @@ -315,8 +332,8 @@ Equivalent to `hmat()' lmult(q) * rmult(q)' hmat() * r` """ function Base.:*(q::UnitQuaternion, r::StaticVector) # must be StaticVector to avoid ambiguity check_length(r, 3) - w = q.w - v = vector(q) + w = real(q.q) + v = vector(q.q) (w^2 - v'v)*r + 2*v*(v'r) + 2*w*cross(v,r) end @@ -373,11 +390,16 @@ end `lmult(q2)*params(q1)` returns a vector equivalent to `q2*q1` (quaternion composition) """ function lmult(q::UnitQuaternion) + w = q.q.s + x = q.q.v1 + y = q.q.v2 + z = q.q.v3 + SA[ - q.w -q.x -q.y -q.z; - q.x q.w -q.z q.y; - q.y q.z q.w -q.x; - q.z -q.y q.x q.w; + w -x -y -z; + x w -z y; + y z w -x; + z -y x w; ] end function lmult(q::Quaternion) @@ -397,11 +419,16 @@ lmult(q::StaticVector{4}) = lmult(UnitQuaternion(q, false)) `rmult(q1)*params(q2)` return a vector equivalent to `q2*q1` (quaternion composition) """ function rmult(q::UnitQuaternion) + w = q.q.s + x = q.q.v1 + y = q.q.v2 + z = q.q.v3 + SA[ - q.w -q.x -q.y -q.z; - q.x q.w q.z -q.y; - q.y -q.z q.w q.x; - q.z q.y -q.x q.w; + w -x -y -z; + x w z -y; + y -z w x; + z y -x w; ] end function rmult(q::Quaternion) @@ -476,11 +503,16 @@ Useful for converting Jacobians from R⁴ to R³ and differential quaternion parameterization are the same up to a constant. """ function ∇differential(q::UnitQuaternion) + w = q.q.s + x = q.q.v1 + y = q.q.v2 + z = q.q.v3 + SA[ - -q.x -q.y -q.z; - q.w -q.z q.y; - q.z q.w -q.x; - -q.y q.x q.w; + -x -y -z; + w -z y; + z w -x; + -y x w; ] end From 020d05589105ef5342661a3341abaf7a76bf7ddb Mon Sep 17 00:00:00 2001 From: hyrodium Date: Mon, 8 Nov 2021 01:52:42 +0900 Subject: [PATCH 3/9] fix typo --- README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/README.md b/README.md index d6b43ed9..24a4a960 100644 --- a/README.md +++ b/README.md @@ -222,7 +222,7 @@ the computationally cheapest map and often performs well. Rotations.jl provides the `RotationError` type for representing rotation errors, that act just like a `SVector{3}` but carry the nonlinear map used to compute the error, which can also be used to convert the error back to a `UnitQuaternion` (and, by extention, any other 3D rotation parameterization). The following methods are useful for computing `RotationError`s and adding them back to the nominal rotation: ```julia rotation_error(R1::Rotation, R2::Rotation, error_map::ErrorMap) # compute the error between `R1` and `R2` using `error_map` -add_error(R::Roation, err::RotationError) # "adds" the error to `R` by converting back a `UnitQuaterion` and composing with `R` +add_error(R::Rotation, err::RotationError) # "adds" the error to `R` by converting back a `UnitQuaterion` and composing with `R` ``` or their aliases ```julia From 37c8d1f5e668a5317e76308935bfff55fc738e0f Mon Sep 17 00:00:00 2001 From: hyrodium Date: Wed, 10 Nov 2021 22:32:21 +0900 Subject: [PATCH 4/9] wip, update around fields of UnitQuaternion --- src/error_maps.jl | 20 ++++++++++---------- test/derivative_tests.jl | 10 +++++----- test/rotation_tests.jl | 32 +++++++++++++++----------------- 3 files changed, 30 insertions(+), 32 deletions(-) diff --git a/src/error_maps.jl b/src/error_maps.jl index 20e55e6a..fb1d38c4 100644 --- a/src/error_maps.jl +++ b/src/error_maps.jl @@ -57,16 +57,16 @@ struct InvExponentialMap <: InvErrorMap end struct InvMRPMap <: InvErrorMap end struct InvQuatVecMap <: InvErrorMap end -inv(::CayleyMap) = InvCayleyMap() -inv(::ExponentialMap) = InvExponentialMap() -inv(::QuatVecMap) = InvQuatVecMap() -inv(::MRPMap) = InvMRPMap() -inv(::IdentityMap) = IdentityMap() - -inv(::InvCayleyMap) = CayleyMap() -inv(::InvExponentialMap) = ExponentialMap() -inv(::InvQuatVecMap) = QuatVecMap() -inv(::InvMRPMap) = MRPMap() +Base.inv(::CayleyMap) = InvCayleyMap() +Base.inv(::ExponentialMap) = InvExponentialMap() +Base.inv(::QuatVecMap) = InvQuatVecMap() +Base.inv(::MRPMap) = InvMRPMap() +Base.inv(::IdentityMap) = IdentityMap() + +Base.inv(::InvCayleyMap) = CayleyMap() +Base.inv(::InvExponentialMap) = ExponentialMap() +Base.inv(::InvQuatVecMap) = QuatVecMap() +Base.inv(::InvMRPMap) = MRPMap() # Scalings diff --git a/test/derivative_tests.jl b/test/derivative_tests.jl index 374b8a71..b8d81609 100644 --- a/test/derivative_tests.jl +++ b/test/derivative_tests.jl @@ -18,7 +18,7 @@ using ForwardDiff # test jacobian to a Rotation matrix R_jac = Rotations.jacobian(RotMatrix, q) FD_jac = ForwardDiff.jacobian(x -> SVector{9}(UnitQuaternion(x[1], x[2], x[3], x[4])), - SVector(q.w, q.x, q.y, q.z)) + Rotations.params(q)) # compare @test FD_jac ≈ R_jac @@ -33,7 +33,7 @@ using ForwardDiff # test jacobian to a Rotation matrix R_jac = Rotations.jacobian(UnitQuaternion, p) FD_jac = ForwardDiff.jacobian(x -> (q = UnitQuaternion(MRP(x[1],x[2],x[3])); - SVector(q.w, q.x, q.y, q.z)), + Rotations.params(q)), SVector(p.x, p.y, p.z)) # compare @@ -46,7 +46,7 @@ using ForwardDiff # test jacobian to a Rotation matrix R_jac = Rotations.jacobian(UnitQuaternion, p) FD_jac = ForwardDiff.jacobian(x -> (q = UnitQuaternion(MRP(x[1],x[2],x[3])); - SVector(q.w, q.x, q.y, q.z)), + Rotations.params(q)), SVector(p.x, p.y, p.z)) # compare @@ -63,7 +63,7 @@ using ForwardDiff R_jac = Rotations.jacobian(MRP, q) FD_jac = ForwardDiff.jacobian(x -> (p = MRP(UnitQuaternion(x[1], x[2], x[3], x[4])); SVector(p.x, p.y, p.z)), - SVector(q.w, q.x, q.y, q.z)) + Rotations.params(q)) # compare @test FD_jac ≈ R_jac @@ -158,7 +158,7 @@ using ForwardDiff # test jacobian to a Rotation matrix R_jac = Rotations.jacobian(q, v) FD_jac = ForwardDiff.jacobian(x -> UnitQuaternion(x[1], x[2], x[3], x[4])*v, - SVector(q.w, q.x, q.y, q.z)) + Rotations.params(q)) # compare @test FD_jac ≈ R_jac diff --git a/test/rotation_tests.jl b/test/rotation_tests.jl index b1f993a3..c5f198ee 100644 --- a/test/rotation_tests.jl +++ b/test/rotation_tests.jl @@ -196,22 +196,20 @@ all_types = (RotMatrix{3}, AngleAxis, RotationVec, @testset "Quaternion double cover" begin repeats = 100 - @testset "Quaternion double cover" begin - Q = UnitQuaternion - for i = 1 : repeats - q = rand(UnitQuaternion) + Q = UnitQuaternion + for i = 1 : repeats + q = rand(UnitQuaternion) - q2 = UnitQuaternion(-q.w, -q.x, -q.y, -q.z) # normalize: need a tolerance - @test SVector(q2.w, q2.x, q2.y, q2.z) ≈ SVector(-q.w, -q.x, -q.y, -q.z) atol = 100 * eps() - @test q ≈ q2 atol = 100 * eps() + q2 = UnitQuaternion(-q.q) # normalize: need a tolerance + @test Rotations.params(q2) ≈ -Rotations.params(q) atol = 100 * eps() + @test q ≈ q2 atol = 100 * eps() - q3 = UnitQuaternion(-q.w, -q.x, -q.y, -q.z, false) # don't normalize: everything is exact - @test (q3.w, q3.x, q3.y, q3.z) == (-q.w, -q.x, -q.y, -q.z) - @test q == q3 + q3 = UnitQuaternion(-q.q.s, -q.q.v1, -q.q.v2, -q.q.v3, false) # don't normalize: everything is exact + @test Rotations.params(q3) == -Rotations.params(q) + @test q == q3 - Δq = q \ q3 - @test Δq ≈ one(UnitQuaternion) atol = 100 * eps() - end + Δq = q \ q3 + @test Δq ≈ one(UnitQuaternion) atol = 100 * eps() end end @@ -440,15 +438,15 @@ all_types = (RotMatrix{3}, AngleAxis, RotationVec, w, x, y, z = 1., 2., 3., 4. quat = UnitQuaternion(w, x, y, z) - @test norm([quat.w, quat.x, quat.y, quat.z]) ≈ 1. + @test norm(Rotations.params(quat)) ≈ 1. quat = UnitQuaternion(w, x, y, z, false) - @test norm([quat.w, quat.x, quat.y, quat.z]) ≈ norm([w, x, y, z]) + @test norm(Rotations.params(quat)) ≈ norm([w, x, y, z]) w, x, y, z = 1., 2., 3., 4. quat = UnitQuaternion(w, x, y, z) - @test norm([quat.w, quat.x, quat.y, quat.z]) ≈ 1. + @test norm(Rotations.params(quat)) ≈ 1. quat = UnitQuaternion(w, x, y, z, false) - @test norm([quat.w, quat.x, quat.y, quat.z]) ≈ norm([w, x, y, z]) + @test norm(Rotations.params(quat)) ≈ norm([w, x, y, z]) end @testset "Testing RotMatrix conversion to Tuple" begin From a2cb8bb9680008a02b0bda13112248e99ea60e37 Mon Sep 17 00:00:00 2001 From: hyrodium Date: Wed, 10 Nov 2021 22:33:02 +0900 Subject: [PATCH 5/9] wip, update around fields of UnitQuaternion --- src/unitquaternion.jl | 2 +- test/principal_value_tests.jl | 20 ++++++++++---------- 2 files changed, 11 insertions(+), 11 deletions(-) diff --git a/src/unitquaternion.jl b/src/unitquaternion.jl index cfb41d81..caf7842e 100644 --- a/src/unitquaternion.jl +++ b/src/unitquaternion.jl @@ -266,7 +266,7 @@ end _pure_quaternion(v::AbstractVector) _pure_quaternion(x, y, z) -Create a `Quaternion` with zero scalar part (i.e. `q.w == 0`). +Create a `Quaternion` with zero scalar part (i.e. `q.q.s == 0`). """ function _pure_quaternion(v::AbstractVector) check_length(v, 3) diff --git a/test/principal_value_tests.jl b/test/principal_value_tests.jl index c23fbfef..761fcc8e 100644 --- a/test/principal_value_tests.jl +++ b/test/principal_value_tests.jl @@ -12,7 +12,7 @@ end for i = 1:1000 qq = rand(UnitQuaternion) qq_prin = principal_value(qq) - @test 0.0 <= qq_prin.w + @test 0.0 ≤ real(qq_prin.q) @test qq_prin ≈ qq end end @@ -21,7 +21,7 @@ end for i = 1:1000 aa = AngleAxis(100.0 * randn(), randn(), randn(), randn()) aa_prin = principal_value(aa) - @test 0.0 <= aa_prin.theta + @test 0.0 ≤ aa_prin.theta @test aa_prin ≈ aa end end @@ -30,12 +30,12 @@ end for i = 1:1000 rv = RotationVec(100.0 * randn(), 100.0 * randn(), 100.0 * randn()) rv_prin = principal_value(rv) - @test rotation_angle(rv_prin) <= pi + @test rotation_angle(rv_prin) ≤ π @test rv_prin ≈ rv end rv = RotationVec(0.0, 0.0, 0.0) rv_prin = principal_value(rv) - @test rotation_angle(rv_prin) <= pi + @test rotation_angle(rv_prin) ≤ π @test rv_prin ≈ rv end @@ -57,7 +57,7 @@ end for i = 1:1000 r = $(rot_type)(100.0 * randn()) r_prin = principal_value(r) - @test -pi <= r_prin.theta <= pi + @test -π ≤ r_prin.theta ≤ π @test r_prin ≈ r end end @@ -69,8 +69,8 @@ end for i = 1:1000 r = $rot_type(100.0 * randn(), 100.0 * randn()) r_prin = principal_value(r) - @test -pi <= r_prin.theta1 <= pi - @test -pi <= r_prin.theta2 <= pi + @test -π ≤ r_prin.theta1 ≤ π + @test -π ≤ r_prin.theta2 ≤ π @test r_prin ≈ r end end @@ -82,9 +82,9 @@ end for i = 1:1000 r = $(rot_type)(100.0 * randn(), 100.0 * randn(), 100.0 * randn()) r_prin = principal_value(r) - @test -pi <= r_prin.theta1 <= pi - @test -pi <= r_prin.theta2 <= pi - @test -pi <= r_prin.theta3 <= pi + @test -π ≤ r_prin.theta1 ≤ π + @test -π ≤ r_prin.theta2 ≤ π + @test -π ≤ r_prin.theta3 ≤ π @test r_prin ≈ r end end From f5c7b466858fa590aaddc17f159a2b883ec20626 Mon Sep 17 00:00:00 2001 From: hyrodium Date: Mon, 8 Nov 2021 02:12:56 +0900 Subject: [PATCH 6/9] wip, update around fields of UnitQuaternion --- src/unitquaternion.jl | 3 --- test/rodrigues_params.jl | 3 ++- test/unitquat.jl | 3 ++- 3 files changed, 4 insertions(+), 5 deletions(-) diff --git a/src/unitquaternion.jl b/src/unitquaternion.jl index caf7842e..2dcf9450 100644 --- a/src/unitquaternion.jl +++ b/src/unitquaternion.jl @@ -233,9 +233,6 @@ Rotations.params(p) == @SVector [1.0, 2.0, 3.0] # true """ @inline params(q::UnitQuaternion) = SVector{4}(q.q.s, q.q.v1, q.q.v2, q.q.v3) -# TODO: this will be removed, because Quaternion is not a subtype of Rotation -@inline params(q::Quaternion) = SVector{4}(q.s, q.v1, q.v2, q.v3) - # ~~~~~~~~~~~~~~~ Initializers ~~~~~~~~~~~~~~~ # function Random.rand(rng::AbstractRNG, ::Random.SamplerType{<:UnitQuaternion{T}}) where T _normalize(UnitQuaternion{T}(randn(rng,T), randn(rng,T), randn(rng,T), randn(rng,T))) diff --git a/test/rodrigues_params.jl b/test/rodrigues_params.jl index 33de7ee5..1e771efb 100644 --- a/test/rodrigues_params.jl +++ b/test/rodrigues_params.jl @@ -67,7 +67,8 @@ end @test qdot ≈ 0.5*lmult(q)*hmat(ω) @test ω ≈ 2*vmat()*lmult(q)'qdot @test ω ≈ 2*vmat()*lmult(inv(q))*qdot - @test qdot ≈ 0.5 * params(Quaternion(q)*_pure_quaternion(ω)) + q2 = Quaternion(q)*_pure_quaternion(ω) + @test qdot ≈ SVector(q2.s, q2.v1, q2.v2, q2.v3)/2 # MRPs ω = @SVector rand(3) diff --git a/test/unitquat.jl b/test/unitquat.jl index 5438c918..90c007db 100644 --- a/test/unitquat.jl +++ b/test/unitquat.jl @@ -92,7 +92,8 @@ import Rotations: vmat, rmult, lmult, hmat, tmat @test rmult(params(q)) == rmult(q) @test lmult(params(q)) == lmult(q) - @test hmat(r) == params(_pure_quaternion(r)) + r_pure = _pure_quaternion(r) + @test hmat(r) == SVector(r_pure.s, r_pure.v1, r_pure.v2, r_pure.v3) # Test Jacobians @test ForwardDiff.jacobian(q -> UnitQuaternion(q, false) * r, params(q)) ≈ From 21d588bb97eaf4a21ce5c17313635077de950b49 Mon Sep 17 00:00:00 2001 From: hyrodium Date: Mon, 8 Nov 2021 02:22:10 +0900 Subject: [PATCH 7/9] update around fields of UnitQuaternion --- src/unitquaternion.jl | 14 -------------- 1 file changed, 14 deletions(-) diff --git a/src/unitquaternion.jl b/src/unitquaternion.jl index 2dcf9450..f7d90ebe 100644 --- a/src/unitquaternion.jl +++ b/src/unitquaternion.jl @@ -57,20 +57,6 @@ function Quaternions.Quaternion(q::UnitQuaternion) return q.q end -function Base.getproperty(q::UnitQuaternion, s::Symbol) - if s == :q - return getfield(q,:q) - elseif s == :w - return getfield(q,:q).s - elseif s == :x - return getfield(q,:q).v1 - elseif s == :y - return getfield(q,:q).v2 - elseif s == :z - return getfield(q,:q).v3 - end -end - # Pass in Vectors @inline function (::Type{Q})(q::AbstractVector, normalize::Bool = true) where Q <: UnitQuaternion check_length(q, 4) From decb24bc1450f5aec9a314fa365018dab7ddad27 Mon Sep 17 00:00:00 2001 From: hyrodium Date: Wed, 10 Nov 2021 22:43:07 +0900 Subject: [PATCH 8/9] fix test for dcm --- test/rotation_tests.jl | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/test/rotation_tests.jl b/test/rotation_tests.jl index c5f198ee..fc3c74fd 100644 --- a/test/rotation_tests.jl +++ b/test/rotation_tests.jl @@ -276,7 +276,7 @@ all_types = (RotMatrix{3}, AngleAxis, RotationVec, for _ = 1:100 not_orthonormal = rand(type_rot) + rand(3, 3) * pert quat_ill_cond = UnitQuaternion(not_orthonormal) - @test 0 <= quat_ill_cond.w + @test 0 <= real(quat_ill_cond.q) @test norm(quat_ill_cond - nearest_rotation(not_orthonormal)) < 10 * pert end end From f2f89a62dac650321343ac9ebeb0149bbaf85866 Mon Sep 17 00:00:00 2001 From: hyrodium Date: Wed, 10 Nov 2021 22:46:30 +0900 Subject: [PATCH 9/9] remove unnecessary package specification in Project.toml --- Project.toml | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/Project.toml b/Project.toml index c90ead31..021fb6d0 100644 --- a/Project.toml +++ b/Project.toml @@ -17,10 +17,8 @@ julia = "0.7, 1" BenchmarkTools = "6e4b80f9-dd63-53aa-95a3-0cdb28fa8baf" ForwardDiff = "f6369f11-7733-5829-9624-2563aa707210" InteractiveUtils = "b77e0a4c-d291-57a0-90e8-8db25a27a240" -Random = "9a3f8284-a2c9-5f02-9a11-845980a1fd5c" Test = "8dfed614-e22c-5e08-85e1-65c5234f0b40" -Quaternions = "94ee1d12-ae83-5a48-8b1c-48b8ff168ae0" Unitful = "1986cc42-f94f-5a68-af5c-568840ba703d" [targets] -test = ["BenchmarkTools", "ForwardDiff", "Random", "Test", "InteractiveUtils", "Unitful", "Quaternions"] +test = ["BenchmarkTools", "ForwardDiff", "Test", "InteractiveUtils", "Unitful"]