def hand_eye_calib(q_gt, q_es, p_gt, p_es, I, delta=10, verbose=True):
"""Implementation of the least squares solution described in the paper:
Robot Sensor Calibration: Solving AX=XB on the Euclidean Group
by Frank C. Park and Bryan J. Martin
"""
n = np.shape(I)[0]
M = np.zeros([3, 3])
C = np.zeros([3 * n, 3])
b_A = np.zeros([3 * n, 1])
b_B = np.zeros([3 * n, 1])
for ix, i in enumerate(I):
A1 = transformations.quaternion_matrix(q_es[i, :])[:3, :3]
A2 = transformations.quaternion_matrix(q_es[i + delta, :])[:3, :3]
A = np.dot(A1.transpose(), A2)
B1 = transformations.quaternion_matrix(q_gt[i, :])[:3, :3]
B2 = transformations.quaternion_matrix(q_gt[i + delta, :])[:3, :3]
B = np.dot(B1.transpose(), B2)
alpha = _matrix_log(A)
beta = _matrix_log(B)
M = M + np.dot(np.matrix(beta).transpose(), np.matrix(alpha))
C[3 * ix:3 * ix + 3, :] = np.eye(3) - A
b_A[3 * ix:3 * ix + 3, 0] = np.dot(np.transpose(A1),
p_es[i + delta, :] - p_es[i, :])
b_B[3 * ix:3 * ix + 3, 0] = np.dot(np.transpose(B1),
p_gt[i + delta, :] - p_gt[i, :])
# compute rotation
D, V = np.linalg.linalg.eig(np.dot(M.transpose(), M))
Lambda = np.diag(
[np.sqrt(1.0 / D[0]),
np.sqrt(1.0 / D[1]),
np.sqrt(1.0 / D[2])])
Vinv = np.linalg.linalg.inv(V)
X = np.dot(V, np.dot(Lambda, np.dot(Vinv, M.transpose())))
# compute translation
d = np.zeros([3 * n, 1])
for i in range(n):
d[3 * i:3 * i +
3, :] = b_A[3 * i:3 * i + 3, :] - np.dot(X, b_B[3 * i:3 * i + 3, :])
b = np.dot(np.linalg.inv(np.dot(np.transpose(C), C)),
np.dot(np.transpose(C), d))
return np.array(X), b
def hand_eye_calib(q_gt, q_es, p_gt, p_es, I, delta=10, verbose=True):
"""Implementation of the least squares solution described in the paper:
Robot Sensor Calibration: Solving AX=XB on the Euclidean Group
by Frank C. Park and Bryan J. Martin
"""
n = np.shape(I)[0]
M = np.zeros([3, 3])
C = np.zeros([3 * n, 3])
b_A = np.zeros([3 * n, 1])
b_B = np.zeros([3 * n, 1])
for ix, i in enumerate(I):
A1 = transformations.quaternion_matrix(q_es[i, :])[:3, :3]
A2 = transformations.quaternion_matrix(q_es[i + delta, :])[:3, :3]
A = np.dot(A1.transpose(), A2)
B1 = transformations.quaternion_matrix(q_gt[i, :])[:3, :3]
B2 = transformations.quaternion_matrix(q_gt[i + delta, :])[:3, :3]
B = np.dot(B1.transpose(), B2)
alpha = _matrix_log(A)
beta = _matrix_log(B)
M = M + np.dot(np.matrix(beta).transpose(), np.matrix(alpha))
C[3 * ix : 3 * ix + 3, :] = np.eye(3) - A
b_A[3 * ix : 3 * ix + 3, 0] = np.dot(np.transpose(A1), p_es[i + delta, :] - p_es[i, :])
b_B[3 * ix : 3 * ix + 3, 0] = np.dot(np.transpose(B1), p_gt[i + delta, :] - p_gt[i, :])
# compute rotation
D, V = np.linalg.linalg.eig(np.dot(M.transpose(), M))
Lambda = np.diag([np.sqrt(1.0 / D[0]), np.sqrt(1.0 / D[1]), np.sqrt(1.0 / D[2])])
Vinv = np.linalg.linalg.inv(V)
X = np.dot(V, np.dot(Lambda, np.dot(Vinv, M.transpose())))
# compute translation
d = np.zeros([3 * n, 1])
for i in range(n):
d[3 * i : 3 * i + 3, :] = b_A[3 * i : 3 * i + 3, :] - np.dot(X, b_B[3 * i : 3 * i + 3, :])
b = np.dot(np.linalg.inv(np.dot(np.transpose(C), C)), np.dot(np.transpose(C), d))
return np.array(X), b
def get_rigid_body_trafo(quat,trans):
T = transformations.quaternion_matrix(quat)
T[0:3,3] = trans
return T
def get_rigid_body_trafo(quat, trans):
T = transformations.quaternion_matrix(quat)
T[0:3, 3] = trans
return T
# hand-eye-calib # select random measurements I = np.array(np.random.rand(n_measurements,1)*(np.shape(matches)[0]-delta), dtype=int)[:,0] R,b = align_trajectory.hand_eye_calib(q_gt, q_es, p_gt, p_es, I, delta, True) print 'quat = ' + str(transformations.quaternion_from_matrix(transformations.convert_3x3_to_4x4(R))) print 'b = ' + str(b) rpy_es = np.zeros([q_es.shape[0]-1, 3]) rpy_gt = np.zeros([q_gt.shape[0]-1, 3]) t_gt = np.zeros([q_es.shape[0]-1,3]) t_es = np.zeros([q_es.shape[0]-1,3]) for i in range(delta,np.shape(q_es)[0]): A1 = transformations.quaternion_matrix(q_es[i-delta,:])[:3,:3] A2 = transformations.quaternion_matrix(q_es[i,:])[:3,:3] A = np.dot(A1.transpose(), A2) B1 = transformations.quaternion_matrix(q_gt[i-delta,:])[:3,:3] B2 = transformations.quaternion_matrix(q_gt[i,:])[:3,:3] B = np.dot(B1.transpose(), B2) B_es = np.dot(np.transpose(R), np.dot(A, R)) rpy_gt[i-delta,:] = transformations.euler_from_matrix(B, 'rzyx') rpy_es[i-delta,:] = transformations.euler_from_matrix(B_es, 'rzyx') t_B = np.dot(np.transpose(B1),(p_gt[i,:]-p_gt[i-delta,:])) t_A = np.dot(np.transpose(A1),(p_es[i,:]-p_es[i-delta,:])) t_gt[i-delta,:] = t_B t_es[i-delta,:] = np.dot(np.transpose(R), np.dot(A,b[:,0]) + t_A - b[:,0]) alignment_error = (t_gt-t_es) error = np.sqrt(np.sum(np.multiply(alignment_error,alignment_error),1))
