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(