It can be simplified to:
function is_right_handed_orthonormal(x, y, z)
R = [x y z]
R*R' ≈ I && det(R) ≈ 1
end
which I think is better because x ≈ 0 returns false unless exactly 0
edit: for example your function returns false for a rotation matrix I just constructed using euler angles
This is probably not what you want: it is equivalent to (x ⋅ y) == 0 because you’re not providing any scale to compare against.
Since you checked that the norms are almost 1, your scale is one, so it would be sensible to check abs(x ⋅ y) < ε for some ε.
Update: @p_f’s suggestion is much nicer.
I have no idea what this sentence means?
I would tend to use StaticArrays.jl for 3d vectors like this, however.
I am using only SVectors in my code, but must this function also be modified to use StaticArrays?
function is_right_handed_orthonormal(x, y, z)
R = [x y z]
R*R' ≈ I && det(R) ≈ 1
end
No, it should be fine. [x y z] should construct an SMatrix from SVectors, and R*R' ≈ I && det(R) ≈ 1 should be non-allocating for SMatrix.
So for now this is the solution:
function is_right_handed_orthonormal(x, y, z)
R = [x y z]
R*R' ≈ I && det(R) ≈ 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 = [ax az ay]
R_bi = [bx bz by]
return R_bi * R_ai'
end
The other thing to be aware of is that you are using ≈ with the default tolerances (about 1e-8 for Float64), so it will allow through matrices that are not orthogonal to the full accuracy. On the other hand, if you’re only using this for a sanity check then it should be reasonable.
Note that this can be simplified to R_ai = [ax az ay], which is essentially equivalent to an hcat call.
I am modifying my code a lot every day, and sometimes I make mistakes with the signs. I just want to catch this before getting stuck with follow-up errors. I just want to increase the trust in my results.
Not that important, mainly meant to do is_right_handed(a,b,c) = dot(cross(a,b),c) > 0 instead since that might be more stable maybe?
I’m not sure what you mean by “more stable”. det([a b c]) essentially uses the same formula for a 3x3 SMatrix.
Hi @ufechner7 !
Just for future information, here is our implementation of TRIAD:
"""
triad(v_b::AbstractVector, v_a::AbstractVector, w_b::AbstractVector, w_a::AbstractVector; minimum_sine::Number) -> Bool, DCM
Compute the direction cosine matrix that rotates the coordinate system `a` into alignment
with the coordinate system `b` considering the measured vectors in `b` `v_b` and `w_b`, and
the reference vectors in `a` `v_a` and `w_a`.
The keyword `minimum_sine` contains the minimum value for the sine of the angle between the
vectors that allow the TRIAD computation. If those angles do not meet this criteria, nothing
will be computed.
# Returns
- `Bool`: `true` if TRIAD could be executed, `false` otherwise.
- `DCM`: DCM computed by TRIAD. If TRIAD could not be computed, the identity matrix is
returned.
"""
function triad(
v_b::AbstractVector{T1},
v_a::AbstractVector{T2},
w_b::AbstractVector{T3},
w_a::AbstractVector{T4};
minimum_sine::Number = 0.5
) where {T1<:Number, T2<:Number, T3<:Number, T4<:Number}
T = promote_type(T1, T2, T3, T4)
# Make sure `minimum_sine` is positive and larger than 0.001 (5°).
sine_threshold = max(T(minimum_sine), T(0.001))
# == Auxiliary Variables ===============================================================
v_a_norm = norm(v_a)
w_a_norm = norm(w_a)
v_b_norm = norm(v_b)
w_b_norm = norm(w_b)
# To avoid division by zero, we must make sure that the norm is at least 1e-6.
norm_threshold = 1e-6
if ((v_a_norm < norm_threshold) ||
(w_a_norm < norm_threshold) ||
(v_b_norm < norm_threshold) ||
(w_b_norm < norm_threshold))
return false, DCM{T}(I)
end
v_x_w_a = v_a × w_a
v_x_w_b = v_b × w_b
v_x_w_a_norm = norm(v_x_w_a)
v_x_w_b_norm = norm(v_x_w_b)
# == Input Verification ================================================================
# We must check the angle between the vectors in `a` and `b` reference systems because
# of the noise.
sin_wb_b = v_x_w_b_norm / (v_b_norm * w_b_norm)
sin_wb_b < sine_threshold && return false, DCM{T}(I)
sin_wb_a = v_x_w_a_norm / (v_a_norm * w_a_norm)
sin_wb_a < sine_threshold && return false, DCM{T}(I)
# == TRIAD =============================================================================
t₁_b = v_b / v_b_norm
t₁_a = v_a / v_a_norm
t₂_b = v_x_w_b / v_x_w_b_norm
t₂_a = v_x_w_a / v_x_w_a_norm
t₃_b = t₁_b × t₂_b
t₃_a = t₁_a × t₂_a
M_b = DCM(
t₁_b[1], t₂_b[1], t₃_b[1],
t₁_b[2], t₂_b[2], t₃_b[2],
t₁_b[3], t₂_b[3], t₃_b[3]
)'
M_a = DCM(
t₁_a[1], t₂_a[1], t₃_a[1],
t₁_a[2], t₂_a[2], t₃_a[2],
t₁_a[3], t₂_a[3], t₃_a[3]
)'
return true, M_b * M_a'
end
Thanks for sharing!
Yes,right, then it was just too late to doo math yesterday for me.
Interesting. I hadn’t run across that before. FYI, I’ve implemented another algorithm in my quaternion package here that has always worked well for me — apparently called Davenport’s method. I guess TRIAD looks like it would be a bit faster, though that’s not usually my concern. But I would also guess that Davenport’s method is probably more robust.