search

JuliaGeometry / Quaternions.jl

A Julia implementation of quaternions
https://juliageometry.github.io/Quaternions.jl
MIT License
116 stars 37 forks source link

Fix `qrotation(dcm::Matrix{T})` #73

Closed hyrodium closed 2 years ago

hyrodium commented 2 years ago

This PR fixes #67. I have resolved the second and third issue in the following list.

The first one will be fixed by #35.

hyrodium commented 2 years ago

The scatter plot is now a ball.

https://user-images.githubusercontent.com/7488140/155521758-5deb916f-6b19-4e87-8a07-707d6ea3b60a.mp4

codecov[bot] commented 2 years ago

Codecov Report

Merging #73 (662fc02) into master (7030d39) will increase coverage by 0.02%. The diff coverage is 100.00%.

Impacted file tree graph

@@            Coverage Diff             @@
##           master      #73      +/-   ##
==========================================
+ Coverage   97.12%   97.15%   +0.02%     
==========================================
  Files           3        3              
  Lines         383      386       +3     
==========================================
+ Hits          372      375       +3     
  Misses         11       11
Impacted Files Coverage Δ
src/Quaternion.jl 98.52% <100.00%> (+0.02%) :arrow_up:

Continue to review full report at Codecov.

Legend - Click here to learn more Δ = absolute <relative> (impact), ø = not affected, ? = missing data Powered by Codecov. Last update 7030d39...662fc02. Read the comment docs.

hyrodium commented 2 years ago

@sethaxen Can I add tests for this? I thought this may make conflicts with #72.

sethaxen commented 2 years ago

@sethaxen Can I add tests for this? I thought this may make conflicts with #72.

I think if you keep them in a well-marked testset, then it shouldn't be a problem to merge it in after #72.

sethaxen commented 2 years ago

The returned quaternion might have rotation angle larger than π.

Can you elaborate on what the problem here is and how this PR fixes it?

hyrodium commented 2 years ago

What the problem is

Before this PR

julia> using Quaternions, Statistics

julia> quaternions = [nquatrand() for _ in 1:100000];  # Generate random unit quaternions

julia> angles = angle.(quaternions);  # Get the rotation angles of the quaternions

julia> minimum(angles), maximum(angles)  # The range is (0, 2π)
(0.07113701865366824, 6.226790376165068)

julia> angles = angle.(qrotation.(rotationmatrix.(quaternions)));

julia> minimum(angles), maximum(angles)  # The range is (0, ???)
(0.056394931014518195, 4.706452526350558)

julia> mean(qrotation.(rotationmatrix.(quaternions)))  # mean of quaternions have non-zero imaginary part.
QuaternionF64(0.1923394580768536, 0.192625369273743, 0.19695935120814975, 0.19429851601128714, false)

After this PR

julia> using Quaternions, Statistics

julia> quaternions = [nquatrand() for _ in 1:100000];

julia> angles = angle.(quaternions);

julia> minimum(angles), maximum(angles)  # (0, 2π)
(0.040367290413436376, 6.228361297695127)

julia> angles = angle.(qrotation.(rotationmatrix.(quaternions)));

julia> minimum(angles), maximum(angles)  # (0, π)
(0.040367290413436376, 3.1415827714115947)

julia> mean(qrotation.(rotationmatrix.(quaternions)))  # mean of quaternions have approximately zero imaginary part.
QuaternionF64(0.42571485159149236, -0.0028643946594000005, -0.0013893812865142013, -0.001049930265626973, false)

How this PR fixes it

This PR enforces the output of qrotation to have a non-negative real part.

About the scatter plot

The set of unit quaternions can be regarded as 3-dimensional sphere, so it can be mapped to ℝ³ with stereographic projection. The scatter plot shows the coordinates in ℝ³.

See https://github.com/JuliaGeometry/Rotations.jl/pull/97 or https://juliageometry.github.io/Rotations.jl/dev/3d_quaternion/#MRP-(Modified-Rodrigues-Parameters) for more information.

hyrodium commented 2 years ago

@sethaxen Could you review this PR again? Sorry if my explanation is poor.

sethaxen commented 2 years ago

@sethaxen Could you review this PR again? Sorry if my explanation is poor.

Sorry for the delay in replying. It's still not clear to me what the problem with the current implementation is? Is it incorrect? Numerically unstable? Unexpected? And how does the new implementation resolve these issues?

hyrodium commented 2 years ago

The mapping from rotation matrix to quaternion is not unique because the set of unit quaternions (≃ S³ ≃ SU(2)) is a double cover of SO(3). I think the most natural range of the mapping should be {q=w+ix+jy+kz | w²+x²+y²+z²=1, w≥0}. Note that I don't much care about the boundary {q=w+ix+jy+kz | w²+x²+y²+z²=1, w=0}. The following are why I choose the set {q=w+ix+jy+kz | w²+x²+y²+z²=1, w≥0}.

Is it incorrect?

It depends on how we define "correct". If we do not distinguish antipodal points q and -q, then the current master implementation is correct. But, I feel we need to care about the mapping more.

Numerically unstable?

No.

Unexpected?

Yes. Because of this if statement, the range of the mapping SO(3) → S³ seems weird. This can be visualized via stereographic projection, and the result is the scatter plot.

We may define the qrotation without the if statement like the following instead of the current implementation.

function qrotation(dcm::AbstractMatrix{T}) where {T<:Real}
    # See https://arxiv.org/pdf/math/0701759.pdf
    a2 = 1 + dcm[1,1] + dcm[2,2] + dcm[3,3]
    b2 = 1 + dcm[1,1] - dcm[2,2] - dcm[3,3]
    c2 = 1 - dcm[1,1] + dcm[2,2] - dcm[3,3]
    d2 = 1 - dcm[1,1] - dcm[2,2] + dcm[3,3]

    # Same as a2 >= max(b2, c2, d2) branch
    a = sqrt(a2)/2
    return Quaternion(a, (dcm[3,2]-dcm[2,3])/4a, (dcm[1,3]-dcm[3,1])/4a, (dcm[2,1]-dcm[1,2])/4a)
end

With this version of the qrotation, the output range will be {q=w+ix+jy+kz | w²+x²+y²+z²=1, w≥0}. But this is undesirable from the standpoint of calculation precision, especially if the dcm has errors. Currently, we don't have tests for precision, But Rotations.jl has the same function and its test. We can refer the test. (I haven't added it yet in this PR.)

And how does the new implementation resolve these issues?

As I said above, this PR enforces the output of qrotation to have a non-negative real part. i.e. the range of the function will be {q=w+ix+jy+kz | w²+x²+y²+z²=1, w≥0}.