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)