<jupyter_start><jupyter_text>Result<jupyter_code>def cost(a,b,R):
    r = (a.spin(R) - b)
    return (r * r * 0.5)[0]
cost(vsr.Vec(1,0,0), vsr.Vec(0,1,0), rotor1)<jupyter_output><empty_output><jupyter_text>Jacobian<jupyter_code>def jacobian34(a,b,R):
    
    def J(Ei,a,b,R):
        Ei = vsr.EGA(Ei)
        a = vsr.EGA(a)
        b = vsr.EGA(b)
        R = vsr.EGA(R)
        return Ei * a * R.rev() + R * a * Ei.rev()

    JR = np.zeros((3,4))
    JR[:,0] = np.array(J(vsr.Rot(1,0,0,0),a,b,R))[1:4]
    JR[:,1] = np.array(J(vsr.Rot(0,1,0,0),a,b,R))[1:4]
    JR[:,2] = np.array(J(vsr.Rot(0,0,1,0),a,b,R))[1:4]
    JR[:,3] = np.array(J(vsr.Rot(0,0,0,1),a,b,R))[1:4]
    return JR
J34 = jacobian34(vectors_a[0], vectors_b[0], rotor1)
print(J34)
vsr.EGA(vsr.Rot(0,1,0,0))*  vsr.EGA(vsr.Vec(1,0,0)) * vsr.EGA(rotor1.rev()) + vsr.EGA(rotor1) * vsr.EGA(vsr.Vec(1,0,0)) * vsr.EGA(vsr.Rot(0,1,0,0).rev())
jacobian34(vsr.Vec(1,0,0), vsr.Vec(0,1,0), rotor1)
def jacobian43(R):
    JB = np.zeros((4,3))
    JB[:,0] = np.array(vsr.Biv(1,0,0) * R)
    JB[:,1] = np.array(vsr.Biv(0,1,0) * R)
    JB[:,2] = np.array(vsr.Biv(0,0,1) * R)
    return JB
J43 = jacobian43(rotor1)
print(J43)
def jacobian(a,b,rot):
    return np.dot(jacobian34(a,b,rot), jacobian43(rot))
def jacobian_cay(a,b,rot):
    return np.dot(jacobian34(a,b,rot), 2.0 * jacobian43(rot) )
J = jacobian(vectors_a[0], vectors_b[0], rotor1)
print(np.dot(J.T,J))
Hess(vectors_a[0], vectors_b[0], rotor1)
def Hi(B,a,b,R):
    G = B.comm(a.spin(R)) * 2
    H = (a.spin(R) - b) * ((B * B * a.spin(R)) + 
                           2.0 * a.spin(R).spin(B) + (a.spin(R) * B * B))
    ans = H + G * G.rev()
#     ans = G * G
#     ans = H
    return ans
r1 = vsr.EGA(rotor1)
Hi(vsr.EGA(vsr.Biv(1,0,0) )* r1, vsr.EGA(vectors_a[0]), vsr.EGA(vectors_b[0]), r1)[0]
def res(a,b,rot):
    return np.array((a.spin(rot) - b))
def cost(a,b,rot):
    r = res(a,b,rot)
    return (r * r * 0.5)[0]
def Hess(a,b,rot):
    a = vsr.EGA(a)
    b = vsr.EGA(b)
    R = vsr.EGA(rot)
    E12 = vsr.EGA(vsr.Biv(1,0,0))
    E13 = vsr.EGA(vsr.Biv(0,1,0))
    E23 = vsr.EGA(vsr.Biv(0,0,1))
    H12 = Hi(E12,a,b,R) 
    H13 = Hi(E13,a,b,R)  
    H23 = Hi(E23,a,b,R)  
    H1213 = 0.25 * (Hi(E12+E13,a,b,R) - Hi(E12-E13,a,b,R))
    H1223 = 0.25 * (Hi(E12+E23,a,b,R) - Hi(E12-E23,a,b,R))
    H1323 = 0.25 * (Hi(E13+E23,a,b,R) - Hi(E13-E23,a,b,R))
    return np.array([[  H12[0], H1213[0], H1223[0]],
                     [H1213[0],   H13[0], H1323[0]],
                     [H1223[0], H1323[0],   H23[0]]])
def grad(a,b,R):
    a = vsr.EGA(a)
    b = vsr.EGA(b)
    R = vsr.EGA(R)
    E12 = vsr.EGA(vsr.Biv(1,0,0)) 
    E13 = vsr.EGA(vsr.Biv(0,1,0))
    E23 = vsr.EGA(vsr.Biv(0,0,1)) 
    d = lambda Ei : ((a.spin(R) - b) * (2 * Ei.comm(a.spin(R))))[0]
    return np.array([d(E12), d(E13), d(E23)])
np.sum([grad(a,b, rotor1) for a,b in zip(vectors_a, vectors_b)], axis=0)
np.sum([Hess(a,b, rotor1) for a,b in zip(vectors_a[:1], vectors_b[:1])], axis=0)
np.dot(jacobians.T, jacobians)
np.dot(jacobians.T,residuals)
residuals = np.array([res(a,b,rotor1) for a,b in zip(vectors_a, vectors_b)]).reshape(-1,1)
residuals
np.dot(residuals.T,residuals)[0,0]
jacobians = np.array([jacobian(a,b,rotor1) for a,b in zip(vectors_a, vectors_b)]).reshape(-1,3)
jacobians
def Cayley(B):
    wt = B.norm()
#     if B.norm() > 1.0:
#         B = B.inv()
    Rp = vsr.Rot(1.0, B[0], B[1], B[2])
    Rn = vsr.Rot(1.0, -B[0], -B[1], -B[2])
    return Rp * Rn.inv()
Cay = lambda B : return vsr.Rot()
Cayley(vsr.Biv(tan(pi/6),0,0))
cos(pi/3)
n_points=3
vectors_a = [vsr.Vec(*np.random.normal(0.0, 0.8, 3)) for i in range(n_points)]
vectors_b = [vector.spin(rotor) for vector in vectors_a]

def update(rot):
    residuals = np.array([res(a,b,rot) for a,b in zip(vectors_a, vectors_b)]).reshape(-1,1)
    cost = np.dot(residuals.T,residuals)[0,0]    
    jacobians = np.array([jacobian_cay(a,b,rot) for a,b in zip(vectors_a, vectors_b)]).reshape(-1,3)
    
    g = np.dot(jacobians.T, residuals)
    H = np.dot(jacobians.T, jacobians)

#     g = np.sum([grad(a,b, rot) for a,b in zip(vectors_a, vectors_b)], axis=0)
#     H = np.sum([Hess(a,b, rot) for a,b in zip(vectors_a, vectors_b)], axis=0)
#     print(H)

    B = np.dot(np.linalg.pinv(H), -g)
#     rot = vsr.Biv(*B).exp() * rot
    rot = Cayley(vsr.Biv(*B)) * rot
    grad_norm = np.linalg.norm(g)
    return rot, cost, grad_norm

rot = vsr.Rot(1,0,0,0)
ress = []
for i in range(1000):
    rot, cost, grad_norm = update(rot)
    ress.append(cost)
#     print(cost)
    if cost < 1e-6: 
        break
print(i)
print(rot)

semilogy(ress)
vsr.Dll(-pi/6,0,0,0,0,1).exp()
class MotorOptimizer(object):
    def __init__(self):
        self._motor_id = vsr.Mot(1,0,0,0,0,0,0,0)
        self._motor = vsr.Vec(1,2,3).trs() * vsr.Biv(pi/6,0,0).exp()
    
    def dataset(self):
        n_points = 10
        sigma = 0.09
        points_a = [(vsr.Vec(*np.random.normal(0.0, 0.8, 3)).null(),
                     vsr.Vec(*np.random.normal(0.0, 0.8, 3)).null()  )
                    for i in range(n_points)]

        points_a_noisy = [(vsr.Vec(*(np.array(a)[:3] + sigma * np.random.randn(3))).null(),
                           vsr.Vec(*(np.array(b)[:3] + sigma * np.random.randn(3))).null()) 
                          for a,b in points_a]

        lines_a = [vsr.Dll(a,b).unit() for (a,b) in points_a]
        lines_b = [line.spin(self._motor) for line in lines_a]
        return lines_a, lines_b
    
    def _jacobian86(self, M):
        J = np.zeros((8,6))
        J[:,0] = np.array(vsr.Dll(1,0,0,0,0,0) * M)
        J[:,1] = np.array(vsr.Dll(0,1,0,0,0,0) * M)
        J[:,2] = np.array(vsr.Dll(0,0,1,0,0,0) * M)
        J[:,3] = np.array(vsr.Dll(0,0,0,1,0,0) * M)
        J[:,4] = np.array(vsr.Dll(0,0,0,0,1,0) * M)
        J[:,5] = np.array(vsr.Dll(0,0,0,0,0,1) * M)
        return J
    
    def _jacobian68(self,a,b,M):
        
        def Ji(Ei,a,b,M):
            Ei = vsr.CGA(Ei)
            a = vsr.CGA(a)
            b = vsr.CGA(b)
            M = vsr.CGA(M)
            return Ei * a * M.rev() + M * a * Ei.rev()
        
        def to_mot(a):
            return np.array([a[6], a[7], a[8], a[12], a[13], a[14]])

        J = np.zeros((6,8))
        J[:,0] = to_mot(Ji(vsr.Mot(1,0,0,0,0,0,0,0),a,b,M))
        J[:,1] = to_mot(Ji(vsr.Mot(0,1,0,0,0,0,0,0),a,b,M))
        J[:,2] = to_mot(Ji(vsr.Mot(0,0,1,0,0,0,0,0),a,b,M))
        J[:,3] = to_mot(Ji(vsr.Mot(0,0,0,1,0,0,0,0),a,b,M))
        J[:,4] = to_mot(Ji(vsr.Mot(0,0,0,0,1,0,0,0),a,b,M))
        J[:,5] = to_mot(Ji(vsr.Mot(0,0,0,0,0,1,0,0),a,b,M))
        J[:,6] = to_mot(Ji(vsr.Mot(0,0,0,0,0,0,1,0),a,b,M))
        J[:,7] = to_mot(Ji(vsr.Mot(0,0,0,0,0,0,0,1),a,b,M))
        return J
        
    def jacobian(self, a, b, M):
        return np.dot(self._jacobian68(a,b,M), self._jacobian86(M))
    
    def res(self,a,b,M):
        return np.array((a.spin(M) - b))
    
    def grad(self, a, b, M):
        return np.dot(self.jacobian(a,b,M).T,self.res(a,b,M))
    
    def JtJ(self, a, b, M):
        J = self.jacobian(a,b,M)
        return np.dot(J.T,J)
    
    def update(self,vectors_a, vectors_b, M):
        residuals = np.array([self.res(a,b,M) 
                              for a,b in zip(vectors_a, vectors_b)]).reshape(-1,1)
        cost = np.dot(residuals.T,residuals)[0,0]    
        jacobians = np.array([self.jacobian(a,b,M) 
                              for a,b in zip(vectors_a, vectors_b)]).reshape(-1,6)

        g = np.dot(jacobians.T, residuals)
        H = np.dot(jacobians.T, jacobians)

        B = np.dot(np.linalg.pinv(H), -g)
        M = vsr.Dll(*B).exp() * M
        grad_norm = np.linalg.norm(g)
        return M, cost, grad_norm
mo = MotorOptimizer()
mot = mo._motor_id
a,b = mo.dataset()
mo.grad(a[0], b[0], mot)
ress = []
for i in range(10):
    mot, cost, grad_norm = mo.update(a,b,mot)
    ress.append(cost)
    print(cost)
    if cost < 1e-6: 
        break
print(i)

semilogy(ress)
print(mo.grad(a[0], b[0], mo._motor_id))
def grad_lines(a,b,R):
    a = vsr.CGA(a)
    b = vsr.CGA(b)
    R = vsr.CGA(R)
    E12 = vsr.CGA(vsr.Dll(1,0,0,0,0,0)) 
    E13 = vsr.CGA(vsr.Dll(0,1,0,0,0,0))
    E23 = vsr.CGA(vsr.Dll(0,0,1,0,0,0)) 
    E1i = vsr.CGA(vsr.Dll(0,0,0,1,0,0)) 
    E2i = vsr.CGA(vsr.Dll(0,0,0,0,1,0)) 
    E3i = vsr.CGA(vsr.Dll(0,0,0,0,0,1)) 
    d = lambda Ei : ((a.spin(R) - b) * (2 * Ei.comm(a.spin(R))))[0]
    return np.array([d(E12), d(E13), d(E23), d(E1i), d(E2i), d(E3i)])
(a[0].spin(R) - b[0]) * vsr.Dll(1,0,0,0,0,0).comm(a[0]) * 2
a[0].drv().vec()
ad,bd = mo.dataset()
a = vsr.CGA(ad[0])
b = vsr.CGA(bd[0])
R = vsr.CGA(mo._motor_id)
E12 = vsr.CGA(vsr.Dll(1,0,0,0,0,0)) 
d = lambda Ei : ((a.spin(R) - b) * (2 * Ei.comm(a.spin(R))))
d(E12)
Hi(E12,a,b,R)
def grad(a,b,R):
    a = vsr.CGA(a)
    b = vsr.CGA(b)
    R = vsr.CGA(R)
    E12 = vsr.EGA(vsr.Biv(1,0,0)) 
    E13 = vsr.EGA(vsr.Biv(0,1,0))
    E23 = vsr.EGA(vsr.Biv(0,0,1)) 
    d = lambda Ei : ((a.spin(R) - b) * (2 * Ei.comm(a.spin(R))))[0]
    return np.array([d(E12), d(E13), d(E23)])<jupyter_output><empty_output>
{'fraction_non_alphanumeric': 0.2358928966585528, 'fraction_numerical': 0.06771409603894667, 'mean_word_length': 2.6999590667212443, 'pattern_counts': {'":': 0, '<': 11, '<?xml version=': 0, '>': 10, 'https://': 0, 'lorem ipsum': 0, 'www.': 0, 'xml': 0}, 'pii_count': 0, 'substrings_counts': 0, 'word_list_counts': {'cursed_substrings.json': 3, 'profanity_word_list.json': 0, 'sexual_word_list.json': 0, 'zh_pornsignals.json': 0}}
{'chain_length': '2', 'dir': 'jupyter-structured-clean-dedup', 'id': '648853', 'license': 'BSD-3-Clause', 'n_tokens_mistral': 4913, 'n_tokens_neox': 4703, 'n_words': 671, 'path': 'python/AnalyticJacobianRotorEstimation.ipynb', 'repo_name': 'tingelst/game', 'size': '9038'}