From 715520733b756e130676be0cfed43b4abb66a654 Mon Sep 17 00:00:00 2001 From: Yuto Horikawa Date: Wed, 10 Nov 2021 21:54:42 +0900 Subject: [PATCH] Correct `UnitQuaternion` with `Quaternions.jl` (#175) * remove incorrect log(::Quaternion) * add method for log(::Rotation) and return SMatrix * fix log method for Rotation{2} * update tests for logarithm function * add dependency on Quaternions.jl * update exports for deprecated types * update around pure_quaternion * remove incorrect LinearAlgebra.norm * remove incorrect *(::Real, UnitQuaternion) * remove incorrect conj(Quaternion) * remove incorrect -(::UnitQuaternion) * update tests around norm related functions * update src and test around norm related functions * remove unnecessary import from Base * remove exp method for Quaternion * update type conversions UnitQuaternion <-> Quaternion * fix tests for Julia under v1.5 * remove unnecessary lines * remove vecnorm(q::UnitQuaternion) function * update comments * add methods for Base.:\(r::Rotation, v) * update UnitQuaternion constructor --- Project.toml | 4 +- src/Rotations.jl | 8 ++- src/core_types.jl | 9 ++++ src/mrps.jl | 1 - src/rodrigues_params.jl | 1 - src/unitquaternion.jl | 107 +++++++++++++++++++++------------------ test/2d.jl | 22 +++++++- test/rodrigues_params.jl | 4 +- test/rotation_tests.jl | 22 +++++++- test/runtests.jl | 1 + test/unitquat.jl | 28 +++++----- 11 files changed, 137 insertions(+), 70 deletions(-) diff --git a/Project.toml b/Project.toml index e6e74bb2..c90ead31 100644 --- a/Project.toml +++ b/Project.toml @@ -4,6 +4,7 @@ version = "1.0.4" [deps] LinearAlgebra = "37e2e46d-f89d-539d-b4ee-838fcccc9c8e" +Quaternions = "94ee1d12-ae83-5a48-8b1c-48b8ff168ae0" Random = "9a3f8284-a2c9-5f02-9a11-845980a1fd5c" StaticArrays = "90137ffa-7385-5640-81b9-e52037218182" Statistics = "10745b16-79ce-11e8-11f9-7d13ad32a3b2" @@ -18,7 +19,8 @@ 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"] +test = ["BenchmarkTools", "ForwardDiff", "Random", "Test", "InteractiveUtils", "Unitful", "Quaternions"] diff --git a/src/Rotations.jl b/src/Rotations.jl index 770fb7cc..5277c172 100644 --- a/src/Rotations.jl +++ b/src/Rotations.jl @@ -6,6 +6,7 @@ module Rotations using LinearAlgebra using StaticArrays using Random +using Quaternions import Statistics @@ -30,13 +31,16 @@ include("deprecated.jl") export Rotation, RotMatrix, RotMatrix2, RotMatrix3, Angle2d, - Quat, UnitQuaternion, - AngleAxis, RodriguesVec, RotationVec, RodriguesParam, MRP, + UnitQuaternion, + AngleAxis, RotationVec, RodriguesParam, MRP, RotX, RotY, RotZ, RotXY, RotYX, RotZX, RotXZ, RotYZ, RotZY, RotXYX, RotYXY, RotZXZ, RotXZX, RotYZY, RotZYZ, RotXYZ, RotYXZ, RotZXY, RotXZY, RotYZX, RotZYX, + # Deprecated, but export for compatibility + RodriguesVec, Quat, SPQuat, + # Quaternion math ops logm, expm, ⊖, ⊕, diff --git a/src/core_types.jl b/src/core_types.jl index 259bce83..740e69de 100644 --- a/src/core_types.jl +++ b/src/core_types.jl @@ -78,6 +78,15 @@ end inv(r1) * r2 end +@inline function Base.:\(r::Rotation, v::AbstractVector) + inv(r) * v +end + +# This definition is for avoiding anbiguity +@inline function Base.:\(r::Rotation, v::StaticVector) + inv(r) * v +end + ################################################################################ ################################################################################ """ diff --git a/src/mrps.jl b/src/mrps.jl index 1ee77a1b..8fcd7a52 100644 --- a/src/mrps.jl +++ b/src/mrps.jl @@ -48,7 +48,6 @@ end @inline Base.Tuple(p::MRP) = Tuple(convert(UnitQuaternion, p)) # ~~~~~~~~~~~~~~~ Math Operations ~~~~~~~~~~~~~~~ # -LinearAlgebra.norm(p::MRP) = sqrt(p.x^2 + p.y^2 + p.z^2) Base.inv(p::MRP) = MRP(-p.x, -p.y, -p.z) # ~~~~~~~~~~~~~~~ Composition ~~~~~~~~~~~~~~~ # diff --git a/src/rodrigues_params.jl b/src/rodrigues_params.jl index 82e19e12..a708a253 100644 --- a/src/rodrigues_params.jl +++ b/src/rodrigues_params.jl @@ -45,7 +45,6 @@ end @inline Base.Tuple(rp::RodriguesParam) = Tuple(convert(UnitQuaternion, rp)) # ~~~~~~~~~~~~~~~ Math Operations ~~~~~~~~~~~~~~~ # -LinearAlgebra.norm(g::RodriguesParam) = sqrt(g.x^2 + g.y^2 + g.z^2) Base.inv(p::RodriguesParam) = RodriguesParam(-p.x, -p.y, -p.z) # ~~~~~~~~~~~~~~~ Composition ~~~~~~~~~~~~~~~ # diff --git a/src/unitquaternion.jl b/src/unitquaternion.jl index 3f26782b..46342c0f 100644 --- a/src/unitquaternion.jl +++ b/src/unitquaternion.jl @@ -1,4 +1,4 @@ -import Base: +, -, *, /, \, exp, ≈, ==, inv, conj +import Base: *, /, \, exp, ≈, ==, inv """ UnitQuaternion{T} <: Rotation @@ -31,6 +31,13 @@ struct UnitQuaternion{T} <: Rotation{3,T} end end + @inline function UnitQuaternion{T}(q::Quaternion) where T + if q.norm + new{T}(q.s, q.v1, q.v2, q.v3) + 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) end @@ -41,6 +48,18 @@ function UnitQuaternion(w,x,y,z, normalize::Bool = true) UnitQuaternion{eltype(types)}(w,x,y,z, normalize) end +function UnitQuaternion(q::T) where T<:Quaternion + if q.norm + return UnitQuaternion(q.s, q.v1, q.v2, q.v3, false) + else + throw(InexactError(nameof(T), T, q)) + end +end + +function Quaternions.Quaternion(q::UnitQuaternion) + Quaternion(q.w, q.x, q.y, q.z, true) +end + # Pass in Vectors @inline function (::Type{Q})(q::AbstractVector, normalize::Bool = true) where Q <: UnitQuaternion check_length(q, 4) @@ -183,6 +202,7 @@ end # ~~~~~~~~~~~~~~~ Getters ~~~~~~~~~~~~~~~ # @inline scalar(q::UnitQuaternion) = q.w @inline vector(q::UnitQuaternion) = SVector{3}(q.x, q.y, q.z) +@inline vector(q::Quaternion) = SVector{3}(q.v1, q.v2, q.v3) """ params(R::Rotation) @@ -197,9 +217,12 @@ 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) +# 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))) + _normalize(UnitQuaternion{T}(randn(rng,T), randn(rng,T), randn(rng,T), randn(rng,T))) end @inline Base.one(::Type{Q}) where Q <: UnitQuaternion = Q(1.0, 0.0, 0.0, 0.0) @@ -207,17 +230,11 @@ end # ~~~~~~~~~~~~~~~ Math Operations ~~~~~~~~~~~~~~~ # # Inverses -conj(q::Q) where Q <: UnitQuaternion = Q(q.w, -q.x, -q.y, -q.z, false) -inv(q::UnitQuaternion) = conj(q) -(-)(q::Q) where Q <: UnitQuaternion = Q(-q.w, -q.x, -q.y, -q.z, false) - -# Norms -LinearAlgebra.norm(q::UnitQuaternion) = sqrt(q.w^2 + q.x^2 + q.y^2 + q.z^2) -vecnorm(q::UnitQuaternion) = sqrt(q.x^2 + q.y^2 + q.z^2) +inv(q::Q) where Q <: UnitQuaternion = Q(q.w, -q.x, -q.y, -q.z, false) -function LinearAlgebra.normalize(q::Q) where Q <: UnitQuaternion - n = inv(norm(q)) - Q(q.w*n, q.x*n, q.y*n, q.z*n) +function _normalize(q::Q) where Q <: UnitQuaternion + n = norm(params(q)) + Q(q.w/n, q.x/n, q.y/n, q.z/n) end # Identity @@ -225,49 +242,41 @@ end # Exponentials and Logarithms """ - pure_quaternion(v::AbstractVector) - pure_quaternion(x, y, z) + _pure_quaternion(v::AbstractVector) + _pure_quaternion(x, y, z) -Create a `UnitQuaternion` with zero scalar part (i.e. `q.w == 0`). +Create a `Quaternion` with zero scalar part (i.e. `q.w == 0`). """ -function pure_quaternion(v::AbstractVector) +function _pure_quaternion(v::AbstractVector) check_length(v, 3) - UnitQuaternion(zero(eltype(v)), v[1], v[2], v[3], false) + Quaternion(zero(eltype(v)), v[1], v[2], v[3], false) end -@inline pure_quaternion(x::Real, y::Real, z::Real) = - UnitQuaternion(zero(x), x, y, z, false) - -function exp(q::Q) where Q <: UnitQuaternion - θ = vecnorm(q) - sθ,cθ = sincos(θ) - es = exp(q.w) - M = es*sθ/θ - Q(es*cθ, q.x*M, q.y*M, q.z*M, false) -end +@inline _pure_quaternion(x::Real, y::Real, z::Real) = + Quaternion(zero(x), x, y, z, false) function expm(ϕ::AbstractVector) check_length(ϕ, 3) θ = norm(ϕ) sθ,cθ = sincos(θ/2) - M = 1//2 *sinc(θ/π/2) + M = sinc(θ/π/2)/2 UnitQuaternion(cθ, ϕ[1]*M, ϕ[2]*M, ϕ[3]*M, false) end function _log_as_quat(q::Q, eps=1e-6) where Q <: UnitQuaternion # Assumes unit quaternion - θ = vecnorm(q) + θ = sqrt(q.x^2 + q.y^2 + q.z^2) if θ > eps M = atan(θ, q.w)/θ else M = (1-(θ^2/(3q.w^2)))/q.w end - pure_quaternion(M*vector(q)) + _pure_quaternion(M*vector(q)) end function logm(q::UnitQuaternion) # Assumes unit quaternion - 2*vector(_log_as_quat(q)) + return 2*vector(_log_as_quat(q)) end # Composition @@ -305,22 +314,8 @@ function Base.:*(q::UnitQuaternion, r::StaticVector) # must be StaticVector to (w^2 - v'v)*r + 2*v*(v'r) + 2*w*cross(v,r) end -""" - (*)(q::UnitQuaternion, w::Real) - -Scalar multiplication of a quaternion. Breaks unit norm. -""" -function (*)(q::Q, w::Real) where Q<:UnitQuaternion - return Q(q.w*w, q.x*w, q.y*w, q.z*w, false) -end -(*)(w::Real, q::UnitQuaternion) = q*w - - - -(\)(q1::UnitQuaternion, q2::UnitQuaternion) = conj(q1)*q2 # Equivalent to inv(q1)*q2 -(/)(q1::UnitQuaternion, q2::UnitQuaternion) = q1*conj(q2) # Equivalent to q1*inv(q2) - -(\)(q::UnitQuaternion, r::SVector{3}) = conj(q)*r # Equivalent to inv(q)*r +(\)(q1::UnitQuaternion, q2::UnitQuaternion) = inv(q1)*q2 +(/)(q1::UnitQuaternion, q2::UnitQuaternion) = q1*inv(q2) """ rotation_between(from, to) @@ -361,7 +356,7 @@ See "Fundamentals of Spacecraft Attitude Determination and Control" by Markley a Sections 3.1-3.2 for more details. """ function kinematics(q::Q, ω::AbstractVector) where Q <: UnitQuaternion - 1//2 * params(q*Q(0.0, ω[1], ω[2], ω[3], false)) + params(q*Q(0.0, ω[1], ω[2], ω[3], false))/2 end # ~~~~~~~~~~~~~~~ Linear Algebraic Conversions ~~~~~~~~~~~~~~~ # @@ -379,6 +374,14 @@ function lmult(q::UnitQuaternion) q.z -q.y q.x q.w; ] end +function lmult(q::Quaternion) + SA[ + q.s -q.v1 -q.v2 -q.v3; + q.v1 q.s -q.v3 q.v2; + q.v2 q.v3 q.s -q.v1; + q.v3 -q.v2 q.v1 q.s; + ] +end lmult(q::StaticVector{4}) = lmult(UnitQuaternion(q, false)) """ @@ -395,6 +398,14 @@ function rmult(q::UnitQuaternion) q.z q.y -q.x q.w; ] end +function rmult(q::Quaternion) + SA[ + q.s -q.v1 -q.v2 -q.v3; + q.v1 q.s q.v3 -q.v2; + q.v2 -q.v3 q.s q.v1; + q.v3 q.v2 -q.v1 q.s; + ] +end rmult(q::SVector{4}) = rmult(UnitQuaternion(q, false)) """ diff --git a/test/2d.jl b/test/2d.jl index 6e8a6bf3..658d61f8 100644 --- a/test/2d.jl +++ b/test/2d.jl @@ -51,7 +51,7 @@ using Rotations, StaticArrays, Test # check on the inverse function ################################ - @testset "Testing inverse()" begin + @testset "Testing inv()" begin repeats = 100 for R in [RotMatrix{2,Float64}, Angle2d{Float64}] I = one(R) @@ -67,6 +67,26 @@ using Rotations, StaticArrays, Test end end + ################################ + # check on the norm functions + ################################ + + @testset "Testing norm() and normalize()" begin + repeats = 100 + for R in [RotMatrix{2,Float64}, Angle2d{Float64}] + I = one(R) + Random.seed!(0) + for i = 1:repeats + r = rand(R) + @test norm(r) ≈ norm(Matrix(r)) + if VERSION ≥ v"1.5" + @test normalize(r) ≈ normalize(Matrix(r)) + @test normalize(r) isa SMatrix + end + end + end + end + ######################################################################### # Rotate some stuff ######################################################################### diff --git a/test/rodrigues_params.jl b/test/rodrigues_params.jl index f1f10758..33de7ee5 100644 --- a/test/rodrigues_params.jl +++ b/test/rodrigues_params.jl @@ -30,7 +30,7 @@ import Rotations: ∇rotate, ∇composition1, ∇composition2, skew, params # Math operations g = rand(R) - @test norm(g) == sqrt(g.x^2 + g.y^2 + g.z^2) + @test norm(g) == norm(Matrix(g)) # Test Jacobians R = RodriguesParam @@ -67,7 +67,7 @@ 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(q*pure_quaternion(ω)) + @test qdot ≈ 0.5 * params(Quaternion(q)*_pure_quaternion(ω)) # MRPs ω = @SVector rand(3) diff --git a/test/rotation_tests.jl b/test/rotation_tests.jl index c232b338..df50ee2b 100644 --- a/test/rotation_tests.jl +++ b/test/rotation_tests.jl @@ -129,7 +129,7 @@ all_types = (RotMatrix{3}, AngleAxis, RotationVec, # check on the inverse function ################################ - @testset "Testing inverse()" begin + @testset "Testing inv()" begin repeats = 100 I = one(RotMatrix{3,Float64}) @testset "$(R)" for R in all_types @@ -149,6 +149,26 @@ all_types = (RotMatrix{3}, AngleAxis, RotationVec, end end + ################################ + # check on the norm functions + ################################ + + @testset "Testing norm() and normalize()" begin + repeats = 100 + for R in all_types + I = one(R) + Random.seed!(0) + for i = 1:repeats + r = rand(R) + @test norm(r) ≈ norm(Matrix(r)) + if VERSION ≥ v"1.5" + @test normalize(r) ≈ normalize(Matrix(r)) + @test normalize(r) isa SMatrix + end + end + end + end + ######################################################################### # Rotate some stuff ######################################################################### diff --git a/test/runtests.jl b/test/runtests.jl index bb9275fc..ca27eeb4 100644 --- a/test/runtests.jl +++ b/test/runtests.jl @@ -3,6 +3,7 @@ using LinearAlgebra using Rotations using StaticArrays using InteractiveUtils: subtypes +using Quaternions import Unitful import Random diff --git a/test/unitquat.jl b/test/unitquat.jl index c3ceba3a..5438c918 100644 --- a/test/unitquat.jl +++ b/test/unitquat.jl @@ -1,7 +1,7 @@ using ForwardDiff import Rotations: jacobian, ∇rotate, ∇composition1, ∇composition2 -import Rotations: kinematics, pure_quaternion, params +import Rotations: kinematics, _pure_quaternion, params import Rotations: vmat, rmult, lmult, hmat, tmat @testset "Unit Quaternions" begin @@ -24,9 +24,9 @@ import Rotations: vmat, rmult, lmult, hmat, tmat r = @SVector rand(3) r32 = SVector{3,Float32}(r) - @test pure_quaternion(r) isa UnitQuaternion{Float64} - @test pure_quaternion(r32) isa UnitQuaternion{Float32} - @test pure_quaternion(r).w == 0 + @test _pure_quaternion(r) isa Quaternion{Float64} + @test _pure_quaternion(r32) isa Quaternion{Float32} + @test real(_pure_quaternion(r)) == 0 @test UnitQuaternion{Float64}(1, 0, 0, 0) isa UnitQuaternion{Float64} @test UnitQuaternion{Float32}(1, 0, 0, 0) isa UnitQuaternion{Float32} @@ -52,22 +52,24 @@ import Rotations: vmat, rmult, lmult, hmat, tmat ϕ = inv(ExponentialMap())(q1) @test expm(ϕ * 2) ≈ q1 - q = Rotations.pure_quaternion(ϕ) - @test exp(q) ≈ q1 + q = Rotations._pure_quaternion(ϕ) + @test UnitQuaternion(exp(q)) ≈ q1 + @test exp(q) isa Quaternion q = UnitQuaternion((@SVector [1, 2, 3, 4.0]), false) - @test 2 * q == UnitQuaternion((@SVector [2, 4, 6, 8.0]), false) - @test q * 2 == UnitQuaternion((@SVector [2, 4, 6, 8.0]), false) + @test 2 * q == 2 * Matrix(q) + @test q * 2 == 2 * Matrix(q) # Axis-angle ϕ = 0.1 * @SVector [1, 0, 0] q = expm(ϕ) + @test q isa UnitQuaternion @test logm(expm(ϕ)) ≈ ϕ @test expm(logm(q1)) ≈ q1 @test rotation_angle(q) ≈ 0.1 @test rotation_axis(q) == [1, 0, 0] - @test norm(q1 * ExponentialMap()(ϕ)) ≈ 1 + @test norm(q1 * ExponentialMap()(ϕ)) ≈ √3 @test q1 ⊖ q2 isa StaticVector{3} @test (q1 * CayleyMap()(ϕ)) ⊖ q1 ≈ ϕ @@ -81,7 +83,7 @@ import Rotations: vmat, rmult, lmult, hmat, tmat @test q3 ⊖ q2 ≈ inv(CayleyMap())(q1) q = q1 - rhat = Rotations.pure_quaternion(r) + rhat = Rotations._pure_quaternion(r) @test q * r ≈ vmat() * lmult(q) * rmult(q)' * vmat()'r @test q * r ≈ vmat() * lmult(q) * rmult(q)' * hmat(r) @test q * r ≈ vmat() * lmult(q) * lmult(rhat) * tmat() * params(q) @@ -90,7 +92,7 @@ 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)) + @test hmat(r) == params(_pure_quaternion(r)) # Test Jacobians @test ForwardDiff.jacobian(q -> UnitQuaternion(q, false) * r, params(q)) ≈ @@ -125,12 +127,12 @@ import Rotations: vmat, rmult, lmult, hmat, tmat @test expm(SA_F32[1, 2, 3]) isa UnitQuaternion{Float32} q32 = rand(UnitQuaternion{Float32}) - @test Rotations._log_as_quat(q32) isa UnitQuaternion{Float32} + @test Rotations._log_as_quat(q32) isa Quaternion{Float32} @test log(q32) isa SMatrix @test eltype(logm(q32)) == Float32 @test expm(logm(q32)) ≈ q32 - @test normalize(q32) isa UnitQuaternion{Float32} + @test normalize(q32) isa SMatrix{3,3,Float32} ω = @SVector rand(3) ω32 = Float32.(ω)