In [1]:
using LinearAlgebra

In [2]:
"""

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