def test_quaternion_to_transformation_matrix(self): q = Quaternion(0.5, 0.5, 0.5, 0.5) transformation_matrix = q.to_transformation_matrix() expected_transformation_matrix = np.array([[0, 0, 1, 0], [1, 0, 0, 0], [0, 1, 0, 0], [0, 0, 0, 1]]) npt.assert_allclose(transformation_matrix, expected_transformation_matrix)
def test_multiplication_with_scalar(self): qr = Quaternion(0.5, 0.5, -0.5, 0.5) qt = Quaternion(1, 3, 3, 0) dq = DualQuaternion(qr, qt) dq_expected = np.array([1.25, 1.25, -1.25, 1.25, 2.5, 7.5, 7.5, 0]).T # Note, the scaling only applies to the translational part. npt.assert_allclose((dq * 2.5).dq, dq_expected)
def compute_orientation_mean_and_rmse_absolute(q_AB_list, q_AC_list): assert q_AB_list.shape == q_AC_list.shape assert q_AC_list.shape[1] == 4 q_AB_0 = Quaternion(q_AB_list[0, 0], q_AB_list[0, 1], q_AB_list[0, 2], q_AB_list[0, 3]) q_AC_0 = Quaternion(q_AC_list[0, 0], q_AC_list[0, 1], q_AC_list[0, 2], q_AC_list[0, 3]) orientation_errors_rad = np.zeros(q_AB_list.shape[0]) for idx in range(q_AB_list.shape[0]): q_AB_xyzw = q_AB_list[idx, :] q_AC_xyzw = q_AC_list[idx, :] q_AB = Quaternion(q_AB_xyzw[0], q_AB_xyzw[1], q_AB_xyzw[2], q_AB_xyzw[3]) q_AC = Quaternion(q_AC_xyzw[0], q_AC_xyzw[1], q_AC_xyzw[2], q_AC_xyzw[3]) # Zero the (arbitary) initial poses. q_AB = q_AB_0.inverse() * q_AB q_AC = q_AC_0.inverse() * q_AC q_BC = q_AB.inverse() * q_AC angle_BC = q_BC.angle_axis()[3] if angle_BC > np.pi: angle_BC -= 2 * np.pi assert q_AC_list.shape[1] == 4 assert angle_BC <= np.pi assert angle_BC >= -np.pi orientation_errors_rad[idx] = np.abs(angle_BC) return ErrorStatistics(orientation_errors_rad)
def read_time_stamped_poses_from_csv_file(csv_file, JPL_quaternion_format=False): """ Reads time stamped poses from a CSV file. Assumes the following line format: timestamp [s], x [m], y [m], z [m], qx, qy, qz, qw The quaternion is expected in Hamilton format, if JPL_quaternion_format is True it expects JPL quaternions and they will be converted to Hamiltonian quaternions. """ with open(csv_file, 'r') as csvfile: csv_reader = csv.reader(csvfile, delimiter=',', quotechar='|') time_stamped_poses = np.array(list(csv_reader)) time_stamped_poses = time_stamped_poses.astype(float) # Extract the quaternions from the poses. times = time_stamped_poses[:, 0].copy() poses = time_stamped_poses[:, 1:] quaternions = [] for pose in poses: pose[3:] /= np.linalg.norm(pose[3:]) if JPL_quaternion_format: quaternion_JPL = np.array([-pose[3], -pose[4], -pose[5], pose[6]]) quaternions.append(Quaternion(q=quaternion_JPL)) else: quaternions.append(Quaternion(q=pose[3:])) return (time_stamped_poses.copy(), times, quaternions)
def test_scalar(self): qr = Quaternion(1, 2, 3, 4) qt = Quaternion(1, 3, 3, 1) dq = DualQuaternion(qr, qt) scalar = dq.scalar() scalar_expected = np.array([0., 0., 0., 4., 0., 0., 0., 1.]).T npt.assert_allclose(scalar.dq, scalar_expected, atol=1e-6)
def from_vector(cls, dq): dual_quaternion_vector = None if isinstance(dq, np.ndarray): dual_quaternion_vector = dq.copy() else: dual_quaternion_vector = np.array(dq) return cls(Quaternion(q=dual_quaternion_vector[0:4]), Quaternion(q=dual_quaternion_vector[4:8]))
def test_division(self): qr_1 = Quaternion(1, 2, 3, 4) qt_1 = Quaternion(1, 3, 3, 6) dq_1 = DualQuaternion(qr_1, qt_1) identity_dq = np.array([0., 0., 0., 1., 0., 0., 0., 0.]).T npt.assert_allclose((dq_1 / dq_1).dq, identity_dq, atol=1e-6) # TODO(ff): Fix this test. qr_2 = Quaternion(1, 4, 5, 1) qt_2 = Quaternion(-4, 2, 3, 4) dq_2 = DualQuaternion(qr_2, qt_2)
def test_addition(self): qr_1 = Quaternion(1., 2., 3., 4.) qt_1 = Quaternion(1., 3., 3., 0.) dq_1 = DualQuaternion(qr_1, qt_1) qr_2 = Quaternion(3., 5., 3., 2.) qt_2 = Quaternion(-4., 2., 3., 0.) dq_2 = DualQuaternion(qr_2, qt_2) dq_expected = np.array([4., 7., 6., 6., -3., 5., 6., 0.]).T npt.assert_allclose((dq_1 + dq_2).dq, dq_expected, rtol=1e-6)
def test_multiplication(self): qr_1 = Quaternion(1., 2., 3., 4.) qt_1 = Quaternion(1., 3., 3., 0.) dq_1 = DualQuaternion(qr_1, qt_1) qr_2 = Quaternion(1., 4., 5., 1.) qt_2 = Quaternion(-4., 2., 3., 0.) dq_2 = DualQuaternion(qr_2, qt_2) # TODO(ff): Check this by hand. (And check if we shouldn't always get 0 # scalars for the rotational quaternion of the dual quaternion). dq_expected = DualQuaternion( dq_1.q_rot * dq_2.q_rot, dq_1.q_rot * dq_2.q_dual + dq_1.q_dual * dq_2.q_rot) npt.assert_allclose((dq_1 * dq_2).dq, dq_expected.dq)
def test_conjugate_identity(self): qr = Quaternion(1, 2, 3, 4) qt = Quaternion(1, 3, 3, 0) dq = DualQuaternion(qr, qt) # TODO(ff): Here we should use dq*dq.conjugate() in one # place. q_rot_identity = dq.q_rot * dq.q_rot.conjugate() q_dual_identity = (dq.q_rot.conjugate() * dq.q_dual + dq.q_dual.conjugate() * dq.q_rot) identity_dq_expected = DualQuaternion(q_rot_identity, q_dual_identity) npt.assert_allclose((dq * dq.conjugate()).dq, identity_dq_expected.dq, atol=1e-6)
def compute_orientation_mean_and_rmse_relative(q_AB_list, q_AC_list): assert q_AB_list.shape == q_AC_list.shape assert q_AC_list.shape[1] == 4 orientation_errors_rad = np.zeros(q_AB_list.shape[0] - 1) for idx in range(q_AB_list.shape[0] - 1): q_ABk = Quaternion(q_AB_list[idx, 0], q_AB_list[idx, 1], q_AB_list[idx, 2], q_AB_list[idx, 3]) q_ACk = Quaternion(q_AC_list[idx, 0], q_AC_list[idx, 1], q_AC_list[idx, 2], q_AC_list[idx, 3]) q_ABkp1 = Quaternion(q_AB_list[idx + 1, 0], q_AB_list[idx + 1, 1], q_AB_list[idx + 1, 2], q_AB_list[idx + 1, 3]) q_ACkp1 = Quaternion(q_AC_list[idx + 1, 0], q_AC_list[idx + 1, 1], q_AC_list[idx + 1, 2], q_AC_list[idx + 1, 3]) q_Bk_Bkp1 = q_ABk.inverse() * q_ABkp1 q_Ck_Ckp1 = q_ACk.inverse() * q_ACkp1 q_error = q_Bk_Bkp1.inverse() * q_Ck_Ckp1 angle_error = q_error.angle_axis()[3] if angle_error > np.pi: angle_error -= 2 * np.pi assert angle_error <= np.pi assert angle_error >= -np.pi orientation_errors_rad[idx] = np.abs(angle_error) return ErrorStatistics(orientation_errors_rad)
def test_normalize(self): qr = Quaternion(1, 2, 3, 4) qt = Quaternion(1, 3, 3, 0) dq = DualQuaternion(qr, qt) dq.normalize() dq_normalized = np.array([ 0.18257419, 0.36514837, 0.54772256, 0.73029674, 0.18257419, 0.54772256, 0.54772256, 0. ]).T npt.assert_allclose(dq.dq, dq_normalized) dq_2 = DualQuaternion.from_pose(1, 2, 3, 1, 1, 1, 1) dq_2.normalize() dq_2_normalized = np.array([0.5, 0.5, 0.5, 0.5, 0., 1., 0.5, -1.5]).T npt.assert_allclose(dq_2.dq, dq_2_normalized)
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 csv_row_data_to_transformation(A_p_AB, q_AB_np): """ Constructs a proper transformation matrix out of the given translation vector and quaternion. Returns: 4x4 transformation matrix. """ assert (math.fabs(1.0 - np.linalg.norm(q_AB_np)) < 1e-8) T_AB = np.identity(4) q_AB = Quaternion(q=q_AB_np) R_AB = q_AB.to_rotation_matrix() T_AB[0:3, 0:3] = R_AB T_AB[0:3, 3] = A_p_AB return T_AB
def compute_orientation_mean_and_rmse(q_AB_list, q_AC_list): assert q_AB_list.shape == q_AC_list.shape assert (q_AC_list.shape[1] == 4) orientation_errors_rad = np.zeros(q_AB_list.shape[0]) for idx in range(q_AB_list.shape[0]): q_AB_xyzw = q_AB_list[idx, :] q_AC_xyzw = q_AC_list[idx, :] q_AB = Quaternion(q_AB_xyzw[0], q_AB_xyzw[1], q_AB_xyzw[2], q_AB_xyzw[3]) q_AC = Quaternion(q_AC_xyzw[0], q_AC_xyzw[1], q_AC_xyzw[2], q_AC_xyzw[3]) q_BC = q_AB.inverse() * q_AC orientation_errors_rad[idx] = q_BC.angle_axis()[3] return np.mean(orientation_errors_rad), RMSE(orientation_errors_rad)
def interpolate_poses_from_samples(time_stamped_poses, samples): """ Interpolate time stamped poses at the time stamps provided in samples. The poses are expected in the following format: [timestamp [s], x [m], y [m], z [m], qx, qy, qz, qw] We apply linear interpolation to the position and use SLERP for the quaternion interpolation. """ # Extract the quaternions from the poses. quaternions = [] for pose in time_stamped_poses[:, 1:]: quaternions.append(Quaternion(q=pose[3:])) # interpolate the quaternions. quaternions_interp = resample_quaternions_from_samples( time_stamped_poses[:, 0], quaternions, samples) # Interpolate the position and assemble the full aligned pose vector. num_poses = samples.shape[0] aligned_poses = np.zeros((num_poses, time_stamped_poses.shape[1])) aligned_poses[:, 0] = samples[:] for i in [1, 2, 3]: aligned_poses[:, i] = np.interp( samples, np.asarray(time_stamped_poses[:, 0]).ravel(), np.asarray(time_stamped_poses[:, i]).ravel()) for i in range(0, num_poses): aligned_poses[i, 4:8] = quaternions_interp[i].q return aligned_poses.copy()
def load_dataset(csv_file, input_format="maplab_console"): """ Loads a dataset in one of the supported formats and returns a Nx8 matrix with the trajectory from the dataset with the following columns: timestamp [s], position x, y, z [m], quaternion x, y, z, w Supported formats: - end_to_end (same as internal end_to_end representation) - maplab_console (output of csv_export command in maplab) - rovioli (output of --export_estimated_poses_to_csv flag from ROVIOLI) - euroc_ground_truth (euroc ground truth CSVs as can be downloaded with the datasets) - orbslam (output from running ORB-SLAM) """ assert os.path.isfile(csv_file), "File " + csv_file + " doesn't exist." NANOSECONDS_TO_SECONDS = 1e-9 trajectory = np.zeros((0, 8)) if input_format.lower() == "end_to_end": trajectory = np.genfromtxt(csv_file, delimiter=',', skip_header=0) elif input_format.lower() == "maplab_console": trajectory = np.genfromtxt(csv_file, delimiter=',', skip_header=1) trajectory = trajectory[:, 1:9] trajectory[:, 0] *= NANOSECONDS_TO_SECONDS elif input_format.lower() == "rovioli": # Compute T_G_I from T_G_M and T_G_I. estimator_data_raw = np.genfromtxt(csv_file, delimiter=',', skip_header=1) num_elements = estimator_data_raw.shape[0] trajectory = np.zeros((num_elements, 8)) for i in range(num_elements): trajectory[i, 0] = estimator_data_raw[i, 0] T_G_M = csv_row_data_to_transformation(estimator_data_raw[i, 1:4], estimator_data_raw[i, 4:8]) T_M_I = csv_row_data_to_transformation( estimator_data_raw[i, 8:11], estimator_data_raw[i, 11:15]) T_G_M = np.dot(T_G_M, T_M_I) trajectory[i, 1:4] = T_G_M[0:3, 3] q_G_I = Quaternion.from_rotation_matrix(T_G_M[0:3, 0:3]) trajectory[i, 4:8] = q_G_I.q elif input_format.lower() == "euroc_ground_truth": trajectory = np.genfromtxt(csv_file, delimiter=',', skip_header=1) # We need to bring the Euroc ground truth data into the format required # by hand-eye calibration. trajectory[:, 0] *= NANOSECONDS_TO_SECONDS # Reorder quaternion wxyz -> xyzw. trajectory[:, [4, 5, 6, 7]] = trajectory[:, [5, 6, 7, 4]] # Remove remaining columns. trajectory = trajectory[:, 0:8] elif input_format.lower() == "orbslam": trajectory = np.genfromtxt(csv_file, delimiter=' ', skip_header=0) # TODO(eggerk): Check quaternion convention from ORBSLAM. else: print "Unknown estimator input format." print "Valid options are maplab_console, rovioli, orbslam." assert False return trajectory
def test_slerp(self): q_1 = Quaternion(1, 2, 3, 4) q_2 = Quaternion(2, 3, 4, 5) q_1.normalize() q_2.normalize() q_expected = np.array([ 0.22772264489951, 0.38729833462074169, 0.54687402434197, 0.70644971406320634 ]).T npt.assert_allclose(quaternion_slerp(q_1, q_2, 0.5).q, q_expected)
def test_nlerp(self): q_1 = Quaternion(1, 2, 3, 4) q_2 = Quaternion(2, 3, 4, 5) q_1.normalize() q_2.normalize() q_expected = np.array([0.22772264, 0.38729833, 0.54687402, 0.70644971]).T npt.assert_allclose(quaternion_nlerp(q_1, q_2, 0.5).q, q_expected, atol=1e-6)
def test_lerp(self): q_1 = Quaternion(1, 2, 3, 4) q_2 = Quaternion(2, 3, 4, 5) q_1.normalize() q_2.normalize() q_expected = np.array([0.22736986, 0.38669833, 0.54602681, 0.70535528]).T npt.assert_allclose(quaternion_lerp(q_1, q_2, 0.5).q, q_expected, atol=1e-6)
def spoil_initial_guess(time_offset_initial_guess, dq_H_E_initial_guess, angular_offset_range, translation_offset_range, time_offset_range): """ Apply a random perturbation to the calibration.""" random_time_offset_offset = np.random.uniform( time_offset_range[0], time_offset_range[1]) * random.choice([-1., 1.]) # Get a random unit vector random_translation_offset = np.random.uniform(-1.0, 1.0, 3) random_translation_offset /= np.linalg.norm(random_translation_offset) assert np.isclose(np.linalg.norm(random_translation_offset), 1., atol=1e-8) # Scale unit vector to a random length between 0 and max_translation_offset. random_translation_length = np.random.uniform( translation_offset_range[0], translation_offset_range[1]) random_translation_offset *= random_translation_length # Get orientation offset of at most max_angular_offset. random_quaternion_offset = Quaternion.get_random( angular_offset_range[0], angular_offset_range[1]) translation_initial_guess = dq_H_E_initial_guess.to_pose()[0: 3] orientation_initial_guess = dq_H_E_initial_guess.q_rot time_offset_spoiled = time_offset_initial_guess + random_time_offset_offset translation_spoiled = random_translation_offset + translation_initial_guess orientation_spoiled = random_quaternion_offset * orientation_initial_guess # Check if we spoiled correctly. random_angle_offset = angle_between_quaternions( orientation_initial_guess, orientation_spoiled) assert random_angle_offset <= angular_offset_range[1] assert random_angle_offset >= angular_offset_range[0] assert np.linalg.norm(translation_spoiled - translation_initial_guess) <= translation_offset_range[1] assert np.linalg.norm(translation_spoiled - translation_initial_guess) >= translation_offset_range[0] # Get random orientation that distorts the original orientation by at most # max_angular_offset rad. dq_H_E_spoiled = DualQuaternion.from_pose( translation_spoiled[0], translation_spoiled[1], translation_spoiled[2], orientation_spoiled.q[0], orientation_spoiled.q[1], orientation_spoiled.q[2], orientation_spoiled.q[3]) print("dq_H_E_initial_guess: {}".format(dq_H_E_initial_guess.dq)) print("dq_H_E_spoiled: {}".format(dq_H_E_spoiled.dq)) print("time_offset_initial_guess: {}".format(time_offset_initial_guess)) print("time_offset_spoiled: {}".format(time_offset_spoiled)) print("random_quaternion_offset: {}".format(random_quaternion_offset.q)) return (time_offset_spoiled, dq_H_E_spoiled, (random_translation_offset, random_angle_offset, random_time_offset_offset))
def from_transformation_matrix(cls, transformation_matrix): q_rot = Quaternion.from_rotation_matrix(transformation_matrix[0:3, 0:3]) pose_vec = np.zeros(7) pose_vec[3:7] = q_rot.q pose_vec[0:3] = transformation_matrix[0:3, 3] return cls.from_pose_vector(pose_vec)
def generate_test_path(n_samples, include_outliers=False, outlier_probability=0.1, include_noise=False, noise_sigma_trans=0.01, noise_sigma_rot=0.1): # Create a sine for x, cos for y and linear motion for z. # Rotate around the curve, while keeping the x-axis perpendicular to the # curve. dual_quaternions = [] t = np.linspace(0, 1, num=n_samples) x = 10.0 + np.cos(4 * np.pi * t) y = -5.0 + -np.sin(4 * np.pi * t) * 4 z = 0.5 + t * 5 theta = 4 * np.pi * t outliers = [] for i in range(n_samples): q_tmp = tf.transformations.quaternion_from_euler( 0., -theta[i] * 0.1, -theta[i], 'rxyz') if include_outliers: if np.random.rand() < outlier_probability: outliers.append(i) q_tmp = np.random.rand(4) x[i] = np.random.rand() * max(x) * ( -1 if np.random.rand() < 0.5 else 0) y[i] = np.random.rand() * max(y) * ( -1 if np.random.rand() < 0.5 else 0) z[i] = np.random.rand() * max(z) * ( -1 if np.random.rand() < 0.5 else 0) if include_noise: # Add zero mean gaussian noise with sigma noise_sigma. x[i] += np.random.normal(0.0, noise_sigma_trans) y[i] += np.random.normal(0.0, noise_sigma_trans) z[i] += np.random.normal(0.0, noise_sigma_trans) # TODO(ff): Fix this, there should only be 3 random numbers drawn. axis_noise_x = np.random.normal(0.0, noise_sigma_rot) axis_noise_y = np.random.normal(0.0, noise_sigma_rot) axis_noise_z = np.random.normal(0.0, noise_sigma_rot) angle_noise = np.pi * np.random.normal(0.0, noise_sigma_rot) q_noise = Quaternion.from_angle_axis( angle_noise, (axis_noise_x, axis_noise_y, axis_noise_z)) q_noise.normalize() q_noise_free = Quaternion(q=q_tmp) q = q_noise * q_noise_free * q_noise.inverse() else: q = Quaternion(q=q_tmp) q.normalize() if q.w < 0.0: q = -q dq = DualQuaternion.from_pose(x[i], y[i], z[i], q.x, q.y, q.z, q.w) dual_quaternions.append(dq) if include_outliers: print("Included {} outliers at {} in the test data.".format( len(outliers), outliers)) return dual_quaternions
def compute_pose_error(pose_A, pose_B): """ Compute the error norm of position and orientation. """ error_position = np.linalg.norm(pose_A[0:3] - pose_B[0:3], ord=2) # Construct quaternions to compare. quaternion_A = Quaternion(q=pose_A[3:7]) quaternion_A.normalize() if quaternion_A.w < 0: quaternion_A.q = -quaternion_A.q quaternion_B = Quaternion(q=pose_B[3:7]) quaternion_B.normalize() if quaternion_B.w < 0: quaternion_B.q = -quaternion_B.q # Sum up the square of the orientation angle error. error_angle_rad = angle_between_quaternions(quaternion_A, quaternion_B) error_angle_degrees = math.degrees(error_angle_rad) if error_angle_degrees > 180.0: error_angle_degrees = math.fabs(360.0 - error_angle_degrees) return (error_position, error_angle_degrees)
def align_datasets(estimator_data_unaligned_G_I, ground_truth_data_unaligned_W_M, estimate_scale, align_data_to_use_meters=-1, time_offset=0.0, marker_calibration_file=None): """Sample and align the estimator trajectory to the ground truth trajectory. Input: - estimator_data_unaligned_G_I: Unaligned estimator trajectory expressed from global frame G to IMU frame I. - ground_truth_data_unaligned_W_M: Unaligned ground truth data expressed from ground truth world frame W (e.g. Vicon frame) to marker frame M (e.g. Vicon marker). M and I should be at the same position at a given time. - estimate_scale: Estimate a scale factor in addition to the transform and apply it to the aligned estimator trajectory. - align_data_to_use_meters: Only use data points up to a total data length as given by this parameter. If a negative value is given, the entire trajectory is used for the alignment. - time_offset: Indicates the time offset (in seconds) between the data from the bag file and the ground truth data. In the alignment, the estimator data will be shifted by this offset. - marker_calibration_file: Contains the calibration between the marker frame M and the eye frame I. If no file is given, the frames M and I are assumed to be identical. Returns: Sampled, aligned and transformed estimator trajectory, sampled ground truth. """ if marker_calibration_file: estimator_data_unaligned_G_I = apply_hand_eye_calibration( estimator_data_unaligned_G_I, marker_calibration_file) else: estimator_data_unaligned_G_I = estimator_data_unaligned_G_I [estimator_data_G_I, ground_truth_data_W_M ] = time_alignment.compute_aligned_poses(estimator_data_unaligned_G_I, ground_truth_data_unaligned_W_M, time_offset) # Align trajectories (umeyama). if align_data_to_use_meters < 0.0: umeyama_estimator_data_G_I = estimator_data_G_I[:, 1:4] umeyama_ground_truth_W_M = ground_truth_data_W_M[:, 1:4] else: trajectory_length = get_cumulative_trajectory_length_for_each_point( ground_truth_data_W_M[:, 1:4]) align_num_data_points = bisect.bisect_left(trajectory_length, align_data_to_use_meters) umeyama_estimator_data_G_I = estimator_data_G_I[:align_num_data_points, 1:4] umeyama_ground_truth_W_M = \ ground_truth_data_W_M[:align_num_data_points, 1:4] T_W_G, scale = umeyama(umeyama_estimator_data_G_I.T, umeyama_ground_truth_W_M.T, estimate_scale) assert T_W_G.shape[0] == 4 assert T_W_G.shape[1] == 4 if estimate_scale: assert scale > 0.0 print("Estimator scale:", 1. / scale) estimator_data_G_I_scaled = estimator_data_G_I.copy() estimator_data_G_I_scaled[:, 1:4] *= scale ground_truth_data_G_M_aligned = ground_truth_data_W_M.copy() num_data_points = ground_truth_data_W_M.shape[0] for index in range(num_data_points): # Get transformation matrix from quaternion (normalized) and position. T_ground_truth_W_M = csv_row_data_to_transformation( ground_truth_data_W_M[index, 1:4], ground_truth_data_W_M[index, 4:8] / np.linalg.norm(ground_truth_data_W_M[index, 4:8])) # Apply found calibration between G and M frame. T_ground_truth_G_M = np.dot(np.linalg.inv(T_W_G), T_ground_truth_W_M) # Extract quaternion and position. q_ground_truth_G_M = Quaternion.from_rotation_matrix( T_ground_truth_G_M[0:3, 0:3]) ground_truth_data_G_M_aligned[index, 4:8] = q_ground_truth_G_M.q ground_truth_data_G_M_aligned[index, 1:4] = T_ground_truth_G_M[0:3, 3] return estimator_data_G_I_scaled, ground_truth_data_G_M_aligned
def align_datasets(estimator_data_G_I_path, ground_truth_data_W_M_path, estimator_input_format="rovioli"): """ Sample and align the ground truth trajectory to the estimator trajectory. Input frames: Estimator: G (Global, from rovioli) --> I (imu, from rovioli) Ground truth: W (Ground truth world, e.g., Vicon frame) --> M (ground truth marker, e.g., Vicon marker) (M and I should be at the same position at a given time.) Returns: aligned estimator trajectory, aligned and sampled ground truth trajectory. Output data frames: Estimator: G --> I Ground truth: G --> M (M and I should be at the same position.) """ assert os.path.isfile(estimator_data_G_I_path) assert os.path.isfile(ground_truth_data_W_M_path) estimator_data_unaligned_G_I = np.zeros((0, 8)) if estimator_input_format.lower() == "maplab_console": estimator_data_unaligned_G_I = np.genfromtxt(estimator_data_G_I_path, delimiter=',', skip_header=1) estimator_data_unaligned_G_I = estimator_data_unaligned_G_I[:, 1:9] # Convert timestamp ns -> s. estimator_data_unaligned_G_I[:, 0] *= 1e-9 elif estimator_input_format.lower() == "rovioli": # Compute T_G_I from T_G_M and T_G_I estimator_data_raw = np.genfromtxt(estimator_data_G_I_path, delimiter=',', skip_header=1) num_elements = estimator_data_raw.shape[0] estimator_data_unaligned_G_I = np.zeros((num_elements, 8)) for i in range(num_elements): estimator_data_unaligned_G_I[i, 0] = estimator_data_raw[i, 0] T_G_M = csv_row_data_to_transformation(estimator_data_raw[i, 1:4], estimator_data_raw[i, 4:8]) T_M_I = csv_row_data_to_transformation( estimator_data_raw[i, 8:11], estimator_data_raw[i, 11:15]) T_G_M = np.dot(T_G_M, T_M_I) estimator_data_unaligned_G_I[i, 1:4] = T_G_M[0:3, 3] q_G_I = Quaternion.from_rotation_matrix(T_G_M[0:3, 0:3]) estimator_data_unaligned_G_I[i, 4:8] = q_G_I.q elif estimator_input_format.lower() == "orbslam": estimator_data_unaligned_G_I = np.genfromtxt(estimator_data_G_I_path, delimiter=' ', skip_header=0) # TODO(eggerk): Check quaternion convention from ORBSLAM. else: print "Unknown estimator input format." print "Valid options are maplab_console, rovioli, orbslam." assert False ground_truth_data_unaligned_W_M = np.genfromtxt(ground_truth_data_W_M_path, delimiter=',', skip_header=1) ground_truth_data_unaligned_W_M = \ convert_ground_truth_data_into_target_format( ground_truth_data_unaligned_W_M) # This is needed for the alignment and sampling. It indicates the time offset # between the data from Rovioli/from the bag file and the ground truth data. # In the case of the Euroc datasets, this is 0 as they image/imu data and # ground truth is already synchronized. time_offset = 0 [ground_truth_data_W_M, estimator_data_G_I] = \ time_alignment.compute_aligned_poses( ground_truth_data_unaligned_W_M, estimator_data_unaligned_G_I, time_offset) # Create a matrix of the right size to store aligned trajectory. ground_truth_data_aligned_G_M = ground_truth_data_W_M # Align trajectories (umeyama). R_G_W, t_G_W, scale = umeyama(ground_truth_data_W_M[:, 1:4].T, estimator_data_G_I[:, 1:4].T) T_G_W = np.identity(4) T_G_W[0:3, 0:3] = R_G_W T_G_W[0:3, 3] = t_G_W if abs(1 - scale) < 1e-8: print "WARNING: Scale is not 1, but", scale, "\b." num_data_points = estimator_data_G_I.shape[0] for index in range(num_data_points): # Create transformations to compare. T_ground_truth_G_M = np.dot( T_G_W, csv_row_data_to_transformation( ground_truth_data_W_M[index, 1:4], ground_truth_data_W_M[index, 4:8] / np.linalg.norm(ground_truth_data_W_M[index, 4:8]))) ground_truth_data_aligned_G_M[index, 1:4] = T_ground_truth_G_M[0:3, 3] q_ground_truth_G_M = Quaternion.from_rotation_matrix( T_ground_truth_G_M[0:3, 0:3]) ground_truth_data_aligned_G_M[index, 4:8] = q_ground_truth_G_M.q return estimator_data_G_I, ground_truth_data_aligned_G_M
def test_conjugate(self): qr = Quaternion(1., 2., 3., 4.) qt = Quaternion(1., 3., 3., 5.) dq = DualQuaternion(qr, qt) dq_expected = np.array([-1., -2., -3., 4., -1., -3., -3., 5.]).T npt.assert_allclose((dq.conjugate()).dq, dq_expected, atol=1e-6)
def test_division_with_scalar(self): qr = Quaternion(0.5, 0.5, -0.5, 0.5) qt = Quaternion(1., 3., 3., 0.) dq = DualQuaternion(qr, qt) dq_expected = np.array([0.25, 0.25, -0.25, 0.25, 0.5, 1.5, 1.5, 0.]).T npt.assert_allclose((dq / 2.).dq, dq_expected)
def test_equality(self): qr = Quaternion(1, 2, 3, 4) qt = Quaternion(1, 3, 3, 1) dq_1 = DualQuaternion(qr, qt) dq_2 = DualQuaternion(qr, qt) self.assertEqual(dq_1, dq_2)
def test_inverse(self): qr = Quaternion(1, 2, 3, 4) qt = Quaternion(5, 6, 7, 8) dq = DualQuaternion(qr, qt) identity_dq = np.array([0., 0., 0., 1., 0., 0., 0., 0.]).T npt.assert_allclose((dq * dq.inverse()).dq, identity_dq, atol=1e-6)