Use sinc to fix RotationVec near zero (#272)

* Use `sinc` to fix RotationVec near zero By converting straight from RotationVec->QuatRotation, and using an implementation that exploits `sinc` to handle θ ≈ 0, we get continuous & differentiable behavior. * Use new implementation only for small theta * Relax adjoint test to approxeq * Fix test with `@test_broken` * Update tests for `inv`, `adjoint`, and `transpose` * Fix type instability --------- Co-authored-by: Yuto Horikawa <[email protected]>
JuliaGeometry · Nov 12, 2023 · bbd5980 · bbd5980
1 parent ca676d3
commit bbd5980
Show file tree

Hide file tree

Showing 3 changed files with 45 additions and 16 deletions.
diff --git a/src/angleaxis_types.jl b/src/angleaxis_types.jl
@@ -172,24 +172,24 @@ 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))
-end
-
-(::Type{RV})(q::QuatRotation) where {RV <: RotationVec} = RV(AngleAxis(q))
-
-function Base.:*(rv::RotationVec{T1}, v::StaticVector{3, T2}) where {T1,T2}
+@inline function (::Type{Q})(rv::RotationVec) where Q <: QuatRotation
     theta = rotation_angle(rv)
-    if (theta > eps(T1)) # use eps here because we have the 1st order series expansion defined
-        return AngleAxis(rv) * v
+    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
-        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])
+        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))
+
+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
 @inline Base.:*(rv::RotationVec, r::MRP) = QuatRotation(rv) * r
@@ -203,7 +203,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
@@ -180,6 +180,20 @@ 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)
+                # 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]]
+            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
@@ -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
@@ -334,9 +333,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
     #########################################################################