def test_action_on_vectors_with_operator(): X = SE3((SO3.rot_x(3 * np.pi / 2), np.array([[1, 2, 3]]).T)) x = np.array([[-1, 0, 1]]).T vec_expected = np.array([[0, 3, 3]]).T np.testing.assert_almost_equal(X * x, vec_expected, 14)
def test_adjoint(): np.testing.assert_equal(SE3().adjoint(), np.identity(6)) X = SE3((SO3.rot_x(3 * np.pi / 2), np.array([[1, 2, 3]]).T)) Adj = X.adjoint() np.testing.assert_almost_equal(Adj[:3, :3], X.rotation.matrix, 14) np.testing.assert_almost_equal(Adj[3:, 3:], X.rotation.matrix, 14) np.testing.assert_almost_equal(Adj[:3, 3:], SO3.hat(X.translation) @ X.rotation.matrix, 14)
def test_composition_with_operator(): X = SE3((SO3.rot_x(3 * np.pi / 2), np.array([[1, 2, 3]]).T)) Y = SE3((SO3.rot_y(np.pi / 2), np.array([[-1, 0, 1]]).T)) Z = X @ Y rot_expected = SO3.from_angle_axis(2 * np.pi / 3, np.array([[-1, 1, -1]]).T / np.sqrt(3)) t_expected = np.array([[0, 3, 3]]).T np.testing.assert_almost_equal(Z.rotation.matrix, rot_expected.matrix, 14) np.testing.assert_almost_equal(Z.translation, t_expected, 14)
def test_composition(): X = SE3((SO3.rot_x(np.pi / 2), np.array([[1, 2, 3]]).T)) Y = SE3((SO3.rot_z(np.pi / 2), np.array([[0, 1, 0]]).T)) Z = X.compose(Y) rot_expected = SO3.from_angle_axis(2 * np.pi / 3, np.array([[1, -1, 1]]).T / np.sqrt(3)) t_expected = np.array([[1, 2, 4]]).T np.testing.assert_almost_equal(Z.rotation.matrix, rot_expected.matrix, 14) np.testing.assert_almost_equal(Z.translation, t_expected, 14)
def test_construct_with_roll_pitch_yaw(): np.testing.assert_almost_equal( SO3.from_roll_pitch_yaw(np.pi / 2, 0, 0).matrix, SO3.rot_x(np.pi / 2).matrix, 14) np.testing.assert_almost_equal( SO3.from_roll_pitch_yaw(0, np.pi / 2, 0).matrix, SO3.rot_y(np.pi / 2).matrix, 14) np.testing.assert_almost_equal( SO3.from_roll_pitch_yaw(0, 0, np.pi / 2).matrix, SO3.rot_z(np.pi / 2).matrix, 14) roll = np.pi pitch = np.pi / 2 yaw = np.pi so3 = SO3.from_roll_pitch_yaw(roll, pitch, yaw) expected = np.array([[0, 0, 1], [0, 1, 0], [-1, 0, 0]]) np.testing.assert_almost_equal(so3.matrix, expected, 14)
def test_rot_x_works(): rot_90_x = SO3.rot_x(0.5 * np.pi) expected_matrix = np.array([[1, 0, 0], [0, 0, -1], [0, 1, 0]]) np.testing.assert_almost_equal(rot_90_x.matrix, expected_matrix, 14)