Merge branch 'orientations' into 'main'

Orientations readout and interpolation

See merge request !32

Makefile

@@ -10,8 +10,11 @@ test:
 clean:
 	rm -rf docs/build/
 
+docwatch:
+	while true; do fswatch -1 docs src && make doc; done
+
 preview:
 	julia -e 'using LiveServer; serve(dir="docs/build")'
 
 
-.PHONY: build doc test clean preview
+.PHONY: build doc docwatch test clean preview

Project.toml

@@ -19,7 +19,7 @@ UnROOT = "3cd96dde-e98d-4713-81e9-a4a1b0235ce9"
 [compat]
 DocStringExtensions = "0.8, 0.9"
 HDF5 = "^0.16.15, ^0.17"
-KM3NeTTestData = "^0.4.12"
+KM3NeTTestData = "^0.4.14"
 StaticArrays = "1"
 UnROOT = "^0.10.26"
 julia = "1"

docs/Project.toml

@@ -10,3 +10,4 @@ PGFPlotsX = "8314cec4-20b6-5062-9cdb-752b83310925"
 [compat]
 Documenter = "1"
 FHist = "^0.11"
+KM3NeTTestData = "^0.4.14"

docs/src/api.md

@@ -93,8 +93,11 @@ read(filename::AbstractString, T::Type{AcousticsTriggerParameter})
 calibrate
 calibratetime
 combine
+Orientations
+Compass
 floordist
 slew
+slerp
 ```
 
 ## Physics

docs/src/manual/calibration.md

 # Calibration
 
-It's implemented but not documented here yet. Check out the docs `calibrate`
-function's docstring!
+In general, data related to detector calibration are either stored in the
+database or in the [Calibration
+Archive](https://git.km3net.de/auxiliary_data/calibration).
 
+## Hits
+
+Hit calibration is implemented but not documented here yet. Check out the docs for [`calibrate()`](@ref).
+
+## Module Orientations
+
+Module orientation data is stored in ROOT files which are generated by the Jpp
+framework. The corresponding ROOT files, which are the output of the [dynamic
+calibration
+procedure](https://common.pages.km3net.de/jpp/Position_calibration.PDF), are
+stored in the [Calibration
+Archive](https://git.km3net.de/auxiliary_data/calibration) under
+`orientations/`.
+
+`KM3io.jl` extends the `Base.read` function with a method which reads the whole
+orientations file at once in an object of type [`Orientations`](@ref). This
+object can be called to calculate the orientation of a module (as a quaternion)
+for a given time, as long as the time is within the time range of the
+orientation data. The quaternions from the orientation data are interpolated for
+the given time using [`slerp()`](@ref).
+
+The following example shows how to read the orientation data and obtain the
+orientation quaternion for a module at a given time.
+
+```@example 1
+using KM3io, KM3NeTTestData
+
+o = read(datapath("calib", "KM3NeT_00000133_D_1.0.0_00017397_00017496_1.orientations.root"), Orientations)
+
+module_id = 817589211
+
+q = o(module_id,  1693408347)
+```
+
+The quaternions can be converted to [`Compass`](@ref) object which has the
+fields `.yaw`, `.pitch` and `.roll`:
+
+```@example 1
+compass = Compass(q)
+```

src/KM3io.jl

@@ -59,6 +59,7 @@ include("hardware.jl")
 include("root/online.jl")
 include("root/offline.jl")
 include("root/root.jl")
+include("root/calibration.jl")
 include("hdf5/hdf5.jl")
 include("daq.jl")
 include("acoustics.jl")

src/exports.jl

@@ -84,6 +84,8 @@ calibratetime,
 combine,
 floordist,
 slew,
+Orientations,
+Compass,
 
 # Reconstruction
 RecStageRange,
@@ -127,6 +129,7 @@ distance,
 phi,
 theta,
 zenith,
+slerp,
 
 # Real-time
 @ip_str,

src/root/calibration.jl

+"""
+
+A data structure to hold orientations data. This struct should be instantiated by
+`Base.read(filename, Orientations)`.
+
+"""
+struct Orientations
+    module_ids::Set{Int}
+    times::Dict{Int, Vector{Float64}}
+    quaternions::Dict{Int, Vector{Quaternion}}
+end
+function Base.show(io::IO, o::Orientations)
+    min_t = Inf
+    max_t = -Inf
+    for times in values(o.times)
+        _min_t, _max_t = extrema(times)
+        if _min_t < min_t
+            min_t = _min_t
+        end
+        if _max_t > max_t
+            max_t = _max_t
+        end
+    end
+    t₁ = unix2datetime(min_t)
+    t₂ = unix2datetime(max_t)
+    print(io, "Orientations of $(length(o.module_ids)) modules ($(t₁) to $(t₂))")
+end
+function (o::Orientations)(module_id::Integer, time::Real)
+    times = o.times[module_id]
+    (time < first(times) || time > last(times)) && error("The requested time is outside of the range of the orientations data.")
+
+    idx2 = searchsortedfirst(times, time)
+    q2 = o.quaternions[module_id][idx2]
+    (idx2 >= length(times) || idx2 == 1) && return q2
+    idx1 = idx2 - 1
+    q1 = o.quaternions[module_id][idx1]
+
+    t1 = times[idx1]
+    t2 = times[idx2]
+    Δt = t2 - t1
+
+    Δt == 0.0 && return q1
+
+    t = (time - t1) / Δt
+
+    slerp(q1, q2, t)
+end
+(o::Orientations)(module_id::Integer) = (t=o.times[module_id], q=o.quaternions[module_id])
+
+
+function Base.read(filename::AbstractString, T::Type{Orientations})
+    f = UnROOT.ROOTFile(filename)
+    module_ids = Set{Int}()
+    quaternions = Dict{Int, Vector{Quaternion}}()
+    times = Dict{Int, Vector{Float64}}()
+    for (module_id, t, a, b, c, d) in zip([UnROOT.LazyBranch(f, "ORIENTATION/ORIENTATION/$(b)") for b in ["id", "t", "JCOMPASS::JQuaternion/a", "JCOMPASS::JQuaternion/b", "JCOMPASS::JQuaternion/c", "JCOMPASS::JQuaternion/d"]]...)
+        if !(module_id ∈ module_ids)
+             push!(module_ids, module_id)
+            quaternions[module_id] = Quaternion[]
+            times[module_id] = Float64[]
+        end
+        push!(quaternions[module_id], Quaternion(a, b, c, d))
+        push!(times[module_id], t)
+    end
+    T(module_ids, times, quaternions)
+end
+
+"""
+A compass with yaw, pitch and roll.
+"""
+struct Compass
+    yaw::Float64
+    pitch::Float64
+    roll::Float64
+end
+
+"""
+
+Initialises a [`Compass`](@ref) from a [`Quaternion`](@ref).
+
+"""
+function Compass(q::Quaternion)
+    yaw = -atan(2.0 * (q.q0 * q.qz + q.qx * q.qy), 1.0 - 2.0 * (q.qy * q.qy + q.qz * q.qz))
+    sp = 2.0 * (q.q0 * q.qy - q.qz * q.qx)
+
+    if (sp >= +1.0)
+        pitch = asin(+1.0)
+    elseif (sp <= -1.0)
+        pitch = asin(-1.0)
+    else
+        pitch = asin(sp)
+    end
+
+    roll = -atan(2.0 * (q.q0 * q.qx + q.qy * q.qz), 1.0 - 2.0 * (q.qx * q.qx + q.qy * q.qy))
+
+    Compass(yaw, pitch, roll)
+end

src/tools/math.jl

@@ -12,3 +12,53 @@ Calculates the disance between two points.
 
 """
 distance(a::Position, b::Position) = norm(a - b)
+
+
+"""
+
+Interpolate between two vectors (e.g. quaternions) using the slerp method. `t`
+should be between 0 and 1. 0 will produce `q₁` and `1` `q₂`.
+
+The input vectors `q₁` and `q₂` will be normalised unless `normalized` is
+`false`. It is not done by default to shave off a few dozens of nanoseconds.
+Make sure to set `normalized=false` if the input vectors are not unit vectors.
+
+"""
+function slerp(q₁, q₂, t::Real; dot_threshold=0.9995, normalized=true)
+    if !normalized
+        q₁ = normalize(q₁)
+        q₂ = normalize(q₂)
+    end
+
+    dot = q₁⋅q₂
+
+    if (dot < 0.0)
+	    q₂ *= -1
+	    dot = -dot
+    end
+
+    s₁ = t
+    s₀ = 1.0 - t
+
+    if dot <= dot_threshold
+        θ₀ = acos(dot)
+        θ₁ = θ₀ * t
+
+        s₁ = sin(θ₁) / sin(θ₀)
+        s₀ = cos(θ₁) - dot * s₁
+    end
+
+    normalize((s₀ * q₁)  +  (s₁ * q₂))
+end
+
+# Another implementation which yields slightly different results.
+# Further reading: http://number-none.com/product/Understanding%20Slerp,%20Then%20Not%20Using%20It/
+#
+# function slerp(q₁, q₂, t::Real; dot_threshold=0.9995)
+#     dot = acos(q₁⋅q₂)
+#     dot > dot_threshold && return normalize(q₁ + t*(q₂ - q₁))
+#     dot = clamp(dot, -1, 1)
+#     θ = acos(dot) * t
+#     q = normalize(q₂ - q₁*dot)
+#     q₁*cos(θ) + q*sin(θ)
+# end

test/calibration.jl

@@ -22,3 +22,31 @@ end
     det = Detector(datapath("detx", "km3net_offline.detx"))
     @test 9.61317647058823 ≈ floordist(det)
 end
+
+
+@testset "orientations" begin
+    o = read(datapath("calib", "KM3NeT_00000133_D_1.0.0_00017397_00017496_1.orientations.root"), Orientations)
+    @show o
+    module_id = 817589211
+    min_t, max_t = extrema(o.times[module_id])
+    Δt = max_t - min_t
+    @test [0.8260205110995139, 0.003912907129683348, -0.004395551387888641, -0.5636093359133512] == o(module_id, min_t)
+    @test [0.8289446524907407, 0.004590185819553083, -0.0007479055911552097, -0.5593113032456739] == o(module_id, max_t)
+    @test [0.8297266567631056, 0.002991865243189534, -0.004798371006076004, -0.5581412898494195] == o(module_id, min_t + Δt/2)
+    @test [0.8305219131347711, 0.003947997911424212, -0.0042572917986734805, -0.5569556899628482] == o(module_id, min_t + Δt/3)
+
+    qdata = o(module_id)
+    @test 5 == length(qdata.t)
+    @test 5 == length(qdata.q)
+    @test 1.693407821152e9 == qdata.t[1]
+    @test [0.8260205110995139, 0.003912907129683348, -0.004395551387888641, -0.5636093359133512] == qdata.q[1]
+end
+
+@testset "Compass" begin
+    q = Quaternion(0.8260205110995139, 0.003912907129683348, -0.004395551387888641, -0.5636093359133512)
+    c = Compass(q)
+    @test c.yaw == 1.1975374212207646
+    @test c.pitch == -0.0028509330922497
+    @test c.roll == -0.011419325278029469
+
+end

test/math.jl

+import KM3io: slerp
+using Test
+
+@testset "slerp()" begin
+    q1 = [1.0, 0.0]
+    q2 = [0.0, 1.0]
+    @test q1 ≈ slerp(q1, q2, 0)
+    @test q2 ≈ slerp(q1, q2, 1)
+    @test [0.9510565162951538, 0.30901699437494745] ≈ slerp(q1, q2, 0.2)
+    @test [0.70710678, 0.70710678] ≈ slerp(q1, q2, 0.5)
+    @test [0.45399049973954686, 0.8910065241883678] ≈ slerp(q1, q2, 0.7)
+
+    # should normalise internally
+    q1 = [0.4, 0.0]
+    q2 = [0.0, 0.9]
+    @test [1.0, 0.0] ≈ slerp(q1, q2, 0; normalized=false)
+    @test [0.0, 1.0] ≈ slerp(q1, q2, 1; normalized=false)
+    @test [0.9510565162951538, 0.30901699437494745] ≈ slerp(q1, q2, 0.2; normalized=false)
+    @test [0.70710678, 0.70710678] ≈ slerp(q1, q2, 0.5; normalized=false)
+    @test [0.45399049973954686, 0.8910065241883678] ≈ slerp(q1, q2, 0.7; normalized=false)
+
+    q1 = [1.0, 0.0, 0.0]
+    q2 = [0.0, 0.0, 1.0]
+    @test q1 ≈ slerp(q1, q2, 0)
+    @test q2 ≈ slerp(q1, q2, 1)
+    @test [0.9510565162951538, 0.0, 0.30901699437494745] ≈ slerp(q1, q2, 0.2)
+    @test [0.70710678, 0.0, 0.70710678] ≈ slerp(q1, q2, 0.5)
+    @test [0.45399049973954686, 0.0, 0.8910065241883678] ≈ slerp(q1, q2, 0.7)
+end

test/runtests.jl

@@ -8,4 +8,5 @@ include("hardware.jl")
 include("acoustics.jl")
 include("calibration.jl")
 include("physics.jl")
+include("math.jl")
 include("controlhost.jl")