def test_from_axis_angle_random(self):
"""Tests converting an axis-angle to a quaternion."""
random_euler_angles = test_helpers.generate_random_test_euler_angles()
axis, angle = axis_angle.from_euler(random_euler_angles)
grountruth = rotation_matrix_3d.from_quaternion(
quaternion.from_euler(random_euler_angles))
prediction = rotation_matrix_3d.from_quaternion(
quaternion.from_axis_angle(axis, angle))
self.assertAllClose(grountruth, prediction, rtol=1e-3)
def test_from_rotation_matrix_random(self):
"""Tests that from_rotation_matrix produces the expected quaternions."""
random_euler_angles = test_helpers.generate_random_test_euler_angles()
random_rotation_matrix_3d = rotation_matrix_3d.from_euler(
random_euler_angles)
groundtruth = rotation_matrix_3d.from_quaternion(
quaternion.from_euler(random_euler_angles))
prediction = rotation_matrix_3d.from_quaternion(
quaternion.from_rotation_matrix(random_rotation_matrix_3d))
self.assertAllClose(groundtruth, prediction)
def test_from_quaternion_jacobian_random(self):
"""Test the Jacobian of the from_quaternion function."""
x_init = test_helpers.generate_random_test_quaternions()
x = tf.convert_to_tensor(value=x_init)
y = rotation_matrix_3d.from_quaternion(x)
self.assert_jacobian_is_correct(x, x_init, y)
def test_from_quaternion_preset(self):
"""Tests that a quaternion maps to correct matrix."""
preset_quaternions = test_helpers.generate_preset_test_quaternions()
preset_matrices = test_helpers.generate_preset_test_rotation_matrices_3d()
self.assertAllClose(preset_matrices,
rotation_matrix_3d.from_quaternion(preset_quaternions))
def test_from_quaternion_normalized_random(self):
"""Tests that random quaternions can be converted to rotation matrices."""
random_quaternion = test_helpers.generate_random_test_quaternions()
tensor_shape = random_quaternion.shape[:-1]
random_matrix = rotation_matrix_3d.from_quaternion(random_quaternion)
self.assertAllEqual(rotation_matrix_3d.is_valid(random_matrix),
np.ones(tensor_shape + (1, )))
def test_from_quaternion_normalized_preset(self):
"""Tests that quaternions can be converted to rotation matrices."""
euler_angles = test_helpers.generate_preset_test_euler_angles()
quat = quaternion.from_euler(euler_angles)
matrix_quat = rotation_matrix_3d.from_quaternion(quat)
self.assertAllEqual(rotation_matrix_3d.is_valid(matrix_quat),
np.ones(euler_angles.shape[0:-1] + (1, )))
def test_from_quaternion_preset(self):
"""Checks that Euler angles can be retrieved from quaternions."""
preset_euler_angles = test_helpers.generate_preset_test_euler_angles()
preset_matrix = rotation_matrix_3d.from_euler(preset_euler_angles)
preset_quaternion = quaternion.from_euler(preset_euler_angles)
predicted_matrix = rotation_matrix_3d.from_quaternion(preset_quaternion)
self.assertAllClose(preset_matrix, predicted_matrix, atol=2e-3)
def test_from_quaternion_random(self):
"""Tests conversion to matrix."""
random_euler_angles = test_helpers.generate_random_test_euler_angles()
random_quaternions = quaternion.from_euler(random_euler_angles)
random_rotation_matrices = rotation_matrix_3d.from_euler(
random_euler_angles)
self.assertAllClose(random_rotation_matrices,
rotation_matrix_3d.from_quaternion(random_quaternions))
def test_from_quaternion_gimbal(self, gimbal_configuration):
"""Checks that from_quaternion works when Ry = pi/2 or -pi/2."""
random_euler_angles = test_helpers.generate_random_test_euler_angles()
random_euler_angles[..., 1] = gimbal_configuration
random_quaternion = quaternion.from_euler(random_euler_angles)
random_matrix = rotation_matrix_3d.from_euler(random_euler_angles)
reconstructed_random_matrices = rotation_matrix_3d.from_quaternion(
random_quaternion)
self.assertAllClose(reconstructed_random_matrices, random_matrix, atol=2e-3)
def test_rotate_random(self):
"""Tests the rotation using a quaternion vs a rotation matrix."""
random_quaternion = test_helpers.generate_random_test_quaternions()
tensor_shape = random_quaternion.shape[:-1]
random_point = np.random.normal(size=tensor_shape + (3,))
rotated_point_quaternion = quaternion.rotate(random_point,
random_quaternion)
matrix = rotation_matrix_3d.from_quaternion(random_quaternion)
rotated_point_matrix = rotation_matrix_3d.rotate(random_point, matrix)
self.assertAllClose(
rotated_point_matrix, rotated_point_quaternion, rtol=1e-3)
def _combined_loss_function(self, y_true, y_pred):
############################
# THIS IS A WORK IN PROGRESS
############################
# extract t,R errors
error = K.square(y_pred[:, 0:7] - y_true[:, 0:7])
translation_error = K.sqrt(error[0] + error[1] + error[2])
orientation_quaternion_error = K.sqrt(error[3] + error[4] + error[5] + error[6])
# extract camera matrices P1,P2
P1 = [[1, 0, 0, 0], [0, 1, 0, 0], [0, 0, 1, 0]]
orientation_matrix = rm3.from_quaternion(y_pred[:,3:7])
P2 = tensorflow.apend(orientation_matrix, translation_error, axis=1)
P2_1 = P2[1, :]
P2_2 = P2[2, :]
P2_3 = P2[3, :]
K1 = y_pred[:,7:17]
K1 = tensorflow.reshape(K1,[-1,3,3])
K2 = y_pred[:, 17:27]
K2 = tensorflow.reshape(K2, [-1,3, 3])
x1 = y_pred[:,27:327]
x1 = tensorflow.reshape(x1,[-1,3,100])
x2 = y_pred[:,327:627]
x2 = tensorflow.reshape(x2,[-1,3,100])
# normalize the given 2D points and cameras
x1normalized = tensorflow.linalg.inv(K1) * x1
x2normalized = tensorflow.linalg.inv(K2) * x2
P1normalized = tensorflow.linalg.inv(K1) * P1
P2normalized = tensorflow.linalg.inv(K2) * P2
# Triangulate the 3D points using the two camera matrices and the givehttps://www.mat.univie.ac.at/~gerald/ftp/book-mfi/mfi1.pdfn 2D points
X = tensorflow.zeros([4, x1.len]) # this is the array of 3D points
for i in range(x1.len):
Mi = np.zeros([6, 6])
Mi[:6, :4] = np.append(P1normalized, P2normalized, axis=0)
Mi[:, 5] = [[-x1normalized[:, i]], [0], [0], [0]]
Mi[:, 6] = [[0], [0], [0], -x2normalized[:, i]]
U, s, VT = svd(Mi)
solution = VT[:, -1]
X[:, i] = self.pflat(solution[1:4])
# sum the reprojection error over all reprojected points relative to given 2D points of camera 2
reprojection_error = 0
for j in range(X.size):
reprojection_error += K.sqrt(
K.square(x1[j] - (P2_1 * X[j]) / (P2_3 * X[j])) + K.square(x2[j] - (P2_2 * X[j]) / (P2_3 * X[j])))
return K.mean(translation_error + (self._beta * orientation_quaternion_error) + (self._gamma * reprojection_error))
def _combined_loss_fucntion2(self, y_true, y_pred):
# extract t,R errors
error = K.square(y_pred[:, 0:7] - y_true[:, 0:7])
translation_error = K.sqrt(error[0] + error[1] + error[2])
orientation_quaternion_error = K.sqrt(error[3] + error[4] + error[5] + error[6])
# extract K1,K2, and all point matches from the prediction
# which were passed through the model
K1 = y_pred[:,7:16]
K1 = tensorflow.reshape(K1,[-1,3,3])
K2 = y_pred[:, 16:25]
K2 = tensorflow.reshape(K2, [-1,3, 3])
x1 = y_pred[:,25:325]
x1 = tensorflow.reshape(x1,[-1,3,100])
x2 = y_pred[:,325:625]
x2 = tensorflow.reshape(x2,[-1,3,100])
# calculate fundamental matrix
t = y_pred[:,0:3]
R = rm3.from_quaternion(y_pred[:,3:7])
# calculate skew matrix by taking the cross product with the normal basis
# t_skew = [e1xt e2xt e3xt]
e1 = tensorflow.repeat([1., 0, 0], repeats=1, axis=0)
e2 = tensorflow.repeat([0, 1., 0], repeats=1, axis=0)
e3 = tensorflow.repeat([0, 0, 1.], repeats=1, axis=0)
t_skew = tensorflow.stack([tensorflow.map_fn(lambda x: tensorflow.linalg.cross(x,e1), t),
tensorflow.map_fn(lambda x: tensorflow.linalg.cross(x, e2), t),
tensorflow.map_fn(lambda x: tensorflow.linalg.cross(x, e3), t)], axis=1)
# Calculate the essential matrix from the prediction
E = tensorflow.linalg.matmul(t_skew, R)
K2_Tinv = tensorflow.linalg.inv(tensorflow.transpose(K2,perm=[0,2,1]))
K1_inv = tensorflow.linalg.inv(K1)
# Calculate the fundamental matrix
F = tensorflow.matmul(tensorflow.matmul(K2_Tinv, E), K1_inv)
# calculate the epipolar constraint error
step = tensorflow.linalg.matmul(tensorflow.transpose(x1, perm=[0,2,1]), F)
constraint_error = K.sqrt(tensorflow.math.reduce_sum(K.square(tensorflow.linalg.diag_part(tensorflow.linalg.matmul(step, x2)))))
return K.mean(translation_error + (self._beta * orientation_quaternion_error) + (self._gamma * constraint_error))
def test_from_axis_angle_random(self):
"""Tests conversion to matrix."""
tensor_shape = np.random.randint(1, 10, size=np.random.randint(3)).tolist()
random_axis = np.random.normal(size=tensor_shape + [3])
random_axis /= np.linalg.norm(random_axis, axis=-1, keepdims=True)
random_angle = np.random.normal(size=tensor_shape + [1])
matrix_axis_angle = rotation_matrix_3d.from_axis_angle(
random_axis, random_angle)
random_quaternion = quaternion.from_axis_angle(random_axis, random_angle)
matrix_quaternion = rotation_matrix_3d.from_quaternion(random_quaternion)
self.assertAllClose(matrix_axis_angle, matrix_quaternion, rtol=1e-3)
# Checks that resulting rotation matrices are normalized.
self.assertAllEqual(
rotation_matrix_3d.is_valid(matrix_axis_angle),
np.ones(tensor_shape + [1]))
