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_rovioli_end_to_end():
config_dir = os.environ["ROVIO_CONFIG_DIR"]
rosbag_local_path = "dataset.bag"
download_url = "http://robotics.ethz.ch/~asl-datasets/maplab/test_data/end_to_end_tests/V1_01_short.bag"
end_to_end_common.download_helpers.download_dataset(
download_url, rosbag_local_path)
ground_truth_data_path = "end_to_end_test/V1_01_easy_ground_truth.csv"
end_to_end_test = end_to_end_common.end_to_end_test.EndToEndTest()
vio_max_allowed_errors = \
end_to_end_common.end_to_end_test.TestErrorStruct(
VIO_MAX_POSITION_RMSE_M, VIO_MAX_POSITION_RMSE_M,
VIO_MAX_ORIENTATION_RMSE_RAD, VIO_MAX_ORIENTATION_RMSE_RAD)
vil_max_allowed_errors = \
end_to_end_common.end_to_end_test.TestErrorStruct(
VIL_MAX_POSITION_RMSE_M, VIL_MAX_POSITION_RMSE_M,
VIL_MAX_ORIENTATION_RMSE_RAD, VIL_MAX_ORIENTATION_RMSE_RAD)
# Run estimator VIO only mode.
estimator_vio_csv_path = "rovioli_estimated_poses_vio.csv"
if RUN_ESTIMATOR_VIO:
bash_utils.run(
"rosrun rovioli rovioli"
" --alsologtostderr"
" --ncamera_calibration=\"%s\""
" --imu_parameters_maplab=\"%s\""
" --imu_parameters_rovio=\"%s\""
" --datasource_type=rosbag"
" --datasource_rosbag=\"%s\""
" --export_estimated_poses_to_csv=\"%s\""
" --rovioli_zero_initial_timestamps=false"
" --rovio_enable_frame_visualization=false" %
(config_dir + "/ncamera-euroc.yaml", config_dir +
"/imu-adis16488.yaml", config_dir + "/imu-sigmas-rovio.yaml",
rosbag_local_path, estimator_vio_csv_path))
# Compare estimator csv - ground truth data.
vio_errors = end_to_end_test.calulate_errors_of_datasets(
"VIO", [vio_max_allowed_errors], estimator_vio_csv_path,
ground_truth_data_path)
# Run estimator VIL mode.
estimator_vil_csv_path = "rovioli_estimated_poses_vil.csv"
localization_reference_map = "V1_01_easy_optimized_summary_map"
if not os.path.isdir(localization_reference_map):
os.mkdir(localization_reference_map)
localization_reference_map_download_path = \
"http://robotics.ethz.ch/~asl-datasets/maplab/test_data/end_to_end_tests/localization_summary_map"
end_to_end_common.download_helpers.download_dataset(
localization_reference_map_download_path,
os.path.join(localization_reference_map, "localization_summary_map"))
if RUN_ESTIMATOR_VIL:
bash_utils.run("rosrun rovioli rovioli"
" --alsologtostderr"
" --ncamera_calibration=\"%s\""
" --imu_parameters_maplab=\"%s\""
" --imu_parameters_rovio=\"%s\""
" --datasource_type=rosbag"
" --datasource_rosbag=\"%s\""
" --export_estimated_poses_to_csv=\"%s\""
" --vio_localization_map_folder=\"%s\""
" --vio_throttler_max_output_frequency_hz=1"
" --rovioli_zero_initial_timestamps=false"
" --rovio_enable_frame_visualization=false" %
(config_dir + "/ncamera-euroc.yaml",
config_dir + "/imu-adis16488.yaml", config_dir +
"/imu-sigmas-rovio.yaml", rosbag_local_path,
estimator_vil_csv_path, localization_reference_map))
# Ensure that VIL position errors are smaller than the VIO position errors.
# For the orientation, we don't want to enforce such a requirement as the
# differences are usually smaller.
vio_position_errors = end_to_end_common.end_to_end_test.TestErrorStruct(
vio_errors.position_mean_m, vio_errors.position_rmse_m,
VIO_MAX_ORIENTATION_RMSE_RAD, VIO_MAX_ORIENTATION_RMSE_RAD)
# Get localizations for VIL case.
vil_localizations = np.genfromtxt(estimator_vil_csv_path,
delimiter=",",
skip_header=1)
vil_localizations = vil_localizations[:, [0, 15]]
vil_localizations = vil_localizations[vil_localizations[:, 1] == 1, :]
vil_errors = end_to_end_test.calulate_errors_of_datasets(
"VIL", [vil_max_allowed_errors, vio_position_errors],
estimator_vil_csv_path,
ground_truth_data_path,
localization_state_list=vil_localizations)
# Get VIWLS errors.
viwls_csv_path = "end_to_end_test/V1_01_easy_short_viwls_vertices.csv"
vil_position_errors = end_to_end_common.end_to_end_test.TestErrorStruct(
vil_errors.position_mean_m, vil_errors.position_rmse_m,
VIL_MAX_ORIENTATION_RMSE_RAD, VIL_MAX_ORIENTATION_RMSE_RAD)
end_to_end_test.calulate_errors_of_datasets(
"VIWLS",
[vio_max_allowed_errors, vio_position_errors, vil_position_errors],
viwls_csv_path,
ground_truth_data_path,
estimator_input_format="maplab_console")
end_to_end_test.print_errors()
end_to_end_test.check_errors()
end_to_end_test.plot_results()
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)
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()
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)