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
# (_, hand_eye_config) = get_baseline_config(True)
hand_eye_config.visualize = args.visualize
hand_eye_config.visualize_plot_every_nth_pose = args.plot_every_nth_pose
if hand_eye_config.use_baseline_approach:
(success, dq_H_E, 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, 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)
if args.extrinsics_output_csv_file is not None:
print("Writing extrinsics to %s." % args.extrinsics_output_csv_file)
from hand_eye_calibration.csv_io import write_double_numpy_array_to_csv_file
write_double_numpy_array_to_csv_file(dq_H_E.to_pose(),
args.extrinsics_output_csv_file)
output_json_calibration = args.calibration_output_json_file is not None
has_time_offset_file = args.time_offset_input_csv_file is not None
if output_json_calibration and has_time_offset_file:
time_offset = float(
readArrayFromCsv(args.time_offset_input_csv_file)[0, 0])
calib = ExtrinsicCalibration(
time_offset, DualQuaternion.from_pose_vector(dq_H_E.to_pose()))
calib.writeJson(args.calibration_output_json_file)
def run_optimization_experiment(time_offset_range, angle_offset_range,
translation_offset_range, iteration_idx,
result_entry_template, experiment_progress):
result_entry = copy.deepcopy(result_entry_template)
result_entry.iteration_num = iteration_idx
# Init result variables.
results_dq_H_E = []
results_poses_H_E = []
pose_pair_idx = 0
# Perform optimization on all pairs.
for ((pose_file_B_H, pose_file_W_E),
(is_absolute_sensor_B_H, is_absolute_sensor_W_E),
dq_H_E_initial_guess,
time_offset_initial_guess) in zip(set_of_pose_pairs,
set_of_is_absolute_sensor_flags,
set_of_dq_H_E_initial_guess,
set_of_time_offset_initial_guess):
print(("\n\nCompute optimized hand-eye calibration between \n\t{} (absolute: {})\n " +
"and \n\t{} (absolute: {})\n\n").format(
pose_file_B_H, is_absolute_sensor_B_H, pose_file_W_E, is_absolute_sensor_W_E))
# Load data.
(time_stamped_poses_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, _,
_) = 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)
# Spoil initial guess, for current pair.
(time_offset_initial_guess_spoiled, dq_H_E_initial_guess_spoiled,
(random_translation_offset, random_angle_offset,
random_time_offset_offset)) = spoil_initial_guess(
time_offset_initial_guess, dq_H_E_initial_guess,
angle_offset_range,
translation_offset_range,
time_offset_range)
print(("Spoiled initial guess: time offset: {} angle offset: {} " +
"translation offset: {}").format(
random_time_offset_offset, random_angle_offset, random_translation_offset))
# Save offsets from initial guess.
result_entry.spoiled_initial_guess_angle_offset.append(
random_angle_offset)
result_entry.spoiled_initial_guess_translation_offset.append(
random_translation_offset)
result_entry.spoiled_initial_guess_time_offset.append(
random_time_offset_offset)
# Write calibration to optimization input file format.
initial_guess_calibration_file = ("{}/optimization/{}_run_{}_it_{}_pose_pair_{}_" +
"dt_{}_ds_{}_da_{}_" +
"init_guess.json"
).format(args.result_directory,
algorithm_name,
experiment_progress[0],
iteration,
pose_pair_idx,
random_time_offset_offset,
np.linalg.norm(
random_translation_offset),
random_angle_offset)
create_path(initial_guess_calibration_file)
initial_guess = ExtrinsicCalibration(
time_offset_initial_guess_spoiled, dq_H_E_initial_guess_spoiled)
initial_guess.writeJson(initial_guess_calibration_file)
# Init optimization result
time_offset_optimized = None
dq_H_E_optimized = None
optimization_success = True
rmse_optimized = (-1, -1)
num_inliers_optimized = 0
model_config_string = ("pose1/absoluteMeasurements={},"
"pose2/absoluteMeasurements={}").format(
is_absolute_sensor_B_H, is_absolute_sensor_W_E).lower()
# Prepare output file path and folder.
optimized_calibration_file = ("{}/optimization/{}_run_{}_it_{}_pose_pair_{}_" +
"dt_{}_ds_{}_da_{}_" +
"optimized.json").format(args.result_directory,
algorithm_name,
experiment_progress[0],
iteration,
pose_pair_idx,
random_time_offset_offset,
np.linalg.norm(
random_translation_offset),
random_angle_offset)
create_path(optimized_calibration_file)
# Run the optimization.
optimization_runtime = 0.
try:
optimization_start_time = timeit.default_timer()
run("rosrun hand_eye_calibration_batch_estimation batch_estimator -v 1 \
--pose1_csv=%s --pose2_csv=%s \
--init_guess_file=%s \
--output_file=%s \
--model_config=%s" % (pose_file_B_H, pose_file_W_E,
initial_guess_calibration_file,
optimized_calibration_file,
model_config_string))
optimization_end_time = timeit.default_timer()
optimization_runtime = optimization_end_time - optimization_start_time
except Exception as ex:
print("Optimization failed: {}".format(ex))
optimization_success = False
# If the optimization was successful, evaluate it.
if optimization_success:
optimized_calibration = ExtrinsicCalibration.fromJson(
optimized_calibration_file)
dq_H_E_optimized = optimized_calibration.pose_dq
time_offset_optimized = optimized_calibration.time_offset
print("Initial guess time offset: \t{}".format(
time_offset_initial_guess))
print("Optimized time offset: \t\t{}".format(
time_offset_optimized))
print("Initial guess dq_H_E: \t\t{}".format(dq_H_E_initial_guess))
print("Optimized dq_H_E: \t\t{}".format(dq_H_E_optimized))
(rmse_optimized,
num_inliers_optimized) = evaluate_calibration(time_stamped_poses_B_H,
time_stamped_poses_W_E,
dq_H_E_optimized,
time_offset_optimized,
hand_eye_config)
if num_inliers_optimized == 0:
print("Could not evaluate calibration, no matching poses found!")
optimization_success = False
else:
print("Solution found by optimization\n"
"\t\tNumber of inliers: {}\n"
"\t\tRMSE position: {:10.4f}\n"
"\t\tRMSE orientation: {:10.4f}".format(num_inliers_optimized,
rmse_optimized[0],
rmse_optimized[1]))
# Store results.
if dq_H_E_optimized is not None:
results_poses_H_E.append(dq_H_E_optimized.to_pose())
else:
results_poses_H_E.append(None)
results_dq_H_E.append(dq_H_E_optimized)
result_entry.optimization_success.append(optimization_success)
result_entry.rmse.append(rmse_optimized)
result_entry.num_inliers.append(num_inliers_optimized)
result_entry.optimization_runtime.append(optimization_runtime)
pose_pair_idx = pose_pair_idx + 1
# Verify results.
assert len(results_dq_H_E) == num_pose_pairs
assert len(results_poses_H_E) == num_pose_pairs
result_entry.check_length(num_pose_pairs)
# Computing the loop error only makes sense if all pose pairs are successfully calibrated.
# If optimization is disabled, optimization_success should always be
# True.
if sum(result_entry.optimization_success) == num_pose_pairs:
(result_entry.loop_error_position,
result_entry.loop_error_orientation) = compute_loop_error(results_dq_H_E,
num_poses_sets,
args.visualize)
else:
print("Error: No loop error computed because not " +
"all pose pairs were successfully calibrated!")
print("\n\nFINISHED EXPERIMENT {}/{}\n\n".format(experiment_progress[0],
experiment_progress[1]))
return (result_entry, results_dq_H_E, results_poses_H_E)
runtime, singular_values, bad_singular_values) = compute_hand_eye_calibration_RANSAC(
dual_quat_B_H_vec, dual_quat_W_E_vec, hand_eye_config)
# Fill in result entries.
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))
# Run optimization if enabled.
if optimization_config.enable_optimization:
# Write initial guess to file for the optimization.
initial_guess = ExtrinsicCalibration(
time_offset_initial_guess, dq_H_E_initial_guess)
initial_guess.writeJson(initial_guess_calibration_file)
optimized_calibration_file = "{}/optimization/{}_pose_pair_{}_it_{}_optimized.json".format(
args.result_directory, algorithm_name, pose_pair_idx, iteration)
create_path(optimized_calibration_file)
# Init optimization result
time_offset_optimized = None
dq_H_E_optimized = None
optimization_success = True
rmse_optimized = (-1, -1)
num_inliers_optimized = 0
model_config_string = ("pose1/absoluteMeasurements={},"
"pose2/absoluteMeasurements={}").format(