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

Use sinc to fix RotationVec near zero #272

Merged
merged 6 commits into from
Nov 12, 2023
Merged

Use sinc to fix RotationVec near zero #272

Show file tree
Hide file tree
Changes from 3 commits
Commits
Show all changes
6 commits
Select commit Hold shift + click to select a range
eec7cb9
Use `sinc` to fix RotationVec near zero
timholy Oct 12, 2023
74d6959
Use new implementation only for small theta
timholy Oct 12, 2023
4330c72
Relax adjoint test to approxeq
timholy Oct 12, 2023
8dd986d
Fix test with `@test_broken`
hyrodium Oct 20, 2023
288b279
Update tests for `inv`, `adjoint`, and `transpose`
hyrodium Oct 20, 2023
6811364
Fix type instability
hyrodium Nov 10, 2023
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
28 changes: 14 additions & 14 deletions src/angleaxis_types.jl
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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π)
hyrodium marked this conversation as resolved.
Show resolved Hide resolved
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
Expand All @@ -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)))
Expand Down
13 changes: 13 additions & 0 deletions test/derivative_tests.jl
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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]]
hyrodium marked this conversation as resolved.
Show resolved Hide resolved
@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
Expand Down
20 changes: 18 additions & 2 deletions test/rotation_tests.jl
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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)
hyrodium marked this conversation as resolved.
Show resolved Hide resolved
@test inv(r1)*r1 ≈ I
@test r1*inv(r1) ≈ I
@test r1/r1 ≈ I
Expand Down Expand Up @@ -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
#########################################################################
Expand Down
Loading