How can I implement the TRIAD algorithm in Julia

General Usage
, ,
1

I try to calculate the rotation matrix that needs to applied on one reference frame to match a second reference frame. I have the following code:

# test calculation of the orientation
using LinearAlgebra, Rotations, StaticArrays, Test
"""
    rot3d(ax, ay, az, bx, by, bz)

Calculate the rotation matrix that needs to be applied on the reference frame (ax, ay, az) to match 
the reference frame (bx, by, bz).
All parameters must be 3-element vectors. Both refrence frames must be orthogonal,
all vectors must already be normalized.

Source: [TRIAD_Algorithm](http://en.wikipedia.org/wiki/User:Snietfeld/TRIAD_Algorithm)
"""
function rot3d(ax, ay, az, bx, by, bz)
    R_ai = hcat(ax, az, ay)
    R_bi = hcat(bx, bz, by)
    return R_bi * R_ai'
end

ax = [1, 0, 0] 
ay = [0, 1, 0] 
az = [0, 0, 1] 

x = [1, 0, 0] 
y = [0, 1, 0] 
z = [0, 0, 1]

rot1 = rot3d(ax, ay, az, x, y, z)
q1 = QuatRotation(rot1)
@test all(Rotations.params(q) .== SVector{4, Float64}([1.0 0 0 0]))

x = [-1, 0, 0] 
y = [0, 1, 0] 
z = [0, 0, 1]
rot2 = rot3d(ax, ay, az, x, y, z)
q2 = QuatRotation(rot2)

If I look at the result:

julia> q1
3×3 QuatRotation{Float64} with indices SOneTo(3)×SOneTo(3)(QuaternionF64(1.0, 0.0, 0.0, 0.0)):
 1.0  0.0  0.0
 0.0  1.0  0.0
 0.0  0.0  1.0

julia> q2
3×3 QuatRotation{Float64} with indices SOneTo(3)×SOneTo(3)(QuaternionF64(1.0, 0.0, 0.0, 0.0)):
 1.0  0.0  0.0
 0.0  1.0  0.0
 0.0  0.0  1.0

both rotations are the same, which cannot be correct. So there must be a mistake in my function rot3d.

How can I implement it correctly?

2

I think the issue is because your new frame is left handed instead of right handed, so there is no pure rotation from the right handed lab frame to [x y z]

The transformation you calculate is correct:

@assert [ax ay az] ≈ rot2 * [x y z]

I’ve never used the rotations package, so I don’t know what QuatRotation does when it’s given a left handed frame.

1 Like
3

How can I write a function that checks if a rotation frame is right handed?

4

You can check

x = [-1,0,0]
y = [0,1,0]
z = [0,0,1]

cross(x,y) ≈ z
#or
det([x y z]) ≈ 1
1 Like
5

Ok, I defined now

function is_right_handed(x, y, z)
    return det([x y z]) ≈ 1
end

and use an assert to check the input vectors.

But what is this function actually doing? I think it actually checks something else, because:

ax = [2, 0, 0] 
ay = [0, 1, 0] 
az = [0, 0, 1]
@assert is_right_handed(ax, ay, az)

fails.

6

At least in your own docs you wrote that the vectors all must already be normalised? your ax is not normalised.

Checking the determinant to be 1 (approx) for right, or -1 for left handed only works for unit frames (all vectors unit norm and orthogonal)

7

So the function should be called: is_right_handed_and_normalized()?

But does it really check that?

I just need a function that checks the required pre-conditions so that I can make my code more robust.

8

So wikipedia:

says: If the cross product points the same direction as your third direction is is right-handed, otherwise left-handed.
If you have unit vectors, that is even just checking for ±1.
The formula form the determinant comes from Cross product - Wikipedia

but you probably, for arbitrary vectors, you also just check the sign of \langle a \times b, c\rangle.
At least for an orthogonal (but not necessarily normal) frame, this would give you a negative number if you have a right-handed system or a positive for left-handed (probably more stable than checking against a 1).

9

Ah, do not ask a mathematician after 6pm to do all signs right. So if you do

using LinearAlgebra
is_right_handed(a,b,c) = dot(cross(a,b),c) > 0

That should just work fine?

For you example

julia> is_right_handed(ax,ay,az)
true
10

Like this:

function is_right_handed_normalized(x, y, z)
    if !norm(x) ≈ 1 || !norm(y) ≈ 1 || !norm(z) ≈ 1
        return false
    end
    return det([x y z]) ≈ 1
end

?
But does that already guarantee that the vectors are orthonormal?

11

Ah, so you. do not only want to check whether it is right-handed but even more whether it is orthonormal?

For that you have to check all norms and all pairwise inner products (all of them have to be 1)

edit: I read you post above in a way, that you already have a rotation frame – that is some rotation matrix; then you would know the ONB property already. So what do you actually have as input then?

12

Notice that Rotations.jl already provides a utility function rotation_between(u, v) to create a rotation from vector u to v. So you could compose these rotations to match the three axes of the frames.

13

What are inner products? Cross products or dot products or something else?

14

eh inner products are just dot products, sorry for not being clear (cf. Inner product space - Wikipedia).

15

But that cannot be correct. You said the inner products must be one. But they are not:

julia> x
3-element Vector{Int64}:
 0
 1
 0

julia> y
3-element Vector{Int64}:
 1
 0
 0

julia> (x ⋅ y)
0
16

Sorry it’s late so I should probably stop here. What I meant was: Inner products of vectors with themselves have to be 1, with others zero.

Or, if you write them column wise into a matrix you get X = [a\ b\ c] you can also write this as X^{\mathrm{T}}X=I, which is also nothing else than classifying a rotation matrix if then the determinant is +1 – otherwise there is a mirroring happening as well.

17

Oh, and if you do not mind the mirroring I mentioned in the last task your original task can also be solved by an SVD Orthogonal Procrustes problem - Wikipedia

18

Thanks, now I have this code:

function is_right_handed_orthonormal(x, y, z)
    if !(norm(x) ≈ 1) || !(norm(y) ≈ 1) || !(norm(z) ≈ 1)
        return false
    end
    if !((x ⋅ y) ≈ 0) || !((y ⋅ z) ≈ 0) || !((z ⋅ x) ≈ 0)
        return false
    end
    return det([x y z]) ≈ 1
end

"""
    rot3d(ax, ay, az, bx, by, bz)

Calculate the rotation matrix that needs to be applied on the reference frame (ax, ay, az) to match 
the reference frame (bx, by, bz).
All parameters must be 3-element vectors. Both refrence frames must be orthogonal,
all vectors must already be normalized.

Source: [TRIAD_Algorithm](http://en.wikipedia.org/wiki/User:Snietfeld/TRIAD_Algorithm)
"""
function rot3d(ax, ay, az, bx, by, bz)
    @assert is_right_handed_orthonormal(ax, ay, az)
    @assert is_right_handed_orthonormal(bx, by, bz)
    R_ai = hcat(ax, az, ay)
    R_bi = hcat(bx, bz, by)
    return R_bi * R_ai'
end

Does that look correct now?

19

Yeah that looks good. I think replacing the det with the dot/cross one might be a bit more stable, but maybe there is also not much difference in practice.

1 Like
20

Thank you so much!

1 Like