def evalError(x, inst, s, Ci, idx = [-1]):
if idx[0] != s:
t0 = inst.refFrame.data[s*npar:(s+1)*npar], inst.refFrame.particle_direction[s*npar:(s+1)*npar]
n = len(x)
sumAAT = np.matrix(np.zeros((n, n)))
sumAs = np.matrix(np.zeros(n))
sumSST = 0.0
for fn in range(inst.nFrame):
A = []
frame = inst.data[fn]
guide = inst.guide[fn]
tref = cd.rigid_trans_batch(frame.rigid_motion, t0)
s2 = s - inst.offset
treal = np.array([frame.data[s2*npar:(s2+1)*npar], frame.particle_direction[s2*npar:(s2+1)*npar]])
treal.resize(6*npar)
for g in Ci:
Bg = guide.particle_motions[g]
state = np.array(cd.point_trans_batch(Bg, tref))
state.resize(6*npar)
A.append(state)
A = np.matrix(A)
sumAAT += A*A.T
sumAs += np.matrix(treal)*A.T
sumSST += treal.dot(treal)
inst.cacheMatrices(sumAAT, sumAs, sumSST);
idx[0] = s
else:
sumAAT, sumAs, sumSST = inst.retrieveMatrices()
return (x * sumAAT).dot(x) - 2 * sumAs.dot(x) + sumSST
def strandInterpolation(self, s, fn):
if self.weights[s][0] == None:
Ci = [s]
w = 1.0
else:
Ci = self.weights[s][1]
w = self.weights[s][0]
s0 = (self.refFrame.data[s*npar:(s+1)*npar], self.refFrame.particle_direction[s*npar:(s+1)*npar])
guide = self.guideData[fn]
tref = cd.rigid_trans_batch(guide.rigid_motion, s0)
A = []
for g in Ci:
Bg = guide.particle_motions[g]
state = np.array(cd.point_trans_batch(Bg, tref))
state.resize(6*npar)
A.append(state)
A = np.matrix(A)
return (A.T * np.matrix(w).T).A1