def evalErrorGroundTruth(w, Rs, Ts, s0, gt):
""" w: 1 x nGuide, R/Ts: nGuide * R/t """
n = len(w)
Rs = np.matrix(Rs.reshape(n, -1))
Ts = np.matrix(Ts.reshape(n, -1))
R = (w*Rs).reshape((3,3))
T = (w*Ts).A1
s1 = cd.point_trans(s0, R, T)
diff = np.array(s1 - gt).flatten()
return diff.dot(diff)
def estimateWeight(sf, BgsfR, Bgsft, constrain):
"""sf: ndarray, nframe * ({pi}, {ti}), exclude 0, so n=factor-1
BgsfR/t: tuple, n * nFrame * {R/t}"""
lambda1 = 1.0
nFrame = sf.shape[0]
nGuide = BgsfR.shape[0]
#print nFrame, nGuide
pmat_square = np.matrix(np.zeros((nGuide, nGuide)))
tmat_square = np.matrix(np.zeros((nGuide, nGuide)))
plinearmat = np.matrix(np.zeros(nGuide))
tlinearmat = np.matrix(np.zeros(nGuide))
pcmat_square = 0.0
tcmat_square = 0.0
for i in range(1, nFrame):
pc, tc = sf[i]
Rs = BgsfR[:, i, ...]
Ts = Bgsft[:, i, ...]
pos_, dir_ = cd.point_trans(sf[0], Rs, Ts, batch=True) # pos_, dir_ are flattened 1 x 3nGuide
posMat = np.matrix(pos_.reshape(nGuide, 3))
dirMat = np.matrix(dir_.reshape(nGuide, 3))
pmat_square += posMat * posMat.T
tmat_square += dirMat * dirMat.T
plinearmat += pc * posMat.T
tlinearmat += tc * dirMat.T
pcmat_square += pc.dot(pc)
tcmat_square += tc.dot(tc)
mats = (pmat_square, plinearmat.A1, pcmat_square, tmat_square, tlinearmat.A1, tcmat_square)
initx = np.array([1.0 / nGuide] * nGuide)
res = minimize(evalError, initx, args=(mats, lambda1), jac=evalDerive,
options={'disp': False, 'ftol':1e-12, 'maxiter':100}, method='SLSQP', constraints=constrain, )
error = evalError(res.x,mats, lambda1)
# error = 0.0
# for i in range(1, nFrame):
# Rs = BgsfR[:, i, ...]
# Ts = Bgsft[:, i, ...]
# error += evalErrorGroundTruth(res.x, Rs, Ts, sf[0], sf[i])
return res, error
