def evaluate_calibration(time_stamped_poses_B_H, time_stamped_poses_W_E,
dq_H_E, time_offset, config):
assert len(time_stamped_poses_B_H) > 0
assert len(time_stamped_poses_W_E) > 0
assert isinstance(config, HandEyeConfig)
(aligned_poses_B_H,
aligned_poses_W_E) = compute_aligned_poses(time_stamped_poses_B_H,
time_stamped_poses_W_E,
time_offset)
assert len(aligned_poses_B_H) == len(aligned_poses_W_E)
# If we found no matching poses, the evaluation failed.
if len(aligned_poses_B_H) == 0:
return ((float('inf'), float('inf')), 0)
# 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))
aligned_dq_B_H = align_paths_at_index(dual_quat_B_H_vec, 0)
aligned_dq_W_E = align_paths_at_index(dual_quat_W_E_vec, 0)
(poses_B_H, poses_W_H) = get_aligned_poses(aligned_dq_B_H, aligned_dq_W_E,
dq_H_E)
(rmse_position, rmse_orientation,
inlier_flags) = evaluate_alignment(poses_B_H, poses_W_H, config,
config.visualize)
if config.visualize:
every_nth_element = config.visualize_plot_every_nth_pose
plot_poses(
[poses_B_H[::every_nth_element], poses_W_H[::every_nth_element]],
True,
title="3D Poses After Fine Alignment")
return ((rmse_position, rmse_orientation), sum(inlier_flags))
def test_conversions(self):
pose = [1, 2, 3, 1., 0., 0., 0.]
dq = DualQuaternion.from_pose_vector(pose)
pose_out = dq.to_pose()
matrix_out = dq.to_matrix()
matrix_expected = np.array([[1, 0, 0, 1], [0, -1, 0, 2], [0, 0, -1, 3],
[0, 0, 0, 1]])
npt.assert_allclose(pose, pose_out)
npt.assert_allclose(matrix_out, matrix_expected)
def fromJson(cls, in_file, switchConvention = False):
with open(in_file, 'r') as f:
data = json.load(f)
p = [ float(data['translation'][name]) for name in 'xyz' ]
q = np.array([ float(data['rotation'][name]) for name in 'ijkw' ])
if switchConvention:
q[:3] *= -1.0
dq = DualQuaternion.from_pose_vector(np.hstack((p, q)))
return ExtrinsicCalibration(float(data['delay']), dq)
def test_dq_to_matrix(self):
pose = [1, 2, 3, 4., 5., 6., 7.]
dq = DualQuaternion.from_pose_vector(pose)
dq.normalize()
matrix_out = dq.to_matrix()
dq_from_matrix = DualQuaternion.from_transformation_matrix(matrix_out)
matrix_out_2 = dq_from_matrix.to_matrix()
npt.assert_allclose(dq.dq, dq_from_matrix.dq)
npt.assert_allclose(matrix_out, matrix_out_2)
def apply_hand_eye_calibration(trajectory_G_I, marker_calibration_file):
"""Returns the transformed estimator trajectory.
Computes the trajectory of the marker frame, given the trajectory if the eye
and the hand_eye_calibration output.
"""
assert os.path.isfile(marker_calibration_file)
# Frames of reference: M = marker (hand), I = IMU (eye).
dq_I_M = ExtrinsicCalibration.fromJson(
marker_calibration_file).pose_dq.inverse()
trajectory_G_M = trajectory_G_I
for idx in range(trajectory_G_M.shape[0]):
dq_G_I = DualQuaternion.from_pose_vector(trajectory_G_I[idx, 1:8])
dq_G_M = dq_G_I * dq_I_M
trajectory_G_M[idx, 1:8] = dq_G_M.to_pose()
return trajectory_G_M
def calibrateTwo(group, a_in, b_in, a, b):
if not os.path.exists(group):
os.mkdir(group)
a_aligned = group + '/' + a + "_aligned.csv"
b_aligned = group + '/' + b + "_aligned.csv"
time_offset_file = group + '/' + 'time_offset.csv'
pose_file = group + '/' + 'pose.csv'
if requiresUpdate([a_in, b_in], [a_aligned, b_aligned, time_offset_file]):
run(
"rosrun hand_eye_calibration compute_aligned_poses.py \
--poses_B_H_csv_file %s \
--poses_W_E_csv_file %s \
--aligned_poses_B_H_csv_file %s \
--aligned_poses_W_E_csv_file %s \
--time_offset_output_csv_file %s" %
(a_in, b_in, a_aligned, b_aligned, time_offset_file), DRY_RUN)
if requiresUpdate([a_aligned, b_aligned], [pose_file]):
run(
"rosrun hand_eye_calibration compute_hand_eye_calibration.py \
--aligned_poses_B_H_csv_file %s \
--aligned_poses_W_E_csv_file %s \
--extrinsics_output_csv_file %s" % (a_aligned, b_aligned, pose_file),
DRY_RUN)
init_guess_file = group + "_init_guess.json"
if requiresUpdate([time_offset_file, pose_file], [init_guess_file]):
time_offset = float(readArrayFromCsv(time_offset_file)[0, 0])
pose = readArrayFromCsv(pose_file).reshape((7, ))
calib = ExtrinsicCalibration(time_offset,
DualQuaternion.from_pose_vector(pose))
calib.writeJson(init_guess_file)
calib_file = group + ".json"
if requiresUpdate([a_in, b_in, init_guess_file], [calib_file]):
run(
"rosrun hand_eye_calibration_batch_estimation batch_estimator -v 1 \
--pose1_csv=%s --pose2_csv=%s \
--init_guess_file=%s \
--output_file=%s" % (a_in, b_in, init_guess_file, calib_file), DRY_RUN)
cal = ExtrinsicCalibration.fromJson(calib_file)
cal_init = ExtrinsicCalibration.fromJson(init_guess_file)
print(cal_init, "->\n", cal)
return cal, cal_init
def test_conversions(self):
poses = [[1, 2, 3, 1.0, 0., 0., 0.],
[1, -2, -3, -0.5 * math.sqrt(2), 0., 0., -0.5 * math.sqrt(2)]]
expected_matrices = [
np.array([[1, 0, 0, 1], [0, -1, 0, 2], [0, 0, -1, 3], [0, 0, 0,
1]]),
np.array([[1, 0, 0, 1], [0, 0, -1, -2], [0, 1, 0, -3],
[0, 0, 0, 1]])
]
for idx in range(len(poses)):
pose = poses[idx]
matrix_expected = expected_matrices[idx]
dq = DualQuaternion.from_pose_vector(pose)
pose_out = dq.to_pose()
matrix_out = dq.to_matrix()
np.set_printoptions(suppress=True)
npt.assert_allclose(pose[0:3], pose_out[0:3])
# Check both quaternions which describe the same rotation.
if not np.allclose(pose[3:7], pose_out[3:7]):
npt.assert_allclose(pose[3:7], -pose_out[3:7])
npt.assert_allclose(matrix_out, matrix_expected, atol=1e-6)
"Provide either poses_W_E or poses_E_W!"
if args.calibration_output_json_file is not None:
assert args.time_offset_input_csv_file is not None, (
"In order to compose a complete calibration result json file, you "
+
"need to provide the time alignment csv file as an input using " +
"this flag: --time_offset_input_csv_file")
if use_poses_B_H:
with open(args.aligned_poses_B_H_csv_file, 'r') as csvfile:
poses1_reader = csv.reader(csvfile, delimiter=',', quotechar='|')
poses1 = np.array(list(poses1_reader))
poses1 = poses1.astype(float)
dual_quat_B_H_vec = [
DualQuaternion.from_pose_vector(pose[1:]) for pose in poses1
]
else:
with open(args.aligned_poses_H_B_csv_file, 'r') as csvfile:
poses1_reader = csv.reader(csvfile, delimiter=',', quotechar='|')
poses1 = np.array(list(poses1_reader))
poses1 = poses1.astype(float)
dual_quat_B_H_vec = [
DualQuaternion.from_pose_vector(pose[1:]).inverse()
for pose in poses1
]
if use_poses_W_E:
with open(args.aligned_poses_W_E_csv_file, 'r') as csvfile:
poses2_reader = csv.reader(csvfile, delimiter=',', quotechar='|')
poses2 = np.array(list(poses2_reader))
use_poses_H_B = (args.aligned_poses_H_B_csv_file is not None)
use_poses_W_E = (args.aligned_poses_W_E_csv_file is not None)
use_poses_E_W = (args.aligned_poses_E_W_csv_file is not None)
assert use_poses_B_H != use_poses_H_B, \
"Provide either poses_B_H or poses_H_B!"
assert use_poses_W_E != use_poses_E_W, \
"Provide either poses_W_E or poses_E_W!"
if use_poses_B_H:
with open(args.aligned_poses_B_H_csv_file, 'r') as csvfile:
poses1_reader = csv.reader(csvfile, delimiter=',', quotechar='|')
poses1 = np.array(list(poses1_reader))
poses1 = poses1.astype(float)
dual_quat_B_H_vec = [
DualQuaternion.from_pose_vector(pose[1:]) for pose in poses1
]
else:
with open(args.aligned_poses_H_B_csv_file, 'r') as csvfile:
poses1_reader = csv.reader(csvfile, delimiter=',', quotechar='|')
poses1 = np.array(list(poses1_reader))
poses1 = poses1.astype(float)
dual_quat_B_H_vec = [
DualQuaternion.from_pose_vector(pose[1:]).inverse()
for pose in poses1
]
if use_poses_W_E:
with open(args.aligned_poses_W_E_csv_file, 'r') as csvfile:
poses2_reader = csv.reader(csvfile, delimiter=',', quotechar='|')
poses2 = np.array(list(poses2_reader))
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)
# 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(
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 args.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(
