From eec7cb9f342fd747aacf796d811247f2afa9ce64 Mon Sep 17 00:00:00 2001 From: Tim Holy Date: Thu, 12 Oct 2023 11:19:23 -0500 Subject: [PATCH 1/6] Use `sinc` to fix RotationVec near zero MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit By converting straight from RotationVec->QuatRotation, and using an implementation that exploits `sinc` to handle θ ≈ 0, we get continuous & differentiable behavior. --- src/angleaxis_types.jl | 22 ++++++++-------------- test/derivative_tests.jl | 13 +++++++++++++ test/rotation_tests.jl | 16 ++++++++++++++++ 3 files changed, 37 insertions(+), 14 deletions(-) diff --git a/src/angleaxis_types.jl b/src/angleaxis_types.jl index 3c45605a..0344b74d 100644 --- a/src/angleaxis_types.jl +++ b/src/angleaxis_types.jl @@ -172,23 +172,17 @@ function (::Type{RV})(aa::AngleAxis) where RV <: RotationVec return RV(aa.theta * aa.axis_x, aa.theta * aa.axis_y, aa.theta * aa.axis_z) end -function (::Type{Q})(rv::RotationVec) where Q <: QuatRotation - return QuatRotation(AngleAxis(rv)) +@inline function (::Type{Q})(rv::RotationVec) where Q <: QuatRotation + theta = rotation_angle(rv) + ϕ = theta / (2π) + sc = sinc(ϕ) / 2 # this form gracefully handles theta = 0 + c = cos(theta / 2) + return Q(c, sc * rv.sx, sc * rv.sy, sc * rv.sz, false) end (::Type{RV})(q::QuatRotation) where {RV <: RotationVec} = RV(AngleAxis(q)) -function Base.:*(rv::RotationVec{T1}, v::StaticVector{3, T2}) where {T1,T2} - theta = rotation_angle(rv) - if (theta > eps(T1)) # use eps here because we have the 1st order series expansion defined - return AngleAxis(rv) * v - else - return similar_type(typeof(v), promote_type(T1,T2))( - v[1] + rv.sy * v[3] - rv.sz * v[2], - v[2] + rv.sz * v[1] - rv.sx * v[3], - v[3] + rv.sx * v[2] - rv.sy * v[1]) - end -end +Base.:*(rv::RotationVec, v::StaticVector{3}) = QuatRotation(rv) * v @inline Base.:*(rv::RotationVec, r::Rotation) = QuatRotation(rv) * r @inline Base.:*(rv::RotationVec, r::RotMatrix) = QuatRotation(rv) * r @@ -203,7 +197,7 @@ end @inline Base.inv(rv::RotationVec) = RotationVec(-rv.sx, -rv.sy, -rv.sz) # rotation properties -@inline rotation_angle(rv::RotationVec) = sqrt(rv.sx * rv.sx + rv.sy * rv.sy + rv.sz * rv.sz) +@inline rotation_angle(rv::RotationVec) = hypot(rv.sx, rv.sy, rv.sz) function rotation_axis(rv::RotationVec) # what should this return for theta = 0? theta = rotation_angle(rv) return (theta > 0 ? SVector(rv.sx / theta, rv.sy / theta, rv.sz / theta) : SVector(one(theta), zero(theta), zero(theta))) diff --git a/test/derivative_tests.jl b/test/derivative_tests.jl index 48e41384..726ea3bf 100644 --- a/test/derivative_tests.jl +++ b/test/derivative_tests.jl @@ -180,6 +180,19 @@ using ForwardDiff @test FD_jac ≈ R_jac end end + + @testset "RotationVec near zero" begin + rot(x) = RotationVec(x, 0, 0) + for i = 1:10 + v = randn(SVector{3,Float64}) + rotv(x) = rot(x)*v + drotv(x) = ForwardDiff.derivative(rotv, x) + @test drotv(0.0) ≈ [0, -v[3], v[2]] + @test drotv(1e-20) ≈ [0, -v[3], v[2]] + @test_broken ForwardDiff.derivative(drotv, 0.0) ≈ [0, -v[2], -v[3]] + @test ForwardDiff.derivative(drotv, 1e-20) ≈ [0, -v[2], -v[3]] + end + end #= # rotate a point by an MRP @testset "Jacobian (MRP rotation)" begin diff --git a/test/rotation_tests.jl b/test/rotation_tests.jl index c0208448..01a314f7 100644 --- a/test/rotation_tests.jl +++ b/test/rotation_tests.jl @@ -334,9 +334,25 @@ all_types = (RotMatrix3, RotMatrix{3}, AngleAxis, RotationVec, end @test norm(rotation_axis(QuatRotation(1.0, 0.0, 0.0, 0.0))) ≈ 1.0 + # TODO RotX, RotXY? end + @testset "RotationVec->QuatRotation, especially near zero" begin + for i = 1:10 + p = rand(RotationVec) + all(iszero, Rotations.params(p)) && continue + @test QuatRotation(p) ≈ QuatRotation(AngleAxis(p)) + end + + @test rotation_angle(RotationVec(0.0, 0.0, 0.0)) ≈ 0.0 + @test rotation_axis(RotationVec(0.0, 0.0, 0.0)) ≈ [1.0, 0.0, 0.0] + @test QuatRotation(RotationVec(0.0, 0.0, 0.0)) == QuatRotation(1.0, 0.0, 0.0, 0.0) + qr = QuatRotation(RotationVec(2e-30, 0.0, 0.0)) + @test qr.w ≈ 1 + @test qr.x ≈ 1e-30 + end + ######################################################################### # Check construction of QuatRotation given two vectors ######################################################################### From 74d6959fdaaf3eac46c8ca30074e465eedd7534e Mon Sep 17 00:00:00 2001 From: Tim Holy Date: Thu, 12 Oct 2023 13:54:30 -0500 Subject: [PATCH 2/6] Use new implementation only for small theta --- src/angleaxis_types.jl | 14 ++++++++++---- 1 file changed, 10 insertions(+), 4 deletions(-) diff --git a/src/angleaxis_types.jl b/src/angleaxis_types.jl index 0344b74d..2dd873ce 100644 --- a/src/angleaxis_types.jl +++ b/src/angleaxis_types.jl @@ -174,10 +174,16 @@ end @inline function (::Type{Q})(rv::RotationVec) where Q <: QuatRotation theta = rotation_angle(rv) - ϕ = theta / (2π) - sc = sinc(ϕ) / 2 # this form gracefully handles theta = 0 - c = cos(theta / 2) - return Q(c, sc * rv.sx, sc * rv.sy, sc * rv.sz, false) + if theta < sqrt(eps(typeof(theta))) + ϕ = theta / (2π) + sc = sinc(ϕ) / 2 # this form gracefully handles theta = 0 + qx, qy, qz = sc * rv.sx, sc * rv.sy, sc * rv.sz + return Q(cos(theta / 2), qx, qy, qz, false) + else + s, c = sincos(theta / 2) + sθ = s / theta + return Q(c, sθ * rv.sx, sθ * rv.sy, sθ * rv.sz, false) + end end (::Type{RV})(q::QuatRotation) where {RV <: RotationVec} = RV(AngleAxis(q)) From 4330c72f47e557d085449116242f7fe390a0f1ab Mon Sep 17 00:00:00 2001 From: Tim Holy Date: Thu, 12 Oct 2023 13:59:12 -0500 Subject: [PATCH 3/6] Relax adjoint test to approxeq --- test/rotation_tests.jl | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/test/rotation_tests.jl b/test/rotation_tests.jl index 01a314f7..e1b652e9 100644 --- a/test/rotation_tests.jl +++ b/test/rotation_tests.jl @@ -154,8 +154,8 @@ all_types = (RotMatrix3, RotMatrix{3}, AngleAxis, RotationVec, r1 = rand(R) r2 = rand(R) v = @SVector rand(3) - @test inv(r1) == adjoint(r1) - @test inv(r1) == transpose(r1) + @test inv(r1) ≈ adjoint(r1) + @test inv(r1) ≈ transpose(r1) @test inv(r1)*r1 ≈ I @test r1*inv(r1) ≈ I @test r1/r1 ≈ I From 8dd986d45b9b2ef6067f2071ae3a07ef22a409ec Mon Sep 17 00:00:00 2001 From: Yuto Horikawa Date: Fri, 20 Oct 2023 21:22:21 +0900 Subject: [PATCH 4/6] Fix test with `@test_broken` --- test/derivative_tests.jl | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/test/derivative_tests.jl b/test/derivative_tests.jl index 726ea3bf..084e8e4f 100644 --- a/test/derivative_tests.jl +++ b/test/derivative_tests.jl @@ -187,7 +187,8 @@ using ForwardDiff v = randn(SVector{3,Float64}) rotv(x) = rot(x)*v drotv(x) = ForwardDiff.derivative(rotv, x) - @test drotv(0.0) ≈ [0, -v[3], v[2]] + # The following broken tests will be fixed by https://github.com/JuliaDiff/ForwardDiff.jl/pull/669 + @test_broken drotv(0.0) ≈ [0, -v[3], v[2]] @test drotv(1e-20) ≈ [0, -v[3], v[2]] @test_broken ForwardDiff.derivative(drotv, 0.0) ≈ [0, -v[2], -v[3]] @test ForwardDiff.derivative(drotv, 1e-20) ≈ [0, -v[2], -v[3]] From 288b279319309377f854435fdb587e8418f7b870 Mon Sep 17 00:00:00 2001 From: Yuto Horikawa Date: Fri, 20 Oct 2023 21:22:53 +0900 Subject: [PATCH 5/6] Update tests for `inv`, `adjoint`, and `transpose` --- test/rotation_tests.jl | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/test/rotation_tests.jl b/test/rotation_tests.jl index e1b652e9..16a3f8e8 100644 --- a/test/rotation_tests.jl +++ b/test/rotation_tests.jl @@ -154,8 +154,7 @@ all_types = (RotMatrix3, RotMatrix{3}, AngleAxis, RotationVec, r1 = rand(R) r2 = rand(R) v = @SVector rand(3) - @test inv(r1) ≈ adjoint(r1) - @test inv(r1) ≈ transpose(r1) + @test inv(r1) === adjoint(r1) === transpose(r1) @test inv(r1)*r1 ≈ I @test r1*inv(r1) ≈ I @test r1/r1 ≈ I From 6811364cbc6341e44e41c29136e183759f6086f9 Mon Sep 17 00:00:00 2001 From: Yuto Horikawa Date: Sat, 11 Nov 2023 00:53:53 +0900 Subject: [PATCH 6/6] Fix type instability --- src/angleaxis_types.jl | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/angleaxis_types.jl b/src/angleaxis_types.jl index 2dd873ce..70f34cff 100644 --- a/src/angleaxis_types.jl +++ b/src/angleaxis_types.jl @@ -175,7 +175,7 @@ end @inline function (::Type{Q})(rv::RotationVec) where Q <: QuatRotation theta = rotation_angle(rv) if theta < sqrt(eps(typeof(theta))) - ϕ = theta / (2π) + ϕ = theta / π / 2 sc = sinc(ϕ) / 2 # this form gracefully handles theta = 0 qx, qy, qz = sc * rv.sx, sc * rv.sy, sc * rv.sz return Q(cos(theta / 2), qx, qy, qz, false)