def test_pose_error(self):
"""
Test rotation and translation error metric.
"""
self.ground_truth = ProjectScene.load(self.data_path,
'bounding_box_sample2')
self.submission = ProjectScene.load(self.data_path,
'bounding_box_sample2')
self.settings.thresholds = [0.5]
# verify that correct pose is ok
evaluator = BBEvaluator(self.submission, self.ground_truth,
self.settings)
rotation_error, translation_error = evaluator.pose_error()
self.assertAlmostEqual(rotation_error, 0)
self.assertAlmostEqual(translation_error, 0)
# verify that rotation by symmetry amount is ok
pose_orig = self.submission.elements["1069"].pose
new_pose = Pose3(R=pose_orig.R * Rot3.Ry(math.pi), t=pose_orig.t)
self.submission.elements["1069"].pose = new_pose
evaluator = BBEvaluator(self.submission, self.ground_truth,
self.settings)
rotation_error, translation_error = evaluator.pose_error()
self.assertAlmostEqual(rotation_error, 0)
self.assertAlmostEqual(translation_error, 0)
# verify that rotation by non-symmetry amount give correct error
new_pose = Pose3(R=pose_orig.R * Rot3.Ry(math.radians(10)),
t=pose_orig.t)
self.submission.elements["1069"].pose = new_pose
evaluator = BBEvaluator(self.submission, self.ground_truth,
self.settings)
rotation_error, translation_error = evaluator.pose_error()
self.assertAlmostEqual(rotation_error, math.radians(10))
self.assertAlmostEqual(translation_error, 0)
# verify that translation gives translation error
new_pose = Pose3(R=pose_orig.R, t=pose_orig.t + [0.05, 0, 0])
self.submission.elements["1069"].pose = new_pose
evaluator = BBEvaluator(self.submission, self.ground_truth,
self.settings)
rotation_error, translation_error = evaluator.pose_error()
self.assertAlmostEqual(rotation_error, 0)
self.assertAlmostEqual(translation_error, 0.05)
# verify that empty sumission gives None, None
new_pose = Pose3(R=pose_orig.R, t=pose_orig.t + [1, 0, 0])
self.submission.elements["1069"].pose = new_pose
evaluator = BBEvaluator(self.submission, self.ground_truth,
self.settings)
rotation_error, translation_error = evaluator.pose_error()
self.assertEqual(rotation_error, None)
self.assertEqual(translation_error, None)
def test_RollPitchYaw(self):
roll, pitch, yaw = 0.1, 0.2, 0.3
np.testing.assert_array_equal(
Rot3.Rx(roll).matrix(), Rot3.AxisAngle(UNIT_X, roll).matrix()
)
np.testing.assert_array_equal(
Rot3.Ry(pitch).matrix(), Rot3.AxisAngle(UNIT_Y, pitch).matrix()
)
np.testing.assert_array_equal(
Rot3.Rz(yaw).matrix(), Rot3.AxisAngle(UNIT_Z, yaw).matrix()
)
def test_ENU_camera_w_photosphere(self):
# Make sure we agree with conventions at
# https://developers.google.com/streetview/spherical-metadata
pitch = 0.1
roll = 0.1
wRt = Rot3.ENU_camera(pitch=pitch, roll=roll)
expected_wRr = Rot3.Rx(pitch) * Rot3.Ry(roll) # from rotated to ENU
# But our tilted frame uses the camera convention.
# TODO: maybe it should not! Why do we do this?
expected_wRt = Rot3(expected_wRr * ENU_R_CAMERA)
np.testing.assert_array_almost_equal(
wRt.matrix(), expected_wRt.matrix(), decimal=2
)
def euler_to_matrix(e):
"""
Convert ZYX Euler angles to 3x3 rotation matrix.
Inputs:
e (numpy 3-vector of float) - ZYX Euler angles (radians)
Return:
matrix (3x3 numpy array2 of float) - rotation matrix
TODO: This could be optimized somewhat by using the direct
equations for the final matrix rather than multiplying out the
matrices.
"""
return (Rot3.Rz(e[0]) * Rot3.Ry(e[1]) * Rot3.Rx(e[2])).R
def test_quat_matrix(self):
rx = 10 # degrees
ry = 20
rz = 30
Rx = Rot3.Rx(math.radians(rx))
Ry = Rot3.Ry(math.radians(ry))
Rz = Rot3.Rz(math.radians(rz))
# matrix -> quat
R = Rz * Ry * Rx
q = utils.matrix_to_quat(R.R)
expected_q = np.array([0.9515485, 0.0381346, 0.1893079,
0.2392983]) # computed manually
np.testing.assert_array_almost_equal(q, expected_q, 4)
# quat -> matrix
R2 = utils.quat_to_matrix(expected_q)
np.testing.assert_array_almost_equal(R2, R.R, 4)
# round trip
R2 = utils.quat_to_matrix(q)
np.testing.assert_array_almost_equal(R.R, R2, 4)
