def calc_R6P_eax_err_penalize_motion(M, data):
weight = 1000000
n = data.shape[1]
w = M[0:3].reshape(3, 1)
t = M[3:6].reshape(3, 1)
v = M[6:9].reshape(3, 1)
C = M[9:12].reshape(3, 1)
u = data[3:5, :]
X = data[0:3, :]
col = u[0, :]
r = np.tile(col, (3, 1))
Rw = eax2RforRS(w, col * np.linalg.norm(w))
Rv = rot.eax2R(v)
Rt = Rw.dot(Rv)
Xrep = np.tile(X, (3, 1))
RX = np.sum(Rt.transpose() * Xrep.ravel(order='F').reshape(
(3, 3 * n), order='F'),
axis=0).reshape((3, n), order='F')
Z = RX + np.tile(C, (1, n)) + r * np.tile(t, (1, n))
u_rs = ut.h2a(Z)
err = np.hstack(((u_rs - u).ravel(),
np.abs(w.ravel() * weight,
np.abs(t.ravel()) * weight)))
return err
def calc_R6P_2lin_err(M, data):
w = M[0:3].reshape(3, 1)
t = M[3:6].reshape(3, 1)
v = M[6:9].reshape(3, 1)
C = M[9:12].reshape(3, 1)
err = np.zeros(data.shape[1])
i = 0
for temp in data.transpose():
X = temp[0:3, None]
u = temp[3:5, None]
proj = ut.h2a(
np.matmul((np.eye(3) + u[0] * ut.x_(w)
), np.matmul((np.eye(3) + ut.x_(v)), X)) + C + u[0] * t)
#print(proj.shape)
err[i] = np.linalg.norm(proj - u)
i += 1
return err
def proj_R6P_eax(M, X, d):
w = M[0:3].reshape(3, 1)
t = M[3:6].reshape(3, 1)
v = M[6:9].reshape(3, 1)
C = M[9:12].reshape(3, 1)
u = np.array((0, 0)).reshape(2, 1)
projs = []
for XX in X.transpose():
for i in range(50):
XX = XX.reshape(3, 1)
unew = ut.h2a(
np.matmul(rot.eax2R(u[d] * w), np.matmul(rot.eax2R(v), XX)) +
C + u[d] * t)
if (np.linalg.norm(unew - u) < 1e-8):
u = unew
break
u = unew
projs.append(u)
return np.squeeze(np.array(projs), axis=2).transpose()
def calc_R6P_eax_err_for_lsq(M, data):
n = data.shape[1]
w = M[0:3].reshape(3, 1)
t = M[3:6].reshape(3, 1)
v = M[6:9].reshape(3, 1)
C = M[9:12].reshape(3, 1)
u = data[3:5, :]
X = data[0:3, :]
col = u[0, :]
r = np.tile(col, (3, 1))
Rw = eax2RforRS(w, col * np.linalg.norm(w))
Rv = rot.eax2R(v)
Rt = Rw.dot(Rv)
Xrep = np.tile(X, (3, 1))
RX = np.sum(Rt.transpose() * Xrep.ravel(order='F').reshape(
(3, 3 * n), order='F'),
axis=0).reshape((3, n), order='F')
Z = RX + np.tile(C, (1, n)) + r * np.tile(t, (1, n))
u_rs = ut.h2a(Z)
err = (np.sqrt(np.sum((u_rs - u)**2, axis=0))).ravel()
return err