def compute_initial_guess_for_all_pairs(set_of_pose_pairs, algorithm_name, hand_eye_config,
filtering_config, optimization_config, visualize=False):
"""
Iterate over all pairs and compute an initial guess for the calibration (both time and
transformation). Also store the experiment results from this computation for each pair.
"""
set_of_dq_H_E_initial_guess = []
set_of_time_offset_initial_guess = []
# Initialize the result entry.
result_entry = ResultEntry()
result_entry.init_from_configs(algorithm_name, 0, filtering_config,
hand_eye_config, optimization_config)
for (pose_file_B_H, pose_file_W_E) in set_of_pose_pairs:
print("\n\nCompute initial guess for calibration between \n\t{} \n and \n\t{} \n\n".format(
pose_file_B_H, pose_file_W_E))
(time_stamped_poses_B_H,
times_B_H,
quaternions_B_H
) = read_time_stamped_poses_from_csv_file(pose_file_B_H)
print("Found ", time_stamped_poses_B_H.shape[0],
" poses in file: ", pose_file_B_H)
(time_stamped_poses_W_E,
times_W_E,
quaternions_W_E) = read_time_stamped_poses_from_csv_file(pose_file_W_E)
print("Found ", time_stamped_poses_W_E.shape[0],
" poses in file: ", pose_file_W_E)
# No time offset.
time_offset_initial_guess = 0.
# Unit DualQuaternion.
dq_H_E_initial_guess = DualQuaternion.from_vector(
[0., 0., 0., 1.0, 0., 0., 0., 0.])
print("Computing time offset...")
time_offset_initial_guess = calculate_time_offset(times_B_H, quaternions_B_H, times_W_E,
quaternions_W_E, filtering_config,
args.visualize)
print("Time offset: {}s".format(time_offset_initial_guess))
print("Computing aligned poses...")
(aligned_poses_B_H, aligned_poses_W_E) = compute_aligned_poses(
time_stamped_poses_B_H, time_stamped_poses_W_E, time_offset_initial_guess, visualize)
# Convert poses to dual quaterions.
dual_quat_B_H_vec = [DualQuaternion.from_pose_vector(
aligned_pose_B_H) for aligned_pose_B_H in aligned_poses_B_H[:, 1:]]
dual_quat_W_E_vec = [DualQuaternion.from_pose_vector(
aligned_pose_W_E) for aligned_pose_W_E in aligned_poses_W_E[:, 1:]]
assert len(dual_quat_B_H_vec) == len(dual_quat_W_E_vec), ("len(dual_quat_B_H_vec): {} "
"vs len(dual_quat_W_E_vec): {}"
).format(len(dual_quat_B_H_vec),
len(dual_quat_W_E_vec))
dual_quat_B_H_vec = align_paths_at_index(dual_quat_B_H_vec)
dual_quat_W_E_vec = align_paths_at_index(dual_quat_W_E_vec)
# Draw both paths in their Global / World frame.
if visualize:
poses_B_H = np.array([dual_quat_B_H_vec[0].to_pose().T])
poses_W_E = np.array([dual_quat_W_E_vec[0].to_pose().T])
for i in range(1, len(dual_quat_B_H_vec)):
poses_B_H = np.append(poses_B_H, np.array(
[dual_quat_B_H_vec[i].to_pose().T]), axis=0)
poses_W_E = np.append(poses_W_E, np.array(
[dual_quat_W_E_vec[i].to_pose().T]), axis=0)
every_nth_element = args.plot_every_nth_pose
plot_poses([poses_B_H[:: every_nth_element], poses_W_E[:: every_nth_element]],
True, title="3D Poses Before Alignment")
print("Computing hand-eye calibration to obtain an initial guess...")
if hand_eye_config.use_baseline_approach:
(success, dq_H_E_initial_guess, rmse,
num_inliers, num_poses_kept,
runtime, singular_values, bad_singular_values) = compute_hand_eye_calibration_BASELINE(
dual_quat_B_H_vec, dual_quat_W_E_vec, hand_eye_config)
else:
(success, dq_H_E_initial_guess, rmse,
num_inliers, num_poses_kept,
runtime, singular_values, bad_singular_values) = compute_hand_eye_calibration_RANSAC(
dual_quat_B_H_vec, dual_quat_W_E_vec, hand_eye_config)
result_entry.success.append(success)
result_entry.num_initial_poses.append(len(dual_quat_B_H_vec))
result_entry.num_poses_kept.append(num_poses_kept)
result_entry.runtimes.append(runtime)
result_entry.singular_values.append(singular_values)
result_entry.bad_singular_value.append(bad_singular_values)
result_entry.dataset_names.append((pose_file_B_H, pose_file_W_E))
set_of_dq_H_E_initial_guess.append(dq_H_E_initial_guess)
set_of_time_offset_initial_guess.append(time_offset_initial_guess)
return (set_of_dq_H_E_initial_guess,
set_of_time_offset_initial_guess, result_entry)
def compute_hand_eye_calibration(dq_B_H_vec_inliers,
dq_W_E_vec_inliers,
scalar_part_tolerance=1e-2,
enforce_same_non_dual_scalar_sign=True):
"""
Do the actual hand eye-calibration as described in the referenced paper.
Assumes the outliers have already been removed and the scalar parts of
each pair are a match.
"""
n_quaternions = len(dq_B_H_vec_inliers)
# Verify that the first pose is at the origin.
assert np.allclose(dq_B_H_vec_inliers[0].dq,
[0., 0., 0., 1.0, 0., 0., 0., 0.],
atol=1.e-8), dq_B_H_vec_inliers[0]
assert np.allclose(dq_W_E_vec_inliers[0].dq,
[0., 0., 0., 1.0, 0., 0., 0., 0.],
atol=1.e-8), dq_W_E_vec_inliers[0]
if enforce_same_non_dual_scalar_sign:
for i in range(n_quaternions):
dq_W_E = dq_W_E_vec_inliers[i]
dq_B_H = dq_B_H_vec_inliers[i]
if ((dq_W_E.q_rot.w < 0. and dq_B_H.q_rot.w > 0.)
or (dq_W_E.q_rot.w > 0. and dq_B_H.q_rot.w < 0.)):
dq_W_E_vec_inliers[i].dq = -dq_W_E_vec_inliers[i].dq.copy()
# 0. Stop alignment if there are still pairs that do not have matching
# scalar parts.
for j in range(n_quaternions):
dq_B_H = dq_W_E_vec_inliers[j]
dq_W_E = dq_B_H_vec_inliers[j]
scalar_parts_B_H = dq_B_H.scalar()
scalar_parts_W_E = dq_W_E.scalar()
assert np.allclose(
scalar_parts_B_H.dq,
scalar_parts_W_E.dq,
atol=scalar_part_tolerance), (
"Mismatch of scalar parts of dual quaternion at idx {}:"
" dq_B_H: {} dq_W_E: {}".format(j, dq_B_H, dq_W_E))
# 1.
# Construct 6n x 8 matrix T
t_matrix = setup_t_matrix(dq_B_H_vec_inliers, dq_W_E_vec_inliers)
# 2.
# Compute SVD of T and check if only two singular values are almost equal to
# zero. Take the corresponding right-singular vectors (v_7 and v_8)
U, s, V = np.linalg.svd(t_matrix)
# Check if only the last two singular values are almost zero.
bad_singular_values = False
for i, singular_value in enumerate(s):
if i < 6:
if singular_value < 5e-1:
bad_singular_values = True
else:
if singular_value > 5e-1:
bad_singular_values = True
v_7 = V[6, :].copy()
v_8 = V[7, :].copy()
# print("v_7: {}".format(v_7))
# print("v_8: {}".format(v_8))
# 3.
# Compute the coefficients of (35) and solve it, finding two solutions for s.
u_1 = v_7[0:4].copy()
u_2 = v_8[0:4].copy()
v_1 = v_7[4:8].copy()
v_2 = v_8[4:8].copy()
# print("u_1: {}, \nu_2: {}, \nv_1: {}, \nv_2: {}".format(u_1, u_2, v_1, v_2))
a = np.dot(u_1.T, v_1)
assert a != 0.0, "This would involve division by zero."
b = np.dot(u_1.T, v_2) + np.dot(u_2.T, v_1)
c = np.dot(u_2.T, v_2)
# print("a: {}, b: {}, c: {}".format(a, b, c))
square_root_term = b * b - 4.0 * a * c
if square_root_term < -1e-2:
assert False, "square_root_term is too negative: {}".format(
square_root_term)
if square_root_term < 0.0:
square_root_term = 0.0
s_1 = (-b + np.sqrt(square_root_term)) / (2.0 * a)
s_2 = (-b - np.sqrt(square_root_term)) / (2.0 * a)
# print("s_1: {}, s_2: {}".format(s_1, s_2))
# 4.
# For these two s values, compute s^2*u_1^T*u_1 + 2*s*u_1^T*u_2 + u_2^T*u_2
# From these choose the largest to compute lambda_2 and then lambda_1
solution_1 = s_1 * s_1 * np.dot(u_1.T, u_1) + 2.0 * \
s_1 * np.dot(u_1.T, u_2) + np.dot(u_2.T, u_2)
solution_2 = s_2 * s_2 * np.dot(u_1.T, u_1) + 2.0 * \
s_2 * np.dot(u_1.T, u_2) + np.dot(u_2.T, u_2)
if solution_1 > solution_2:
assert solution_1 > 0.0, solution_1
lambda_2 = np.sqrt(1.0 / solution_1)
lambda_1 = s_1 * lambda_2
else:
assert solution_2 > 0.0, solution_2
lambda_2 = np.sqrt(1.0 / solution_2)
lambda_1 = s_2 * lambda_2
# print("lambda_1: {}, lambda_2: {}".format(lambda_1, lambda_2))
# 5.
# The result is lambda_1*v_7 + lambda_2*v_8
dq_H_E = DualQuaternion.from_vector(lambda_1 * v_7 + lambda_2 * v_8)
# Normalize the output, to get rid of numerical errors.
dq_H_E.normalize()
if (dq_H_E.q_rot.w < 0.):
dq_H_E.dq = -dq_H_E.dq.copy()
return (dq_H_E, s, bad_singular_values)
times_B_H,
quaternions_B_H
) = read_time_stamped_poses_from_csv_file(pose_file_B_H)
print("Found ", time_stamped_poses_B_H.shape[0],
" poses in file: ", pose_file_B_H)
(time_stamped_poses_W_E,
times_W_E,
quaternions_W_E) = read_time_stamped_poses_from_csv_file(pose_file_W_E)
print("Found ", time_stamped_poses_W_E.shape[0],
" poses in file: ", pose_file_W_E)
# No time offset.
time_offset_initial_guess = 0.
# Unit DualQuaternion.
dq_H_E_initial_guess = DualQuaternion.from_vector(
[0., 0., 0., 1.0, 0., 0., 0., 0.])
print("Computing time offset...")
time_offset_initial_guess = calculate_time_offset(times_B_H, quaternions_B_H, times_W_E,
quaternions_W_E, filtering_config, args.visualize)
print("Time offset: {}s".format(time_offset_initial_guess))
print("Computing aligned poses...")
(aligned_poses_B_H, aligned_poses_W_E) = compute_aligned_poses(
time_stamped_poses_B_H, time_stamped_poses_W_E, time_offset_initial_guess, args.visualize)
# Convert poses to dual quaterions.
dual_quat_B_H_vec = [DualQuaternion.from_pose_vector(
aligned_pose_B_H) for aligned_pose_B_H in aligned_poses_B_H[:, 1:]]
dual_quat_W_E_vec = [DualQuaternion.from_pose_vector(
