<jupyter_start><jupyter_text>Derivation of matrix to CGA rotor conversion We want to find a function taking a projective transformation matrix and returning the rotor in CGA. Given the Tait-Byen angles $\alpha $ (yaw), $\beta $ (pitch), $\gamma$ (roll) of the orientation transformation and the translation vector $\mathbf{t}$. The translation vector represents the origin of the camera coordinate system in the world coordinate system.  Relation between world and camera coordinates$$\mathbf{x}_{camera} = R \mathbf{x}_{world} + \mathbf{t}$$With the inverse operation:$$\mathbf{x}_{world} = R^T \mathbf{x}_{camera} - R^T t $$If we represent the points in homogenous coordinates i.e. $X = \begin{bmatrix}        \mathbf{x} \\         1    \end{bmatrix} $$$ X_{camera} = \begin{bmatrix}        R & \mathbf{t} \\        0 & 1    \end{bmatrix} X_{world} $$$$X_{world} = \begin{bmatrix}        R^T & - R^T \mathbf{t} \\        0 & 1    \end{bmatrix} X_{camera} $$ Conformal versionIn conformal representation we can translate between the two coordinate systems with a simple sandwich product with the versor $V$. Note that $X_{camera}$ and $X_{world}$ are now conformal points. $ X_{camera} = V  X_{world} \tilde{V} $And similarly we can do the inverse operation$ X_{world} = \tilde{V}  X_{camera} V $We can decompose the versor $V$ to its rotation and translation component repectively $ V = T(\mathbf{t}) \ R(\alpha, \beta, \gamma)$Here is a sketch of the setup. Notice that getting the point $A_c$ in the frame of reference of the world given that $A_c$ is in the frame of reference of the camera involves first rotating it and then translating it to its correct location. ![Coordinate System](rotfigure.png) Let's decompose the matrix further to determine a good conversion formula$$ V = f\left(\begin{bmatrix}        R & \mathbf{t} \\        0 & 1    \end{bmatrix}\right) $$    $$ R =  R_z(\gamma) R_y(\beta) R_x(\alpha)$$$$ \mathbf{t} = \begin{bmatrix}        x, y, z    \end{bmatrix}^T$$\begin{equation} R_x(\theta) = \begin{pmatrix}        1 & 0           & 0 \\        0 & \cos(\theta) & -\sin(\theta) \\        0 & \sin(\theta) & \cos(\theta)    \end{pmatrix}  \quad \ R_y(\theta) = \begin{pmatrix}        \cos(\theta)  & 0 & \sin(\theta) \\        0            & 1 & 0            \\        -\sin(\theta) & 0 & \cos(\theta)    \end{pmatrix}  \quad \ R_z(\theta) = \begin{pmatrix}        \cos(\theta) & -\sin(\theta) & 0\\        \sin(\theta) & \cos(\theta)  & 0\\        0           & 0            & 1    \end{pmatrix}  \end{equation}From https://en.wikipedia.org/wiki/Euler_anglesIntrinsic_rotations we get that the complete $R$ is ![Rotation matrix ZYX](images/rot_matrix_zyx.svg)Note that $c_1 =\cos(\gamma), \ c_2 =\cos(\beta), \ c_3 = \cos(\alpha) $ and $s_1 =\sin(\gamma), \ s_2 =\sin(\beta), \ s_3 = \sin(\alpha) $     $$ Z_1 Y_2 X_3 = \begin{bmatrix}         c_1c_2 & c_1s_2s_3 - c_3s_1 & s_1s_3 + c_1c_3s_2 \\        c_2s_1 & c_1c_3 + s_1s_2s_3 & c_3s_1s_2 - c_1s_3 \\        -s_2   & c_2s_3             & c_2c_3             \\    \end{bmatrix}$$ This allows us to recover the angles $$\gamma = \tan^{-1}\left(\frac{R_{2, 1}}{ R_{1, 1}}\right) $$$$\beta  = \tan^{-1}\left(\frac{-R_{3, 1}}{\sqrt{R_{3, 2}^2 + R_{3, 3}^2}}\right)$$$$\alpha = \tan^{-1}\left(\frac{R_{3, 2}}{R_{3, 3}}\right)$$  Conformal decompositionLets define these in the conformal representation tooAs shown above we have $ V = T(\mathbf{t}) \ R(\alpha, \beta, \gamma)$$T(\mathbf{t}) = e^{\frac{n_{\infty} \mathbf{t}}{2}} = 1 + \frac{n_{\infty} \mathbf{t}}{2}$The reason for the ordering here is a bit unclear$ R(\alpha, \beta, \gamma) = R(\gamma, B_z)R(\beta, B_y)R(\alpha, B_x)$$B_x = e_{2, 3}, \ B_y = -e_{1, 3}, \ B_z = e_{1, 2}$$R(\theta,  B_x) = \cos(-\theta/2.) + B  \sin(-\theta/2.)$TODO: no longer valid due to reordering$$ R(\alpha, \beta, \gamma) = \left(\cos(-\alpha/2.) + e_{2, 3} \sin(-\alpha/2.)\right)\left(\cos(-\beta/2.) + (-e_{1, 3}) \sin(-\beta/2.)\right)\left(\cos(-\gamma/2.) + e_{1, 2} \sin(-\gamma/2.)\right) $$ TODO: Check the signs \begin{align}R(\alpha, \beta, \gamma) & = \cos(-\alpha/2.) \cos(-\beta/2.) \cos(-\gamma/2.) +  \sin(-\alpha/2.)\sin(-\beta/2.)\sin(-\gamma/2.) \\& + e_{1, 2}\left( \cos(-\alpha/2.) \cos(-\beta/2.) \sin(-\gamma/2.) -  \sin(-\alpha/2.)\sin(-\beta/2.)\cos(-\gamma/2.) \right) \\& + e_{2, 3}\left( \cos(-\gamma/2.) \cos(-\beta/2.) \sin(-\alpha/2.) -  \sin(-\gamma/2.)\sin(-\beta/2.)\cos(-\alpha/2.) \right) \\& +(-e_{1, 3})\left( \cos(-\alpha/2.) \cos(-\gamma/2.) \sin(-\beta/2.) +  \sin(-\gamma/2.)\sin(-\alpha/2.)\cos(-\beta/2.) \right)\end{align}For simplicity we will now define this as$R = \gamma + B_R$Where $\gamma$ is a scalar and $B_R$ is a bivector in $\mathcal{GA}_{3, 0}$However, we will later take advantage of this being the same representation as the quaternions accounting for the sign difference of $e_{13}$ $$    \alpha = -\tan^{-1}(\frac{2(q_0 q_3 + q_1 q_2)}{1 - 2 (q_2^2 + q_3^2)}) \\    \beta  = \sin^{-1}(2(q_0 q_1 + q_2 q_3)) \\    \gamma = -\tan^{-1}(\frac{2(q_0 q_1 + q_2 q_3)}{1 - 2 (q_1^2 + q_2^2)}) \\$$  We can now reason about the terms in $V$:$V =  \left(1 + \frac{n_{\infty} \mathbf{t}}{2}\right)\left(\gamma + B_R \right)$$V = \gamma + B_R + \gamma \frac{n_{\infty} \mathbf{t}}{2}  + B_R + \frac{n_{\infty} \mathbf{t}}{2} B_R$$\frac{n_{\infty} \mathbf{t}}{2} B_R = \frac{n_{\infty}}{2} \left(\mathbf{t} \cdot B_R + \mathbf{t} \wedge B_R \right) $We can therefore easily extract the various components from V: $\gamma = \langle V \rangle_0$$B_R = \langle V \rangle_2 - \langle V \rangle_2 \cdot e_p$ %TODO: Check$R = \gamma + B_R$$T = V \tilde{R} $$\mathbf{t} = 2 \ (T \cdot e_p) $$V = T R$  CodeImport libraries<jupyter_code>import os
import sys
sys.path.insert(0, os.path.abspath(os.path.realpath('../../CGA_CameraCalibration/src/')))

print(sys.executable)
from clifford import MultiVector
from clifford import g3c
from numpy import pi, e
import numpy as np

from common.cgatools import Translator, Rotor, MVto3DVec, MVEqual, VectoMV
locals().update(g3c.blades)


ep, en, up, down, homo, E0, ninf, no = (g3c.stuff["ep"], g3c.stuff["en"], 
                                        g3c.stuff["up"], g3c.stuff["down"], g3c.stuff["homo"], 
                                        g3c.stuff["E0"], g3c.stuff["einf"], -g3c.stuff["eo"])<jupyter_output><empty_output><jupyter_text>Defining transformation functions<jupyter_code>#Useful transformations


def rotation_to_matrix(theta):
    alpha, beta, gamma = theta

    cy = np.cos(alpha)
    sy = np.sin(alpha)
    X = np.matrix([[1, 0, 0],
                   [0, cy, -sy],
                   [0, sy, cy]])
    

    cp = np.cos(beta)
    sp = np.sin(beta)
    Y = np.matrix([[cp,  0, sp],
                   [0,   1, 0],
                   [-sp, 0, cp]])

    cr = np.cos(gamma)
    sr = np.sin(gamma)
    Z = np.matrix([[cr, -sr, 0],
                   [sr, cr, 0],
                   [0, 0, 1]])
    
    R = np.dot(Z, np.dot(Y, X))

    return R

def rotation_to_rotor(theta):
    alpha, beta, gamma = theta
    return Rotor(e12, gamma) * Rotor(-e13, beta) * Rotor(e23, alpha) 

def translation_to_rotor(translation):
    x, y, z = translation
    t = x*e1 + y*e2 + z*e3
    return Translator(t)

def matrix_to_rotation(R_mat):
    #From http://planning.cs.uiuc.edu/node103.html
    alpha = np.arctan2(R_mat[2, 1], R_mat[2, 2])
    beta  = np.arctan2(-R_mat[2, 0],np.sqrt(R_mat[2, 1]**2 + R_mat[2, 2]**2))
    gamma = np.arctan2(R_mat[1, 0], R_mat[0, 0])
    return np.array([alpha, beta, gamma])
    
def matrix_to_translation(R):
    return R[:3, 3]

def full_matrix_to_param(R_mat):
    theta = matrix_to_rotation(R_mat)
    translation = matrix_to_translation(R_mat)
    return theta, translation
    
def parameters_to_versor(theta, translation):
    T = translation_to_rotor(translation)
    R = rotation_to_rotor(theta)
    return T*R

"""
#Modified
#From wikipedia: https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles
def quaternion_to_rotation(quat):
    w, x, y, z = quat
    ysqr = y * y
    
    t0 = +2.0 * (w * x + y * z)
    t1 = +1.0 - 2.0 * (x * x + ysqr)
    Z  = -np.arctan2(t0, t1)
    
    t2 = +2.0 * (w * y - z * x)
    t2 = +1.0 if t2 > +1.0 else t2
    t2 = -1.0 if t2 < -1.0 else t2
    Y  = -np.arcsin(t2)
    
    t3 = +2.0 * (w * z + x * y)
    t4 = +1.0 - 2.0 * (ysqr + z * z)
    X  = -np.arctan2(t3, t4)
    
    return np.array([X, Y, Z])
"""

def quaternion_to_rotation(quat):
    w, x, y, z = quat
    
    t0 = 2*(w*z - x*y)
    t1 = w*w + x*x - y*y - z*z 
    X  = -np.arctan2(t0, t1)
    
    t2 = +2.0 * (w * y + z * x)
    t2 = +1.0 if t2 > +1.0 else t2
    t2 = -1.0 if t2 < -1.0 else t2
    Y  = -np.arcsin(t2)
    
    t3 = +2.0 * (w * x - y * z)
    t4 =  w*w - x*x - y*y + z*z 
    Z  = -np.arctan2(t3, t4)
    
    return np.array([Z, Y, X])


def rotation_to_quaternion(theta):
    c = np.cos(-theta/2)
    s = np.sin(-theta/2)
    
    quat = np.zeros(4)
    
    quat[0] = c[2]*c[1]*c[0] - s[2]*s[1]*s[0]
    quat[1] = c[2]*c[1]*s[0] + s[2]*s[1]*c[0]
    quat[2] = c[2]*s[1]*c[0] - s[2]*c[1]*s[0]
    quat[3] = s[2]*c[1]*c[0] + c[2]*s[1]*s[0]
    return quat 

def rotor_to_quaternion(rotor):
    quat = np.zeros(4)
    quat[0] = rotor[0]
    quat[1] = rotor[(2, 3)]
    quat[2] = -rotor[(1, 3)]  
    quat[3] = rotor[(1, 2)]
    return quat 



def rotor_to_translation(T):
    return MVto3DVec(ep | T)

def versor_decomposition(V):
    B_R = V[(1, 2)]*e12 + V[(1, 3)]*e13 + V[(2, 3)]*e23  
    R = V(0) + B_R
    T = (V * ~R)
    return T, R

def versor_to_param(V):
    T, R = versor_decomposition(V)
    theta = quaternion_to_rotation(rotor_to_quaternion(R))
    t     = rotor_to_translation(T)*2
    return theta, t
    
    
def full_projection_matrix(theta, translation):
    ans = np.zeros((4, 4))
    ans[:3, :3] = rotation_to_matrix(theta)
    ans[:3, 3] = translation
    ans[3, 3] = 1
    return ans
    
    
def versors_to_projection(V):
    return full_projection_matrix(*versor_to_param(V))

def projection_to_versor(P):
    theta = matrix_to_rotation(P) 
    t     = matrix_to_translation(P)
    return parameters_to_versor(theta, t)<jupyter_output><empty_output><jupyter_text>Testing:<jupyter_code>alpha = -0.65
beta  = 0.43
gamma = -0.21

x, y, z = 123, 456, 789

theta = np.array([alpha, beta, gamma])
t = np.array([x, y, z])

def quaternion_mult(q,r):
    w0, x0, y0, z0 = q
    w1, x1, y1, z1 = r
    return np.array([-x1*x0 - y1*y0 - z1*z0 + w1*w0,
                         x1*w0 + y1*z0 - z1*y0 + w1*x0,
                        -x1*z0 + y1*w0 + z1*x0 + w1*y0,
                         x1*y0 - y1*x0 + z1*w0 + w1*z0], dtype=np.float64)

def point_rotation_by_quaternion(point, quat):
    r = np.array([0, point[0], point[1], point[2]]) 
    q = quat
    q_conj = np.array([q[0],-1*q[1],-1*q[2],-1*q[3]])
    return quaternion_mult(quaternion_mult(q,r),q_conj)[1:]



print()
print("Testing rotation")
a = t
R_rot = rotation_to_rotor(theta)
R_mat = rotation_to_matrix(theta)

print(R_mat)

A     = up(VectoMV(a))
B_rot = R_rot*A*~R_rot
print(B_rot)

b     = np.dot(R_mat, a)
print(b)
#B_mat = up(VectoMV(b))
b_rot = MVto3DVec(down(B_rot))
print(b_rot)

assert(np.allclose(b, b_rot))




rotor = rotation_to_rotor(theta)
print(rotor)

quat = rotor_to_quaternion(rotor)
print(quat)

quat_direct = rotation_to_quaternion(theta)
print(quat_direct)
b_quat = point_rotation_by_quaternion(a, quat_direct)
print(b_quat)
assert(np.allclose(b, b_quat))

print()
print("Testing matrices")

#Testing angles <-> matrix
R_mat = rotation_to_matrix(theta)
print(R_mat)
angles = matrix_to_rotation(R_mat)
print(angles)
assert(np.allclose(angles, theta))

projection = full_projection_matrix(theta, t)
angles = matrix_to_rotation(projection)
t_mat = matrix_to_translation(projection)
print(t_mat)
print(angles)
assert(np.allclose(angles, theta))


print()
#Testing angles <-> quaternion
print("Testing quaternions")




quat_direct   = rotation_to_quaternion(theta)
angles_direct = quaternion_to_rotation(quat_direct)

print(angles_direct)
print(quat_direct)



rotor = rotation_to_rotor(theta)
print(rotor)

quat = rotor_to_quaternion(rotor)

print(quat)

angles = quaternion_to_rotation(quat)
print(angles)
print(theta)
assert(np.allclose(angles, theta))
assert(np.allclose(quat_direct, quat))
assert(np.allclose(angles_direct, theta))


#Testing versor decomposition
print()
print("Testing versor")


V = parameters_to_versor(theta, t)
print("V:", V)
print()

T, R = versor_decomposition(V)
print()
print("R_dec:", R)
print("R    :", rotation_to_rotor(theta))
assert(MVEqual(R, rotation_to_rotor(theta)))

print()
print("T_dec:", T)
print("T    :", translation_to_rotor(t))
assert(MVEqual(T, translation_to_rotor(t)))

theta_ver, t_ver = versor_to_param(V) 
print()
print("t    ", t_ver)
assert(np.allclose(t, t_ver))

print("theta", theta_ver)
assert(np.allclose(theta_ver, theta))


#Testing all together
print("")
P = full_projection_matrix(theta, t)
V_proj = projection_to_versor(P)
P_test = versors_to_projection(V)

print("V:     ", V)
print("")
print("P:     ")
print(P)
print("P_test:")
print(P_test)

assert(MVEqual(V, V_proj))
assert(np.allclose(P, P_test))




alpha = 0.5
beta  = 0.7
gamma = -0.6

theta = np.array([alpha, beta, gamma])

R_rot = rotation_to_rotor(theta)
R_mat = rotation_to_matrix(theta)
quat_rot = rotor_to_quaternion(R_rot)
quat_real = rotation_to_quaternion(theta)

s3, s2, s1 = np.sin(-theta/2)
c3, c2, c1 = np.cos(-theta/2)


print(quat_real)
print(quat_rot)


print("Something \n")
#quaternion_to_rotation(quat)

print()
print("Testing rotation")
a = t

A     = up(VectoMV(a))
B_rot = R_rot*A*~R_rot
print(B_rot)

b     = np.array(np.dot(R_mat, a)).reshape((3))

print(b)
B_mat = up(VectoMV(b))
b_rot = MVto3DVec(down(B_rot))
print(b_rot)

assert(np.allclose(b, b_rot))

b_quat = point_rotation_by_quaternion(a, quat_real)
b_quat_2 = point_rotation_by_quaternion(a, quat_rot)
print(b_quat)
print(b_quat_2)
assert(np.allclose(b, b_quat))
assert(np.allclose(b, b_quat_2))

print()
print(R_mat)

C3, C2, C1 = np.cos(theta) 
S3, S2, S1 = np.sin(theta) 

print(C1 * C2)<jupyter_output>[ 0.84444826 -0.32020755 -0.2487188   0.35001887]
[ 0.84444826 -0.32020755 -0.2487188   0.35001887]
Something 


Testing rotation
(574.40695^e1) - (366.42507^e2) + (617.55593^e3) + (422792.5^e4) + (422793.5^e5) - (0.0^e124) + (0.0^e125) + (0.0^e135) + (0.0^e235)
[ 574.40694877 -366.42507189  617.55592775]
[ 574.40694873 -366.42507187  617.55592771]
[ 574.40694877 -366.42507189  617.55592775]
[ 574.40694877 -366.42507189  617.55592775]

[[ 0.6312515   0.75042893  0.19590294]
 [-0.43186238  0.54990782 -0.71490997]
 [-0.64421769  0.36668488  0.67121217]]
0.6312514969513067<jupyter_text>Some work on deriving relations  $\frac{n_{\infty}}{2} \wedge \mathbf{t} \wedge B_R =  \langle V \rangle_4$$$    \alpha = \tan^{-1}(\frac{-R_{2, 3}}{R_{3, 3}}) $$$$    \beta  = \sin^{-1}(R_{1, 3}) $$$$    \gamma = \tan^{-1}(\frac{-R_{1, 2}}{R_{1, 1}}) $$<jupyter_code>V = rotation2Rotor(alpha, beta, gamma)

print(V)

a1 = V[0]
a2 = -float(V(2)|e12)
a3 = float(V(2)|e13)
a4 = -float(V(2)|e23)

print(V[(1, 2)])

a1, a2, a3, a4
import sympy as sp
import numpy as np
import scipy.linalg as lin
from sympy import init_printing , expand, simplify, cos, sin, latex

init_printing()

gamma, beta, alpha = sp.symbols("phi theta psi")

e12_sym, e31_sym, e23_sym = sp.symbols("e12 e31 e23")

def R_sym(theta, B):
    return sp.cos(-theta/2) + B * sp.sin(-theta/2)

expr = expand(R_sym(gamma, e12_sym)*R_sym(beta, e31_sym)*R_sym(alpha, e23_sym))
expr
expr_simple = expr.subs(e12_sym*e23_sym*e31_sym, 1).subs(e12_sym*e23_sym, -e31_sym).subs(e12_sym*e31_sym, e23_sym).subs(e31_sym*e23_sym, e12_sym).collect((e12_sym, e23_sym, e31_sym))
print(repr(expr_simple))

expr_simple
print(latex(expr_simple))
q0 = - sin(alpha)*sin(beta)*sin(gamma) + cos(alpha)*cos(beta)*cos(gamma)
q1 = (sin(alpha)*sin(beta)*cos(gamma) - sin(gamma)*cos(alpha)*cos(beta))
q2 = -(-sin(alpha)*sin(gamma)*cos(beta) - sin(beta)*cos(alpha)*cos(gamma))
q3 = (-sin(alpha)*cos(beta)*cos(gamma) + sin(beta)*sin(gamma)*cos(alpha))
w, x, y, z = q0, q1, q2, q3
ysqr = y * y

t0 = 2 * (w * x + y * z)
t1 = 1 - 2 * (x * x + y* y)

t0.expand().simplify().factor()
t1.expand().simplify().factor()
(t0/t1).expand().simplify().factor()
f1 = cos(theta_1)*cos(theta_2)*cos(theta_3) - sin(theta_1)*sin(theta_2)*sin(theta_3) #- a1 # constant
f2 = sin(theta_1)*cos(theta_2)*cos(theta_3) - sin(theta_2)*sin(theta_3)*cos(theta_1) - a2 # proportional to e12
f3 = sin(theta_3)*cos(theta_1)*cos(theta_2) - sin(theta_1)*sin(theta_2)*cos(theta_3) - a3 # proportional to -e13
f4 = sin(theta_2)*cos(theta_1)*cos(theta_3) + sin(theta_1)*sin(theta_3)*cos(theta_2) - a4 # proportional to e23




x = np.array([alpha, beta, gamma])

print(F(alpha, beta, gamma))
#print(J(*x))


#for i in range(10):
#    ans = lin.lstsq(J(*x), F(*x))
#    dx = ans[0].squeeze()
#    x = x + dx
#    print(dx)

#print(x)
    
quaternion_to_euler_angle(a1, a2, a3, a4)<jupyter_output><empty_output>
{'fraction_non_alphanumeric': 0.18704443921937455, 'fraction_numerical': 0.05801786973900776, 'mean_word_length': 3.3136409736308314, 'pattern_counts': {'":': 0, '<': 20, '<?xml version=': 0, '>': 20, 'https://': 2, 'lorem ipsum': 0, 'www.': 0, 'xml': 0}, 'pii_count': 0, 'substrings_counts': 0, 'word_list_counts': {'cursed_substrings.json': 7, 'profanity_word_list.json': 0, 'sexual_word_list.json': 0, 'zh_pornsignals.json': 0}}
{'chain_length': '4', 'dir': 'jupyter-structured-clean-dedup', 'id': '443430', 'license': 'MIT', 'n_tokens_mistral': 7658, 'n_tokens_neox': 7106, 'n_words': 1521, 'path': 'examples/notebooks/mat_rotor_conversion.ipynb', 'repo_name': 'ereide/pyga-camcal', 'size': '17012'}