def from_axis_angle_translation( axis: type_alias.TensorLike, angle: type_alias.TensorLike, translation_vector: type_alias.TensorLike, name: str = "dual_quat_from_axis_angle_trans" ) -> type_alias.TensorLike: """Converts an axis-angle rotation and translation to a dual quaternion. Note: In the following, A1 to An are optional batch dimensions. Args: axis: A tensor of shape `[A1, ..., An, 3]`, where the last dimension represents a normalized axis. angle: A tensor of shape `[A1, ..., An, 1]`, where the last dimension represents an angle. translation_vector: A `[A1, ..., An, 3]`-tensor, where the last dimension represents a translation vector. name: A name for this op that defaults to "dual_quat_from_axis_angle_trans". Returns: A `[A1, ..., An, 8]`-tensor, where the last dimension represents a normalized dual quaternion. Raises: ValueError: If the shape of `axis`, `angle`, or `translation_vector` is not supported. """ with tf.name_scope(name): axis = tf.convert_to_tensor(value=axis) angle = tf.convert_to_tensor(value=angle) translation_vector = tf.convert_to_tensor(value=translation_vector) shape.check_static(tensor=axis, tensor_name="axis", has_dim_equals=(-1, 3)) shape.check_static(tensor=angle, tensor_name="angle", has_dim_equals=(-1, 1)) shape.check_static(tensor=translation_vector, tensor_name="translation_vector", has_dim_equals=(-1, 3)) shape.compare_batch_dimensions(tensors=(axis, angle, translation_vector), last_axes=-2, broadcast_compatible=True) scalar_shape = tf.concat((tf.shape(translation_vector)[:-1], (1, )), axis=-1) dtype = translation_vector.dtype quaternion_rotation = quaternion.from_axis_angle(axis, angle) quaternion_translation = tf.concat( (translation_vector, tf.zeros(scalar_shape, dtype)), axis=-1) dual_quaternion_dual_part = 0.5 * quaternion.multiply( quaternion_translation, quaternion_rotation) return tf.concat((quaternion_rotation, dual_quaternion_dual_part), axis=-1)
def predict_the_pose(pos_init, quat_init, xy_pred, z_pred, quat_pred, img_scaling, cam_mat): img_center = tf.matmul(tf.expand_dims(pos_init, axis=1), cam_mat, transpose_b=True) img_center = tf.reduce_sum(img_center, axis=1) img_center = tf.div(img_center, tf.expand_dims(img_center[:, -1], axis=1)) xy_image = img_center[:, :2] + xy_pred / tf.expand_dims(img_scaling, axis=-1) z_model = pos_init[:, 2] x_model_cam_frame = (xy_image[:, 0] - cam_mat[:, 0, 2]) / cam_mat[:, 0, 0] x_model_cam_frame = tf.expand_dims(tf.multiply(x_model_cam_frame, z_model), axis=-1) y_model_cam_frame = (xy_image[:, 1] - cam_mat[:, 1, 2]) / cam_mat[:, 1, 1] y_model_cam_frame = tf.expand_dims(tf.multiply(y_model_cam_frame, z_model), axis=-1) z_model = tf.expand_dims(z_model, axis=-1) pose_model_lateral = tf.concat( (x_model_cam_frame, y_model_cam_frame, z_model), axis=-1) z_out = z_pred / 3000.0 pos_out = tf.multiply(tf.exp(z_out), pose_model_lateral) quat = quat_pred / 10.0 quat_len = tf.expand_dims(tf.norm(quat, axis=-1), axis=-1) quat_out = tf.div(quat, quat_len) quat_out = quaternion.multiply(quat_out, quat_init) return pos_out, quat_out
def to_rotation_translation( dual_quaternion: type_alias.TensorLike, name: str = "dual_quaternion_to_rot_trans" ) -> Tuple[tf.Tensor, tf.Tensor]: """Converts a dual quaternion into a quaternion for rotation and translation. Args: dual_quaternion: A `[A1, ..., An, 8]`-tensor, where the last dimension represents a qual quaternion. name: A name for this op that defaults to "dual_quaternion_to_rot_trans". Returns: A tuple with a `[A1, ..., An, 4]`-tensor for rotation in quaternion form, and a `[A1, ..., An, 3]`-tensor for translation, in that order. """ with tf.name_scope(name): dual_quaternion = tf.convert_to_tensor(value=dual_quaternion) shape.check_static(tensor=dual_quaternion, tensor_name="dual_quaternion", has_dim_equals=(-1, 8)) rotation = dual_quaternion[..., 0:4] translation = 2 * quaternion.multiply(dual_quaternion[..., 4:8], quaternion.inverse(rotation)) translation = translation[..., 0:3] return rotation, translation
def eval_accuracy(X, Y, Y_hat, X_train, Y_train, X_test, Y_test, t_treshold, r_threshold): # Have to swap axes due to how tensorflow quaternions work Yq = tf.transpose(Y[:4], [1, 0]) Yq_hat = tf.transpose(Y_hat[:4], [1, 0]) # Compute difference quaternion then swap axes back diff = tfq.multiply(tfq.normalize(Yq), tfq.inverse(tfq.normalize(Yq_hat))) diff = tf.transpose(diff, [1, 0]) # Compute angle difference in degrees diffW = tf.clip_by_value(diff[3], clip_value_min=-1.0, clip_value_max=1.0) angle_diff = tf_rad2deg(math.pi - tf.math.abs(2 * tf.math.acos(diffW) - math.pi)) correct_rot = tf.cast(tf.less(angle_diff, [r_threshold]), "float") correct_trans = tf.cast(tf.less(tf.norm(Y[4:] - Y_hat[4:], axis = 0), [t_treshold]), "float") t_accuracy = tf.reduce_mean(tf.cast(correct_trans, "float")) r_accuracy = tf.reduce_mean(tf.cast(correct_rot, "float")) accuracy = tf.reduce_mean(tf.cast(correct_trans * correct_rot, "float")) print('\n--------------') print("Evaluating accuracy with rotation threshold of " + str(r_threshold) + " and translation treshold of " + str(t_treshold)) print('') print ("Train Accuracy:", accuracy.eval({X: X_train, Y: Y_train})) print ("Test Accuracy:", accuracy.eval({X: X_test, Y: Y_test})) print('') print ("Train Accuracy on just translation:", t_accuracy.eval({X: X_train, Y: Y_train})) print ("Test Accuracy on just translation:", t_accuracy.eval({X: X_test, Y: Y_test})) print('') print ("Train Accuracy on just rotation:", r_accuracy.eval({X: X_train, Y: Y_train})) print ("Test Accuracy on just rotation:", r_accuracy.eval({X: X_test, Y: Y_test})) print('\n--------------')
def multiply(dual_quaternion1: type_alias.TensorLike, dual_quaternion2: type_alias.TensorLike, name: str = "dual_quaternion_multiply") -> tf.Tensor: """Multiplies two dual quaternions. Note: In the following, A1 to An are optional batch dimensions. Args: dual_quaternion1: A TensorLike of shape `[A1, ..., An, 8]`, where the last dimension represents a dual quaternion. dual_quaternion2: A TensorLike of shape `[A1, ..., An, 8]`, where the last dimension represents a dual quaternion. name: A name for this op that defaults to "dual_quaternion_multiply". Returns: A tensor of shape `[A1, ..., An, 8]` representing dual quaternions. """ with tf.name_scope(name): dual_quaternion1 = tf.convert_to_tensor(value=dual_quaternion1) dual_quaternion2 = tf.convert_to_tensor(value=dual_quaternion2) shape.check_static(tensor=dual_quaternion1, tensor_name="dual_quaternion1", has_dim_equals=(-1, 8)) shape.check_static(tensor=dual_quaternion2, tensor_name="dual_quaternion2", has_dim_equals=(-1, 8)) dual_quaternion1_real, dual_quaternion1_dual = tf.split( dual_quaternion1, (4, 4), axis=-1) dual_quaternion2_real, dual_quaternion2_dual = tf.split( dual_quaternion2, (4, 4), axis=-1) dual_quaternion_output_real = quaternion.multiply( dual_quaternion1_real, dual_quaternion2_real) dual_quaternion_output_dual = ( quaternion.multiply(dual_quaternion1_real, dual_quaternion2_dual) + quaternion.multiply(dual_quaternion1_dual, dual_quaternion2_real)) return tf.concat( (dual_quaternion_output_real, dual_quaternion_output_dual), axis=-1)
def test_multiply_jacobian_preset(self): """Test the Jacobian of the multiply function.""" x_1_init = test_helpers.generate_preset_test_quaternions() x_2_init = test_helpers.generate_preset_test_quaternions() x_1 = tf.convert_to_tensor(value=x_1_init) x_2 = tf.convert_to_tensor(value=x_2_init) y = quaternion.multiply(x_1, x_2) self.assert_jacobian_is_correct(x_1, x_1_init, y) self.assert_jacobian_is_correct(x_2, x_2_init, y)
def test_inverse_random(self): """Tests that multiplying with the inverse gives identity.""" random_quaternion = test_helpers.generate_random_test_quaternions() inverse_quaternion = quaternion.inverse(random_quaternion) final_quaternion = quaternion.multiply(random_quaternion, inverse_quaternion) tensor_shape = random_quaternion.shape[:-1] identity_quaternion = np.array((0.0, 0.0, 0.0, 1.0), dtype=np.float32) identity_quaternion = np.tile(identity_quaternion, tensor_shape + (1,)) self.assertAllClose(final_quaternion, identity_quaternion, rtol=1e-3)
def from_rotation_translation( rotation_quaternion: type_alias.TensorLike, translation_vector: type_alias.TensorLike, name: str = "dual_quaternion_from_rotation_translation") -> tf.Tensor: """Converts a rotation matrix and translation vector to a dual quaternion. Warning: This function is not smooth everywhere. Note: In the following, A1 to An are optional batch dimensions. Rotation is applied first. Args: rotation_quaternion: A `[A1, ..., An, 4]`-tensor, where the last dimension represents a rotation in the form a quaternion. translation_vector: A `[A1, ..., An, 3]`-tensor, where the last dimension represents a translation vector. name: A name for this op that defaults to "dual_quaternion_from_rot_trans". Returns: A `[A1, ..., An, 8]`-tensor, where the last dimension represents a normalized dual quaternion. Raises: ValueError: If the shape of `rotation_matrix` is not supported. """ with tf.name_scope(name): rotation_quaternion = tf.convert_to_tensor(value=rotation_quaternion) translation_vector = tf.convert_to_tensor(value=translation_vector) shape.check_static(tensor=rotation_quaternion, tensor_name="rotation_quaternion", has_rank_greater_than=1, has_dim_equals=(-1, 4)) shape.check_static(tensor=translation_vector, tensor_name="translation_vector", has_dim_equals=(-1, 3)) scalar_shape = tf.concat((tf.shape(translation_vector)[:-1], (1, )), axis=-1) dtype = translation_vector.dtype quaternion_translation = tf.concat( (translation_vector, tf.zeros(scalar_shape, dtype)), axis=-1) dual_quaternion_dual_part = 0.5 * quaternion.multiply( quaternion_translation, rotation_quaternion) return tf.concat((rotation_quaternion, dual_quaternion_dual_part), axis=-1)
def test_multiply_jacobian_random(self): """Test the Jacobian of the multiply function.""" tensor_dimensions = np.random.randint(low=1, high=3) tensor_shape = np.random.randint(1, 10, size=(tensor_dimensions)).tolist() x_1_init = test_helpers.generate_random_test_quaternions(tensor_shape) x_2_init = test_helpers.generate_random_test_quaternions(tensor_shape) x_1 = tf.convert_to_tensor(value=x_1_init) x_2 = tf.convert_to_tensor(value=x_2_init) y = quaternion.multiply(x_1, x_2) self.assert_jacobian_is_correct(x_1, x_1_init, y) self.assert_jacobian_is_correct(x_2, x_2_init, y)
def generate_preset_test_dual_quaternions(): """Generates pre-set test quaternions.""" angles = generate_preset_test_euler_angles() preset_quaternion_real = quaternion.from_euler(angles) translations = generate_preset_test_translations() translations = np.concatenate( (translations / 2.0, np.zeros((np.ma.size(translations, 0), 1))), axis=1) preset_quaternion_translation = tf.convert_to_tensor(value=translations) preset_quaternion_dual = quaternion.multiply(preset_quaternion_translation, preset_quaternion_real) preset_dual_quaternion = tf.concat( (preset_quaternion_real, preset_quaternion_dual), axis=-1) return preset_dual_quaternion
def generate_random_test_dual_quaternions(): """Generates random test dual quaternions.""" angles = generate_random_test_euler_angles() random_quaternion_real = quaternion.from_euler(angles) min_translation = -3.0 max_translation = 3.0 translations = np.random.uniform(min_translation, max_translation, angles.shape) translations_quaternion_shape = np.asarray(translations.shape) translations_quaternion_shape[-1] = 1 translations = np.concatenate( (translations / 2.0, np.zeros(translations_quaternion_shape)), axis=-1) random_quaternion_translation = tf.convert_to_tensor(value=translations) random_quaternion_dual = quaternion.multiply(random_quaternion_translation, random_quaternion_real) random_dual_quaternion = tf.concat( (random_quaternion_real, random_quaternion_dual), axis=-1) return random_dual_quaternion
# actually want the center-to-edge distance, i.e. the 'square radius'. source_size /= 2 # ============================================================================= # Make the source. angle_type = "quaternion" if angle_type == "vector": source_angle = np.array((0.0, -source_y_offset, target_z_distance), dtype=np.float64) else: rot1 = quaternion.from_euler((0.0, -PI / 2, 0.0)) rot2 = quaternion.from_euler( tf.cast((math.atan2(source_y_offset, target_z_distance), 0.0, 0.0), dtype=tf.float64)) source_angle = quaternion.multiply(rot2, rot1) source_center = np.array( (source_x_offset, source_y_offset, -target_z_distance), dtype=np.float64) angular_distribution = distributions.SquareRankLambertianSphere( ray_sample_count, source_angular_cutoff) base_point_distribution = distributions.RandomUniformSquare( source_size, sqrt_ray_sample_count) source = sources.AngularSource(3, source_center, source_angle, angular_distribution, base_point_distribution, wavelengths, dense=False, ray_length=100,