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.

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

rename UnitQuaternion to QuatRotation #201

Merged
merged 7 commits into from
Nov 17, 2021
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
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
24 changes: 12 additions & 12 deletions README.md
Original file line number Diff line number Diff line change
Expand Up @@ -56,15 +56,15 @@ rotation_angle(r::Rotation)
#### Initialization
All rotation types support `one(R)` to construct the identity rotation for the desired parameterization. A random rotation, uniformly sampled over the space of rotations, can be sampled using `rand(R)`. For example:
```julia
r = one(UnitQuaternion) # equivalent to Quat(1.0, 0.0, 0.0, 0.0)
q = rand(UnitQuaternion)
r = one(QuatRotation) # equivalent to QuatRotation(1.0, 0.0, 0.0, 0.0)
q = rand(QuatRotation)
p = rand(MRP{Float32})
```

#### Conversion
All rotatations can be converted to another parameterization by simply calling the constructor for the desired parameterization. For example:
```julia
q = rand(UnitQuaternion)
q = rand(QuatRotation)
aa = AngleAxis(q)
r = RotMatrix(aa)
```
Expand All @@ -83,12 +83,12 @@ r = rand(RotMatrix{3}) # uses Float64 by default
# create a point
p = SVector(1.0, 2.0, 3.0) # from StaticArrays.jl, but could use any AbstractVector...

# convert to a quaternion (Quat) and rotate the point
q = UnitQuaternion(r)
# convert to a quaternion (QuatRotation) and rotate the point
q = QuatRotation(r)
p_rotated = q * p

# Compose rotations
q2 = rand(UnitQuaternion)
q2 = rand(QuatRotation)
q3 = q * q2

# Take the inverse (equivalent to transpose)
Expand Down Expand Up @@ -145,14 +145,14 @@ j2 = Rotations.jacobian(q, p) # How does the rotated point q*p change w.r.t. the
renormalized by the constructor to be a unit vector, so that `theta` always
represents the rotation angle in radians.

3. **Quaternions** `UnitQuaternion{T}`
3. **Quaternions** `QuatRotation{T}`

A 3D rotation parameterized by a unit quaternion. Note that the constructor
will renormalize the quaternion to be a unit quaternion, and that although
they follow the same multiplicative *algebra* as quaternions, it is better
to think of `UnitQuaternion` as a 3×3 matrix rather than as a quaternion *number*.
to think of `QuatRotation` as a 3×3 matrix rather than as a quaternion *number*.

Previously `Quat`.
Previously `Quat`, `UnitQuaternion`.

4. **Rotation Vector** `RotationVec{T}`

Expand Down Expand Up @@ -219,7 +219,7 @@ differential unit quaternion. This mapping goes singular at 360°.
differential unit quaternion. This mapping also goes singular at 180° but is
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:
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 `QuatRotation` (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::Rotation, err::RotationError) # "adds" the error to `R` by converting back a `UnitQuaterion` and composing with `R`
Expand All @@ -232,7 +232,7 @@ R1 ⊕ err # alias for `add_error(R1, err)`

For a practical application of these ideas, see the quatenrion-multiplicative Extended Kalman Filter (MEKF). [This article](https://ntrs.nasa.gov/archive/nasa/casi.ntrs.nasa.gov/20040037784.pdf) provides a good description.

When taking derivatives with respect to quaternions we need to account both for these mappings and the fact that local perturbations to a rotation act through composition instead of addition, as they do in vector space (e.g. `q * dq` vs `x + dx`). The following methods are useful for computing these Jacobians for `UnitQuaternion`, `RodriguesParam` or `MRP`
When taking derivatives with respect to quaternions we need to account both for these mappings and the fact that local perturbations to a rotation act through composition instead of addition, as they do in vector space (e.g. `q * dq` vs `x + dx`). The following methods are useful for computing these Jacobians for `QuatRotation`, `RodriguesParam` or `MRP`
* `∇rotate(q,r)`: Jacobian of the `q*r` with respect to the rotation
* `∇composition1(q2,q1)`: Jacobian of `q2*q1` with respect to q1
* `∇composition2(q2,q1,b)`: Jacobian of `q2*q1` with respect to q2
Expand All @@ -251,7 +251,7 @@ All parameterizations can be converted to and from (mutable or immutable)
using StaticArrays, Rotations

# export
q = UnitQuaternion(1.0,0,0,0)
q = QuatRotation(1.0,0,0,0)
matrix_mutable = Array(q)
matrix_immutable = SMatrix{3,3}(q)

Expand Down
12 changes: 6 additions & 6 deletions docs/src/3d_quaternion.md
Original file line number Diff line number Diff line change
Expand Up @@ -4,9 +4,9 @@
using Rotations
```

## `UnitQuaternion`
## `QuatRotation`
A 3D rotation parameterized by a unit [quaternion](https://en.wikipedia.org/wiki/Quaternion) ([versor](https://en.wikipedia.org/wiki/Versor)).
Note that the constructor will renormalize the quaternion to be a unit quaternion, and that although they follow the same multiplicative *algebra* as quaternions, it is better to think of `UnitQuaternion` as a ``3 \times 3`` matrix rather than as a quaternion *number*.
Note that the constructor will renormalize the quaternion to be a unit quaternion, and that although they follow the same multiplicative *algebra* as quaternions, it is better to think of `QuatRotation` as a ``3 \times 3`` matrix rather than as a quaternion *number*.

```math
\begin{aligned}
Expand All @@ -17,11 +17,11 @@ Note that the constructor will renormalize the quaternion to be a unit quaternio

**example**
```@repl quaternions
one(UnitQuaternion) # null rotation
one(QuatRotation) # null rotation
α, β, γ = 1.2, -0.8, 0.1;
RotX(α) ≈ UnitQuaternion(cos(α/2),sin(α/2),0,0) # These matrices are equal
RotY(β) ≈ UnitQuaternion(cos(β/2),0,sin(β/2),0) # These matrices are equal
RotZ(γ) ≈ UnitQuaternion(cos(γ/2),0,0,sin(γ/2)) # These matrices are equal
RotX(α) ≈ QuatRotation(cos(α/2),sin(α/2),0,0) # These matrices are equal
RotY(β) ≈ QuatRotation(cos(β/2),0,sin(β/2),0) # These matrices are equal
RotZ(γ) ≈ QuatRotation(cos(γ/2),0,0,sin(γ/2)) # These matrices are equal
```

## `RodriguesParam`
Expand Down
6 changes: 3 additions & 3 deletions docs/src/functions.md
Original file line number Diff line number Diff line change
Expand Up @@ -35,7 +35,7 @@ Rotations.params(RotYZY(R)) # Proper Euler angles, (y,z,y)
Rotations.params(RotXYZ(R)) # Tait–Bryan angles, (y,z,y)
Rotations.params(AngleAxis(R)) # Rotation around an axis (theta, axis_x, axis_y, axis_z)
Rotations.params(RotationVec(R)) # Rotation vector (v_x, v_y, v_z)
Rotations.params(UnitQuaternion(R)) # Quaternion (w, x, y, z)
Rotations.params(QuatRotation(R)) # Quaternion (w, x, y, z)
Rotations.params(RodriguesParam(R)) # Rodrigues Parameters (x, y, z)
Rotations.params(MRP(R)) # Modified Rodrigues Parameters (x, y, z)
```
Expand Down Expand Up @@ -93,7 +93,7 @@ The following types have an algebraic structure that is homomorphic to ``SO(3)``
* `RotXYZ` (and other Euler angles)
* `AngleAxis`
* `RotationVec`
* `UnitQuaternion`
* `QuatRotation`
* `RodriguesParam`
* `MRP`

Expand All @@ -118,4 +118,4 @@ Note that:
* `rand(RotX)` is same as `RotX(2π*rand())`.
* `rand(RotXY)` is same as `RotXY(2π*rand(), 2π*rand())`.
* `rand(RotXYZ)` is **not** same as `RotXYZ(2π*rand(), 2π*rand(), 2π*rand())`.
* But `rand(RotXYZ)` is same as `RotXYZ(rand(UnitQuaternion))`.
* But `rand(RotXYZ)` is same as `RotXYZ(rand(QuatRotation))`.
2 changes: 1 addition & 1 deletion docs/src/index.md
Original file line number Diff line number Diff line change
Expand Up @@ -27,7 +27,7 @@ R_euler = RotXYZ(1,2,3)
rotation_angle(R_euler), rotation_axis(R_euler)

# Convert the rotation to unit quaternion
R_quat = UnitQuaternion(R_euler)
R_quat = QuatRotation(R_euler)

# Get quaternion parameters of the rotation
Rotations.params(R_quat)
Expand Down
2 changes: 1 addition & 1 deletion docs/src/rotation_types.md
Original file line number Diff line number Diff line change
Expand Up @@ -43,7 +43,7 @@ For more information, see the sidebar page.
* Rotation around given axis and angle.
* `RotationVec`
* Rotation around given axis. The length of axis vector represents its angle.
* `UnitQuaternion`
* `QuatRotation`
* A 3D rotation parameterized by a unit quaternion.
* `MRP`
* A 3D rotation encoded by the stereographic projection of a unit quaternion.
2 changes: 1 addition & 1 deletion perf/runbenchmarks.jl
Original file line number Diff line number Diff line change
Expand Up @@ -9,7 +9,7 @@ const suite = BenchmarkGroup()
Random.seed!(1)

noneuler = suite["Non-Euler conversions"] = BenchmarkGroup()
rotationtypes = [RotMatrix3{T}, UnitQuaternion{T}, MRP{T}, AngleAxis{T}, RotationVec{T}]
rotationtypes = [RotMatrix3{T}, QuatRotation{T}, MRP{T}, AngleAxis{T}, RotationVec{T}]
for (from, to) in product(rotationtypes, rotationtypes)
if from != to
name = "$(string(from)) -> $(string(to))"
Expand Down
4 changes: 2 additions & 2 deletions src/Rotations.jl
Original file line number Diff line number Diff line change
Expand Up @@ -32,15 +32,15 @@ export
# Rotation types
Rotation, RotMatrix, RotMatrix2, RotMatrix3,
Angle2d,
UnitQuaternion,
QuatRotation,
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,
UnitQuaternion,

# Quaternion math ops
logm, expm, ⊖, ⊕,
Expand Down
52 changes: 26 additions & 26 deletions src/angleaxis_types.jl
Original file line number Diff line number Diff line change
Expand Up @@ -46,8 +46,8 @@ params(aa::AngleAxis) = SVector{4}(aa.theta, aa.axis_x, aa.axis_y, aa.axis_z)
end

# These functions are enough to satisfy the entire StaticArrays interface:
@inline (::Type{AA})(t::NTuple{9}) where {AA <: AngleAxis} = AA(UnitQuaternion(t)) # TODO: consider going directly from tuple (RotMatrix) to AngleAxis
@inline Base.getindex(aa::AngleAxis, i::Int) = UnitQuaternion(aa)[i]
@inline (::Type{AA})(t::NTuple{9}) where {AA <: AngleAxis} = AA(QuatRotation(t)) # TODO: consider going directly from tuple (RotMatrix) to AngleAxis
@inline Base.getindex(aa::AngleAxis, i::Int) = QuatRotation(aa)[i]

@inline function Base.Tuple(aa::AngleAxis{T}) where T
# Rodrigues' rotation formula.
Expand All @@ -72,12 +72,12 @@ end
c1xz + sy, c1yz - sx, one(T) - c1x2 - c1y2)
end

@inline function (::Type{Q})(aa::AngleAxis) where Q <: UnitQuaternion
@inline function (::Type{Q})(aa::AngleAxis) where Q <: QuatRotation
s, c = sincos(aa.theta / 2)
return Q(c, s * aa.axis_x, s * aa.axis_y, s * aa.axis_z, false)
end

@inline function (::Type{AA})(q::UnitQuaternion) where AA <: AngleAxis
@inline function (::Type{AA})(q::QuatRotation) where AA <: AngleAxis
w = q.q.s
x = q.q.v1
y = q.q.v2
Expand Down Expand Up @@ -118,13 +118,13 @@ function Base.:*(aa::AngleAxis, v::StaticVector)
v[3] * ct + w_cross_pt[3] * st + w[3] * m)
end

@inline Base.:*(aa::AngleAxis, r::Rotation) = UnitQuaternion(aa) * r
@inline Base.:*(aa::AngleAxis, r::RotMatrix) = UnitQuaternion(aa) * r
@inline Base.:*(aa::AngleAxis, r::MRP) = UnitQuaternion(aa) * r
@inline Base.:*(r::Rotation, aa::AngleAxis) = r * UnitQuaternion(aa)
@inline Base.:*(r::RotMatrix, aa::AngleAxis) = r * UnitQuaternion(aa)
@inline Base.:*(r::MRP, aa::AngleAxis) = r * UnitQuaternion(aa)
@inline Base.:*(aa1::AngleAxis, aa2::AngleAxis) = UnitQuaternion(aa1) * UnitQuaternion(aa2)
@inline Base.:*(aa::AngleAxis, r::Rotation) = QuatRotation(aa) * r
@inline Base.:*(aa::AngleAxis, r::RotMatrix) = QuatRotation(aa) * r
@inline Base.:*(aa::AngleAxis, r::MRP) = QuatRotation(aa) * r
@inline Base.:*(r::Rotation, aa::AngleAxis) = r * QuatRotation(aa)
@inline Base.:*(r::RotMatrix, aa::AngleAxis) = r * QuatRotation(aa)
@inline Base.:*(r::MRP, aa::AngleAxis) = r * QuatRotation(aa)
@inline Base.:*(aa1::AngleAxis, aa2::AngleAxis) = QuatRotation(aa1) * QuatRotation(aa2)

@inline Base.inv(aa::AngleAxis) = AngleAxis(-aa.theta, aa.axis_x, aa.axis_y, aa.axis_z)
@inline Base.:^(aa::AngleAxis, t::Real) = AngleAxis(aa.theta*t, aa.axis_x, aa.axis_y, aa.axis_z)
Expand Down Expand Up @@ -163,9 +163,9 @@ params(aa::RotationVec) = SVector{3}(aa.sx, aa.sy, aa.sz)
@inline RotationVec(x::X, y::Y, z::Z) where {X,Y,Z} = RotationVec{promote_type(promote_type(X, Y), Z)}(x, y, z)

# These functions are enough to satisfy the entire StaticArrays interface:
@inline (::Type{RV})(t::NTuple{9}) where {RV <: RotationVec} = RV(UnitQuaternion(t)) # TODO: go through AngleAxis once it's faster
@inline Base.getindex(aa::RotationVec, i::Int) = UnitQuaternion(aa)[i]
@inline Base.Tuple(rv::RotationVec) = Tuple(UnitQuaternion(rv))
@inline (::Type{RV})(t::NTuple{9}) where {RV <: RotationVec} = RV(QuatRotation(t)) # TODO: go through AngleAxis once it's faster
@inline Base.getindex(aa::RotationVec, i::Int) = QuatRotation(aa)[i]
@inline Base.Tuple(rv::RotationVec) = Tuple(QuatRotation(rv))

function (::Type{AA})(rv::RotationVec) where AA <: AngleAxis
# TODO: consider how to deal with derivative near theta = 0. There should be a first-order expansion here.
Expand All @@ -177,11 +177,11 @@ 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 <: UnitQuaternion
return UnitQuaternion(AngleAxis(rv))
function (::Type{Q})(rv::RotationVec) where Q <: QuatRotation
return QuatRotation(AngleAxis(rv))
end

(::Type{RV})(q::UnitQuaternion) where {RV <: RotationVec} = RV(AngleAxis(q))
(::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)
Expand All @@ -195,15 +195,15 @@ function Base.:*(rv::RotationVec{T1}, v::StaticVector{3, T2}) where {T1,T2}
end
end

@inline Base.:*(rv::RotationVec, r::Rotation) = UnitQuaternion(rv) * r
@inline Base.:*(rv::RotationVec, r::RotMatrix) = UnitQuaternion(rv) * r
@inline Base.:*(rv::RotationVec, r::MRP) = UnitQuaternion(rv) * r
@inline Base.:*(rv::RotationVec, r::AngleAxis) = UnitQuaternion(rv) * r
@inline Base.:*(r::Rotation, rv::RotationVec) = r * UnitQuaternion(rv)
@inline Base.:*(r::RotMatrix, rv::RotationVec) = r * UnitQuaternion(rv)
@inline Base.:*(r::MRP, rv::RotationVec) = r * UnitQuaternion(rv)
@inline Base.:*(r::AngleAxis, rv::RotationVec) = r * UnitQuaternion(rv)
@inline Base.:*(rv1::RotationVec, rv2::RotationVec) = UnitQuaternion(rv1) * UnitQuaternion(rv2)
@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
@inline Base.:*(rv::RotationVec, r::AngleAxis) = QuatRotation(rv) * r
@inline Base.:*(r::Rotation, rv::RotationVec) = r * QuatRotation(rv)
@inline Base.:*(r::RotMatrix, rv::RotationVec) = r * QuatRotation(rv)
@inline Base.:*(r::MRP, rv::RotationVec) = r * QuatRotation(rv)
@inline Base.:*(r::AngleAxis, rv::RotationVec) = r * QuatRotation(rv)
@inline Base.:*(rv1::RotationVec, rv2::RotationVec) = QuatRotation(rv1) * QuatRotation(rv2)

@inline Base.inv(rv::RotationVec) = RotationVec(-rv.sx, -rv.sy, -rv.sz)
@inline Base.:^(rv::RotationVec, t::Real) = RotationVec(rv.sx*t, rv.sy*t, rv.sz*t)
Expand Down
2 changes: 1 addition & 1 deletion src/core_types.jl
Original file line number Diff line number Diff line change
Expand Up @@ -58,7 +58,7 @@ function Random.rand(rng::AbstractRNG, ::Random.SamplerType{R}) where R <: Rotat
T = Float64
end

q = UnitQuaternion(randn(rng, T), randn(rng, T), randn(rng, T), randn(rng, T))
q = QuatRotation(randn(rng, T), randn(rng, T), randn(rng, T), randn(rng, T))
return R(q)
end

Expand Down
11 changes: 2 additions & 9 deletions src/deprecated.jl
Original file line number Diff line number Diff line change
@@ -1,9 +1,2 @@

# Deprecate RodriguesVec => RotationVec
Base.@deprecate_binding RodriguesVec RotationVec true

# Deprecate Quat => UnitQuaternion
Base.@deprecate_binding Quat UnitQuaternion true

# Deprecate SPQuat => MRP
Base.@deprecate_binding SPQuat MRP true
# Deprecate UnitQuaternion => QuatRotation
Base.@deprecate_binding UnitQuaternion QuatRotation true
16 changes: 8 additions & 8 deletions src/derivatives.jl
Original file line number Diff line number Diff line change
Expand Up @@ -30,7 +30,7 @@ to the output parameterization, centered at the value of R.
jacobian(R::rotation_type, X::AbstractVector)
Returns the jacobian for rotating the vector X by R.
"""
function jacobian(::Type{RotMatrix}, q::UnitQuaternion)
function jacobian(::Type{RotMatrix}, q::QuatRotation)
w = q.q.s
x = q.q.v1
y = q.q.v2
Expand Down Expand Up @@ -82,10 +82,10 @@ end
function jacobian(::Type{RotMatrix}, X::MRP)

# get the derivatives of the quaternion w.r.t to the mrp
dQdX = jacobian(UnitQuaternion, X)
dQdX = jacobian(QuatRotation, X)

# get the derivatives of the rotation matrix w.r.t to the mrp
dRdQ = jacobian(RotMatrix, UnitQuaternion(X))
dRdQ = jacobian(RotMatrix, QuatRotation(X))

# and return
return dRdQ * dQdX
Expand All @@ -98,7 +98,7 @@ end
#
#######################################################

function jacobian(::Type{UnitQuaternion}, X::MRP)
function jacobian(::Type{QuatRotation}, X::MRP)

# differentiating
# q = Quaternion((1-alpha2) / (alpha2 + 1), 2*X.x / (alpha2 + 1), 2*X.y / (alpha2 + 1), 2*X.z / (alpha2 + 1), true)
Expand Down Expand Up @@ -137,7 +137,7 @@ end
#
# Jacobian converting from a Quaternion to an MRP
#
function jacobian(::Type{MRP}, q::UnitQuaternion{T}) where T
function jacobian(::Type{MRP}, q::QuatRotation{T}) where T
w = q.q.s
x = q.q.v1
y = q.q.v2
Expand Down Expand Up @@ -183,7 +183,7 @@ end
end

# TODO: should this be jacobian(:rotate, q, X) # or something?
function jacobian(q::UnitQuaternion, X::AbstractVector)
function jacobian(q::QuatRotation, X::AbstractVector)
w = q.q.s
x = q.q.v1
y = q.q.v2
Expand All @@ -209,8 +209,8 @@ function jacobian(q::UnitQuaternion, X::AbstractVector)
end

function jacobian(spq::MRP, X::AbstractVector)
dQ = jacobian(UnitQuaternion, spq)
q = UnitQuaternion(spq)
dQ = jacobian(QuatRotation, spq)
q = QuatRotation(spq)
return jacobian(q, X) * dQ
end

Expand Down
Loading