In [1]:
using LinearAlgebra
In [2]:
"""
    quadraticSolver(A, b, x; maxiter, tolerance)

Solves an equation system of the form:

    forall k in 1..n sum_{i=1}^n sum_{j=1}^n a_{kij} x_i x_j = b_k

Uses Newton's method; the optimization starts from `x`.
"""
function quadraticSolver(A, b, x; maxiter = 100, tolerance = 1.0e-8)
    n = length(A)
    fn(x) = [x' * A[k] * x for k in 1:n] - b
    dfn(x) = [sum(i -> (A[k][i,l] + A[k][l,i]) * x[i], 1:n) for k in 1:n, l in 1:n]
    for _ in 1:maxiter
        delta = inv(dfn(x)) * fn(x)
        x -= delta
        norm(delta) < norm(x) * tolerance && break
    end
    x
end
Out[2]:
quadraticSolver
In [3]:
"""
    findMatrix(e, n)

`e` and `n` are vectors of three 3D vectors.
Returns a rotation matrix s.t. n[i]'*R*e[i] == 0.
"""
function findMatrix(e, n)
    # Set up the quaternion equations
    A = [Matrix{Float64}(I, 4, 4)]
    for i in 1:3
        E = [   0    -e[i][1] -e[i][2] -e[i][3]
             e[i][1]     0     e[i][3] -e[i][2]
             e[i][2] -e[i][3]     0     e[i][1]
             e[i][3]  e[i][2] -e[i][1]     0   ]
        N = [   0    -n[i][1] -n[i][2] -n[i][3]
             n[i][1]     0    -n[i][3]  n[i][2]
             n[i][2]  n[i][3]     0    -n[i][1]
             n[i][3] -n[i][2]  n[i][1]     0   ]
        push!(A, E' * N)
    end
    b = [1., 0, 0, 0]
    # Compute q such that q' * A[i] * q = b[i], i = 1..4
    q = quadraticSolver(A, b, [1., 0, 0, 0])
    # Convert q to rotation matrix form
    [q[1]^2+q[2]^2-q[3]^2-q[4]^2 2(q[2]*q[3]-q[1]*q[4])      2(q[2]*q[4]+q[1]*q[3])
     2(q[2]*q[3]+q[1]*q[4])      q[1]^2+q[3]^2-q[2]^2-q[4]^2 2(q[3]*q[4]-q[1]*q[2])
     2(q[2]*q[4]-q[1]*q[3])      2(q[3]*q[4]+q[1]*q[2])      q[1]^2+q[4]^2-q[2]^2-q[3]^2]
end
Out[3]:
findMatrix
In [4]:
e = [normalize(rand(3)) for _ in 1:3]
Out[4]:
3-element Array{Array{Float64,1},1}:
 [0.381173, 0.839643, 0.386919] 
 [0.45235, 0.689038, 0.566221]  
 [0.0553677, 0.908065, 0.415153]
In [5]:
n = [normalize(rand(3)) for _ in 1:3]
Out[5]:
3-element Array{Array{Float64,1},1}:
 [0.714805, 0.575686, 0.397039] 
 [0.971803, 0.0881776, 0.218685]
 [0.763926, 0.151546, 0.627256]
In [6]:
R = findMatrix(e, n)
Out[6]:
3×3 Array{Float64,2}:
 0.203831  -0.844959   0.494467
 0.782934  -0.162539  -0.600496
 0.587765   0.509534   0.628417
In [9]:
[dot(n[i], R * e[i]) for i in 1:3] # error
Out[9]:
3-element Array{Float64,1}:
 -7.505557952924846e-9 
 -6.0869778384375905e-9
 -4.026134425849648e-10
In [16]:
(R[:,1]'*R[:,2], R[:,1]'*R[:,3], R[:,2]'*R[:,3]) # R's columns are pairwise orthogonal
Out[16]:
(-5.551115123125783e-17, 5.551115123125783e-17, 5.551115123125783e-17)
In [17]:
[norm(R[:,i]) for i in 1:3] # R's columns are unit vectors
Out[17]:
3-element Array{Float64,1}:
 1.0000000000011153
 1.0000000000011153
 1.0000000000011153