def test_hand_eye_calibration_with_noise(self):
dq_B_H_vec, dq_W_E_vec = he_helpers.generate_test_paths(
20,
self.dq_H_E,
self.dq_B_W,
self.paths_start_at_origin,
include_outliers_B_H=False,
include_noise_B_H=True,
noise_sigma_trans_B_H=0.0001,
noise_sigma_rot_B_H=0.001,
include_outliers_W_E=False,
include_noise_W_E=True,
noise_sigma_trans_W_E=0.0001,
noise_sigma_rot_W_E=0.001)
# Plot the poses with noise.
poses_B_H = np.array([dq_B_H_vec[0].to_pose().T])
poses_W_E = np.array([dq_W_E_vec[0].to_pose().T])
for i in range(1, len(dq_B_H_vec)):
poses_B_H = np.append(poses_B_H,
np.array([dq_B_H_vec[i].to_pose().T]),
axis=0)
poses_W_E = np.append(poses_W_E,
np.array([dq_W_E_vec[i].to_pose().T]),
axis=0)
plot_poses([poses_B_H, poses_W_E], blocking=self.make_plots_blocking)
hand_eye_config = HandEyeConfig()
hand_eye_config.visualize = False
hand_eye_config.ransac_max_number_iterations = 50
hand_eye_config.ransac_sample_size = 3
(success, dq_H_E_estimated, rmse, num_inliers, num_poses_kept, runtime,
singular_values,
bad_singular_values) = compute_hand_eye_calibration_RANSAC(
dq_B_H_vec, dq_W_E_vec, hand_eye_config)
assert success, "Hand-eye calibration, failed!"
pose_H_E_estimated = dq_H_E_estimated.to_pose()
dq_H_E_estimated.normalize()
assert pose_H_E_estimated[6] > 0.0, (
"The real part of the pose's quaternion should be positive. "
"The pose is: \n{}\n where the dual quaternion was: "
"\n{}".format(pose_H_E_estimated, dq_H_E_estimated))
print("The static input pose was: \n{}".format(self.pose_H_E))
print("The hand-eye calibration's output pose is: \n{}".format(
pose_H_E_estimated))
print("T_H_E ground truth: \n{}".format(self.dq_H_E.to_matrix()))
print("T_H_E estimated: \n{}".format(dq_H_E_estimated.to_matrix()))
assert np.allclose(self.dq_H_E.dq, dq_H_E_estimated.dq, atol=4e-2), (
"input dual quaternion: {}, estimated dual quaternion: {}".format(
self.dq_H_E, dq_H_E_estimated))
def test_plot_poses(self):
# Draw both paths in their Global/World frame.
poses_B_H = np.array([self.dq_B_H_vec[0].to_pose().T])
poses_W_E = np.array([self.dq_W_E_vec[0].to_pose().T])
for i in range(1, len(self.dq_B_H_vec)):
poses_B_H = np.append(poses_B_H,
np.array([self.dq_B_H_vec[i].to_pose().T]),
axis=0)
poses_W_E = np.append(poses_W_E,
np.array([self.dq_W_E_vec[i].to_pose().T]),
axis=0)
plot_poses([poses_B_H, poses_W_E], blocking=self.make_plots_blocking)
def compute_loop_error(results_dq_H_E, num_poses_sets, visualize=False):
calibration_transformation_chain = []
# Add point at origin to represent the first coordinate
# frame in the chain of transformations.
calibration_transformation_chain.append(
DualQuaternion(Quaternion(0, 0, 0, 1), Quaternion(0, 0, 0, 0)))
# Add first transformation
calibration_transformation_chain.append(results_dq_H_E[0])
# Create chain of transformations from the first frame to the last.
i = 0
idx = 0
while i < (num_poses_sets - 2):
idx += (num_poses_sets - i - 1)
calibration_transformation_chain.append(results_dq_H_E[idx])
i += 1
# Add inverse of first to last frame to close the loop.
calibration_transformation_chain.append(results_dq_H_E[num_poses_sets -
2].inverse())
# Check loop.
assert len(calibration_transformation_chain) == (num_poses_sets + 1), (
len(calibration_transformation_chain), (num_poses_sets + 1))
# Chain the transformations together to get points we can plot.
poses_to_plot = []
dq_tmp = DualQuaternion(Quaternion(0, 0, 0, 1), Quaternion(0, 0, 0, 0))
for i in range(0, len(calibration_transformation_chain)):
dq_tmp *= calibration_transformation_chain[i]
poses_to_plot.append(dq_tmp.to_pose())
(loop_error_position,
loop_error_orientation) = compute_pose_error(poses_to_plot[0],
poses_to_plot[-1])
print("Error when closing the loop of hand eye calibrations - position: {}"
" m orientation: {} deg".format(loop_error_position,
loop_error_orientation))
if visualize:
assert len(poses_to_plot) == len(calibration_transformation_chain)
plot_poses([np.array(poses_to_plot)],
plot_arrows=True,
title="Hand-Eye Calibration Results - Closing The Loop")
# Compute error of loop.
return (loop_error_position, loop_error_orientation)
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 compute_hand_eye_calibration_RANSAC(dq_B_H_vec, dq_W_E_vec, config):
"""
Runs various RANSAC-based hand-eye calibration algorithms.
- RANSAC using the position/orientation error to determine model inliers
- RANSAC using the scalar part equality constraint to determine model inliers
For evaluation purposes:
- Exhaustive search versions of both algorithms above.
Outputs a tuple containing:
- success
- dq_H_E
- (best_rmse_position, best_rmse_orientation)
- best_num_inliers
- num_poses_after_filtering
- runtime
- singular_values
- bad_singular_values
"""
assert len(dq_W_E_vec) == len(dq_B_H_vec)
start_time = timeit.default_timer()
num_poses = len(dq_W_E_vec)
assert config.ransac_sample_size < num_poses, (
"The RANSAC sample size ({}) is bigger than the number "
"of poses ({})!".format(config.ransac_sample_size, num_poses))
# Reject pairs whose motion is not informative,
# i.e. their screw axis dot product is large
if config.prefilter_poses_enabled:
dq_B_H_vec_filtered, dq_W_E_vec_filtered = prefilter_using_screw_axis(
dq_B_H_vec, dq_W_E_vec, config.prefilter_dot_product_threshold)
assert len(dq_W_E_vec_filtered) == len(dq_B_H_vec_filtered)
assert len(dq_B_H_vec) == num_poses
assert len(dq_W_E_vec) == num_poses
num_poses_after_filtering = len(dq_W_E_vec_filtered)
else:
dq_B_H_vec_filtered = dq_B_H_vec
dq_W_E_vec_filtered = dq_W_E_vec
num_poses_after_filtering = num_poses
print("Ignore {} poses based on the screw axis".format(
num_poses - num_poses_after_filtering))
print("Drawing samples from remaining {} poses".format(
num_poses_after_filtering))
indices_set = set(range(0, num_poses_after_filtering))
# Result variables:
best_inlier_idx_set = None
best_num_inliers = 0
best_rmse_position = np.inf
best_rmse_orientation = np.inf
best_estimated_dq_H_E = None
best_singular_values = None
best_singular_value_status = True
all_sample_combinations = []
max_number_samples = np.inf
if not config.enable_exhaustive_search:
print("Running RANSAC...")
print("Inlier classification method: {}".format(
config.ransac_inlier_classification))
else:
all_sample_combinations = list(
itertools.combinations(indices_set, config.ransac_sample_size))
max_number_samples = len(all_sample_combinations)
print("Running exhaustive search, exploring {} "
"sample combinations...".format(max_number_samples))
sample_number = 0
full_iterations = 0
prerejected_samples = 0
while ((not config.enable_exhaustive_search
and full_iterations < config.ransac_max_number_iterations)
or (config.enable_exhaustive_search
and sample_number < max_number_samples)):
# Get sample, either at:
# - random (RANSAC)
# - from the list of all possible samples (exhaustive search)
if config.enable_exhaustive_search:
sample_indices = list(all_sample_combinations[sample_number])
else:
sample_indices = random.sample(indices_set,
config.ransac_sample_size)
sample_number += 1
# Extract sampled poses.
samples_dq_W_E = [dq_W_E_vec_filtered[idx] for idx in sample_indices]
samples_dq_B_H = [dq_B_H_vec_filtered[idx] for idx in sample_indices]
assert len(samples_dq_W_E) == len(samples_dq_B_H)
assert len(samples_dq_W_E) == config.ransac_sample_size
if config.ransac_sample_size > 1:
# Transform all sample poses such that the first pose becomes the origin.
aligned_samples_dq_B_H = align_paths_at_index(samples_dq_B_H,
align_index=0)
aligned_samples_dq_W_E = align_paths_at_index(samples_dq_W_E,
align_index=0)
assert len(aligned_samples_dq_B_H) == len(aligned_samples_dq_W_E)
# Reject the sample early if not even the samples have a
# similar scalar part. This should speed up RANSAC and is required, as
# the hand eye calibration does not accept outliers.
good_sample = True
for i in range(0, config.ransac_sample_size):
scalar_parts_W_E = aligned_samples_dq_W_E[i].scalar()
scalar_parts_B_H = aligned_samples_dq_B_H[i].scalar()
if not np.allclose(
scalar_parts_W_E.dq,
scalar_parts_B_H.dq,
atol=config.
ransac_sample_rejection_scalar_part_equality_tolerance
):
good_sample = False
prerejected_samples += 1
break
if not good_sample:
continue
# Transform all poses based on the first sample pose and rearange poses,
# such that the first sample pose is the first pose.
aligned_dq_B_H = align_paths_at_index(dq_B_H_vec, sample_indices[0])
aligned_dq_W_E = align_paths_at_index(dq_W_E_vec, sample_indices[0])
assert len(aligned_dq_B_H) == num_poses
assert len(aligned_dq_W_E) == num_poses
# Compute model and determine inliers
dq_H_E_initial = None
num_inliers = 0
inlier_dq_B_H = []
inlier_dq_W_E = []
if config.ransac_inlier_classification == "rmse_threshold":
assert config.ransac_sample_size >= 2, (
"Cannot compute the hand eye calibration with a "
"sample size of less than 2!")
try:
# Compute initial hand-eye calibration on SAMPLES only.
(dq_H_E_initial, singular_values,
bad_singular_values) = compute_hand_eye_calibration(
aligned_samples_dq_B_H, aligned_samples_dq_W_E, config.
hand_eye_calibration_scalar_part_equality_tolerance)
dq_H_E_initial.normalize()
except:
print("\n\n Hand-eye calibration FAILED! "
"algorithm_name: {} exception: \n\n".format(
config.algorithm_name,
sys.exc_info()[0]))
continue
# Inliers are determined by evaluating the hand-eye calibration computed
# based on the samples on all the poses and thresholding the RMSE of the
# position/orientation.
(poses_B_H, poses_W_H) = get_aligned_poses(aligned_dq_B_H,
aligned_dq_W_E,
dq_H_E_initial)
(rmse_position, rmse_orientation,
inlier_flags) = evaluate_alignment(poses_B_H, poses_W_H, config)
assert inlier_flags[0], (
"The sample idx used for alignment should be "
"part of the inlier set!")
num_inlier_removed_due_to_scalar_part_inequality = 0
for i in range(0, num_poses):
if inlier_flags[i]:
scalar_parts_W_E = aligned_dq_W_E[i].scalar()
scalar_parts_B_H = aligned_dq_B_H[i].scalar()
if not np.allclose(
scalar_parts_W_E.dq,
scalar_parts_B_H.dq,
atol=config.
ransac_sample_rejection_scalar_part_equality_tolerance
):
num_inlier_removed_due_to_scalar_part_inequality += 1
inlier_flags[i] = False
if num_inlier_removed_due_to_scalar_part_inequality > 0:
print(
"WARNING: At least one inlier selected based on the "
"position/orientation error did not pass the scalar part "
"equality test! Use tighter values for "
"ransac_position_error_threshold_m and "
"ransac_orientation_error_threshold_deg. "
"Inliers removed: {}".format(
num_inlier_removed_due_to_scalar_part_inequality))
# TODO(mfehr): Find a good way to tune the parameters.
continue
elif config.ransac_inlier_classification == "scalar_part_equality":
# Inliers are determined without computing an initial model but by simply
# selecting all poses that have a matching scalar part.
inlier_flags = [False] * num_poses
for i in range(0, num_poses):
scalar_parts_B_H = aligned_dq_B_H[i].scalar()
scalar_parts_W_E = aligned_dq_W_E[i].scalar()
if np.allclose(
scalar_parts_W_E.dq,
scalar_parts_B_H.dq,
atol=config.
ransac_sample_rejection_scalar_part_equality_tolerance
):
inlier_flags[i] = True
else:
assert False, "Unkown ransac inlier classification."
# Filter poses based on inlier flags.
inlier_dq_B_H = list(compress(aligned_dq_B_H, inlier_flags))
inlier_dq_W_E = list(compress(aligned_dq_W_E, inlier_flags))
assert len(inlier_dq_B_H) == len(inlier_dq_W_E)
num_inliers = len(inlier_dq_B_H)
# Reject sample if not enough inliers.
if num_inliers < config.min_num_inliers:
print("==> Not enough inliers ({})".format(num_inliers))
continue
if (config.ransac_model_refinement or dq_H_E_initial is None):
try:
# Refine hand-calibration using all inliers.
(dq_H_E_refined, singular_values,
bad_singular_values) = compute_hand_eye_calibration(
inlier_dq_B_H, inlier_dq_W_E, config.
hand_eye_calibration_scalar_part_equality_tolerance)
dq_H_E_refined.normalize()
except:
print("\n\n Hand-eye calibration FAILED! "
"algorithm_name: {} exception: \n\n".format(
config.algorithm_name,
sys.exc_info()[0]))
continue
else:
assert dq_H_E_initial is not None
dq_H_E_refined = dq_H_E_initial
# Rerun evaluation to determine the RMSE for the refined
# hand-eye calibration.
if config.ransac_evaluate_refined_model_on_inliers_only:
(poses_B_H, poses_W_H) = get_aligned_poses(inlier_dq_B_H,
inlier_dq_W_E,
dq_H_E_refined)
else:
(poses_B_H, poses_W_H) = get_aligned_poses(aligned_dq_B_H,
aligned_dq_W_E,
dq_H_E_refined)
(rmse_position_refined, rmse_orientation_refined,
inlier_flags) = evaluate_alignment(poses_B_H, poses_W_H, config)
if (rmse_position_refined < best_rmse_position
and rmse_orientation_refined < best_rmse_orientation):
best_estimated_dq_H_E = dq_H_E_refined
best_rmse_position = rmse_position_refined
best_rmse_orientation = rmse_orientation_refined
best_inlier_idx_set = sample_indices
best_num_inliers = num_inliers
best_singular_values = singular_values
best_singular_value_status = bad_singular_values
print("Found a new best sample: {}\n"
"\t\tNumber of inliers: {}\n"
"\t\tRMSE position: {:10.4f}\n"
"\t\tRMSE orientation: {:10.4f}\n"
"\t\tdq_H_E_initial: {}\n"
"\t\tdq_H_E_refined: {}".format(sample_indices,
num_inliers,
rmse_position_refined,
rmse_orientation_refined,
dq_H_E_initial,
dq_H_E_refined))
else:
print("Rejected sample: {}\n"
"\t\tNumber of inliers: {}\n"
"\t\tRMSE position: {:10.4f}\n"
"\t\tRMSE orientation: {:10.4f}".format(
sample_indices, num_inliers, rmse_position_refined,
rmse_orientation_refined))
full_iterations += 1
# Abort RANSAC early based on prior about the outlier probability.
if (not config.enable_exhaustive_search
and config.ransac_enable_early_abort):
s = config.ransac_sample_size
w = (1.0 - config.ransac_outlier_probability) # Inlier probability
w_pow_s = w**s
required_iterations = (
math.log(1. - config.ransac_success_probability_threshold) /
math.log(1. - w_pow_s))
if (full_iterations > required_iterations):
print(
"Reached a {}% probability that RANSAC succeeded in finding a sample "
"containing only inliers, aborting ...".format(
config.ransac_success_probability_threshold * 100.))
break
if not config.enable_exhaustive_search:
print("Finished RANSAC.")
print("RANSAC iterations: {}".format(full_iterations))
print("RANSAC early rejected samples: {}".format(prerejected_samples))
else:
print("Finished exhaustive search!")
if best_estimated_dq_H_E is None:
print("!!! RANSAC couldn't find a solution !!!")
end_time = timeit.default_timer()
runtime = end_time - start_time
return (False, None, (best_rmse_position, best_rmse_orientation),
best_num_inliers, num_poses_after_filtering, runtime,
best_singular_values, best_singular_value_status)
# Visualize best alignment.
if config.visualize:
aligned_dq_W_E = align_paths_at_index(dq_W_E_vec)
aligned_dq_B_H = align_paths_at_index(dq_B_H_vec)
(poses_B_H, poses_W_H) = get_aligned_poses(aligned_dq_B_H,
aligned_dq_W_E,
best_estimated_dq_H_E)
(rmse_position_all, rmse_orientation_all,
inlier_flags) = evaluate_alignment(poses_B_H, poses_W_H, config,
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 Alignment")
pose_vec = best_estimated_dq_H_E.to_pose()
print("Solution found with sample: {}\n"
"\t\tNumber of inliers: {}\n"
"\t\tRMSE position: {:10.4f}\n"
"\t\tRMSE orientation: {:10.4f}\n"
"\t\tdq_H_E_refined: {}\n"
"\t\tpose_H_E_refined: {}\n"
"\t\tTranslation norm: {:10.4f}".format(
best_inlier_idx_set, best_num_inliers, best_rmse_position,
best_rmse_orientation, best_estimated_dq_H_E, pose_vec,
np.linalg.norm(pose_vec[0:3])))
if best_singular_values is not None:
if best_singular_value_status:
print("The singular values of this solution are bad. "
"Either the smallest two are too big or the first 6 "
"are too small! singular values: {}".format(
best_singular_values))
end_time = timeit.default_timer()
runtime = end_time - start_time
return (True, best_estimated_dq_H_E, (best_rmse_position,
best_rmse_orientation),
best_num_inliers, num_poses_after_filtering, runtime,
best_singular_values, best_singular_value_status)
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:
poses1 = np.array([dual_quat_B_H_vec[0].to_pose().T])
poses2 = np.array([dual_quat_W_E_vec[0].to_pose().T])
for i in range(1, len(dual_quat_B_H_vec)):
poses1 = np.append(poses1,
np.array([dual_quat_B_H_vec[i].to_pose().T]),
axis=0)
poses2 = np.append(poses2,
np.array([dual_quat_W_E_vec[i].to_pose().T]),
axis=0)
every_nth_element = args.plot_every_nth_pose
plot_poses([poses1[::every_nth_element], poses2[::every_nth_element]],
True,
title="3D Poses Before Alignment")
# TODO(mfehr): Add param to switch between algorithms.
# (_, hand_eye_config) = get_RANSAC_scalar_part_inliers_config(True)
# (_, hand_eye_config) = get_RANSAC_classic_config(False)
(_, hand_eye_config) = get_exhaustive_search_scalar_part_inliers_config()
# (_, 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(
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)
