def calibration_t(T_I_C):
"""
:param T_I_C:
:return:
"""
# print T_I_C
homog = ([0, 0, 0, 1])
# w -> i
r_w_i = tf.euler_to_rot(ideal_cam_r[:, 0])
t_w_i = -r_w_i.dot(ideal_cam_t[:, 0])
A_w_i = np.c_[r_w_i, t_w_i]
A_w_i = np.vstack((A_w_i, homog))
# i -> c
r_i_c = tf.euler_to_rot(result_rotation)
A_i_c = np.c_[r_i_c, T_I_C]
A_i_c = np.vstack((A_i_c, homog))
residual = 0
for i in range(0, 8):
# w -> tcp
r_w_tcp = tf.euler_to_rot(snake_r[:, i])
# translation vector expressed in TCP coordinate system
t_w_tcp = -r_w_tcp.dot(snake_t[:, i])
A_w_tcp = np.c_[r_w_tcp, t_w_tcp]
A_w_tcp = np.vstack((A_w_tcp, homog))
# tcp -> m
angles = ([1.5707963267948966, 0, -1.5707963267948966])
r_tcp_m = tf.euler_to_rot(angles)
t_tcp_m = np.zeros(3)
A_tcp_m = np.c_[r_tcp_m, t_tcp_m]
A_tcp_m = np.vstack((A_tcp_m, homog))
# c -> m
r_c_m = tf.quat_to_rot(pose_r[:, i])
t_c_m = -r_c_m.dot(pose_t[:, i])
A_c_m = np.c_[r_c_m, t_c_m]
A_c_m = np.vstack((A_c_m, homog))
# .....
A_w_m_lhs = A_tcp_m.dot(A_w_tcp)
A_w_m_rhs = A_c_m.dot(A_i_c).dot(A_w_i)
t_res = inv(A_w_m_lhs).dot(A_w_m_rhs)
residual += norm(t_res[:, 3])
return residual
def calibration_r(R_I_C):
"""
:param R_I_C: array consists of euler angles (alpha, beta, gamma)
:return: sum of the norm of angles of all observations
"""
r_i_c = tf.euler_to_rot(R_I_C)
r_w_i = tf.euler_to_rot(ideal_cam_r[:, 0])
residual = 0
for i in range(0, 8):
r_w_tcp = tf.euler_to_rot(snake_r[:, i])
angles = ([1.5707963267948966, 0, -1.5707963267948966])
r_tcp_m = tf.euler_to_rot(angles)
r_w_m = r_tcp_m.dot(r_w_tcp)
r_c_m = tf.quat_to_rot(pose_r[:, i])
rhs = r_c_m.dot(r_i_c).dot(r_w_i)
res = inv(r_w_m).dot(rhs)
angles = tf.rot_to_euler(res)
residual += norm(angles)
return residual
