def test_composition_with_operator_works():
so3_90_z = SO3.from_angle_axis(np.pi / 2, np.array([[0, 0, 1]]).T)
so3_n90_x = SO3.from_angle_axis(np.pi / 2, np.array([[-1, 0, 0]]).T)
so3_comp = so3_n90_x @ so3_90_z
expected = np.array([[0, -1, 0], [0, 0, 1], [-1, 0, 0]])
np.testing.assert_almost_equal(so3_comp.matrix, expected, 14)
def test_composition_returns_correct_rotation():
so3_90_x = SO3.from_angle_axis(np.pi / 2, np.array([[1, 0, 0]]).T)
so3_90_y = SO3.from_angle_axis(np.pi / 2, np.array([[0, 1, 0]]).T)
so3_comp = so3_90_y.compose(so3_90_x)
expected = np.array([[0, 1, 0], [0, 0, -1], [-1, 0, 0]])
np.testing.assert_almost_equal(so3_comp.matrix, expected, 14)
def test_jacobian_composition_XY_wrt_Y():
X = SO3.from_angle_axis(np.pi / 4, np.array([[1, -1, 1]]).T / np.sqrt(3))
Y = SO3.from_angle_axis(np.pi / 2, np.array([[1, 0, 1]]).T / np.sqrt(2))
J_comp_Y = X.jac_composition_XY_wrt_Y()
# Jacobian should be identity
np.testing.assert_array_equal(J_comp_Y, np.identity(3))
# Test the Jacobian numerically.
delta = 1e-3 * np.ones((3, 1))
taylor_diff = X.compose(Y.oplus(delta)) - X.compose(Y).oplus(
J_comp_Y @ delta)
np.testing.assert_almost_equal(taylor_diff, np.zeros((3, 1)), 14)
def test_jacobian_Y_ominus_X_wrt_Y():
X = SO3.from_angle_axis(np.pi / 4, np.array([[1, 1, 1]]).T / np.sqrt(3))
Y = SO3.from_angle_axis(np.pi / 3, np.array([[1, 0, -1]]).T / np.sqrt(2))
J_ominus_Y = Y.jac_Y_ominus_X_wrt_Y(X)
# Should be J_r_inv.
np.testing.assert_equal(J_ominus_Y, SO3.jac_right_inverse(Y - X))
# Test the Jacobian numerically.
delta = 1e-3 * np.ones((3, 1))
taylor_diff = Y.oplus(delta).ominus(X) - (Y.ominus(X) +
(J_ominus_Y @ delta))
np.testing.assert_almost_equal(taylor_diff, np.zeros((3, 1)), 6)
def test_jacobian_composition_XY_wrt_X():
X = SO3.from_angle_axis(np.pi / 4, np.array([[1, -1, 1]]).T / np.sqrt(3))
Y = SO3.from_angle_axis(np.pi / 2, np.array([[1, 0, 1]]).T / np.sqrt(2))
J_comp_X = X.jac_composition_XY_wrt_X(Y)
# Jacobian should be Y.inverse().adjoint()
np.testing.assert_array_equal(J_comp_X, Y.inverse().adjoint())
# Test the Jacobian numerically.
delta = 1e-3 * np.ones((3, 1))
taylor_diff = X.oplus(delta).compose(Y) - X.compose(Y).oplus(
J_comp_X @ delta)
np.testing.assert_almost_equal(taylor_diff, np.zeros((3, 1)), 14)
def test_action_on_vector_works():
unit_x = np.array([[1, 0, 0]]).T
unit_y = np.array([[0, 1, 0]]).T
unit_z = np.array([[0, 0, 1]]).T
so3 = SO3.from_angle_axis(np.pi / 2, unit_y)
np.testing.assert_almost_equal(so3.action(unit_x), -unit_z, 14)
def test_composition_with_identity_works():
so3 = SO3.from_angle_axis(np.pi / 4, np.array([[0, 1, 0]]).T)
comp_with_identity = so3.compose(SO3())
comp_from_identity = SO3().compose(so3)
np.testing.assert_almost_equal(comp_with_identity.matrix, so3.matrix, 14)
np.testing.assert_almost_equal(comp_from_identity.matrix, so3.matrix, 14)
def test_construct_with_angle_axis():
angle = 3 * np.pi / 4
axis = np.array([[1, 1, -1]]) / np.sqrt(3)
theta_vec = angle * axis
so3 = SO3.from_angle_axis(angle, axis)
np.testing.assert_array_equal(so3.matrix, SO3.Exp(theta_vec).matrix)
def test_action_on_vectors():
unit_x = np.array([[1, 0, 0]]).T
unit_z = np.array([[0, 0, 1]]).T
t = np.array([[3, 2, 1]]).T
X = SE3((SO3.from_angle_axis(np.pi / 2, np.array([[0, 1, 0]]).T), t))
np.testing.assert_almost_equal(X.action(unit_x), -unit_z + t, 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_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_jacobian_inverse():
X = SO3.from_angle_axis(np.pi / 3, np.array([[1, -1, -1]]).T / np.sqrt(3))
J_inv = X.jac_inverse_X_wrt_X()
# Jacobian should be -X.adjoint().
np.testing.assert_array_equal(J_inv, -X.adjoint())
# Test the Jacobian numerically.
delta = 1e-3 * np.ones((3, 1))
taylor_diff = X.oplus(delta).inverse() - X.inverse().oplus(J_inv @ delta)
np.testing.assert_almost_equal(taylor_diff, np.zeros((3, 1)), 14)
def test_jacobian_X_oplus_tau_wrt_tau():
X = SO3.from_angle_axis(np.pi / 4, np.array([[-1, -1, -1]]).T / np.sqrt(3))
theta_vec = np.pi / 3 * np.array([[-1, -1, 1]]).T / np.sqrt(3)
J_oplus_tau = X.jac_X_oplus_tau_wrt_tau(theta_vec)
# Should be J_r.
np.testing.assert_equal(J_oplus_tau, X.jac_right(theta_vec))
# Test the Jacobian numerically.
delta = 1e-3 * np.ones((3, 1))
taylor_diff = X.oplus(theta_vec + delta) - X.oplus(theta_vec).oplus(
J_oplus_tau @ delta)
np.testing.assert_almost_equal(taylor_diff, np.zeros((3, 1)), 6)
def test_jacobian_action_Xx_wrt_x():
X = SO3.from_angle_axis(3 * np.pi / 4,
np.array([[1, -1, 1]]).T / np.sqrt(3))
x = np.array([[1, 2, 3]]).T
J_action_x = X.jac_action_Xx_wrt_x()
# Jacobian should be X.matrix.
np.testing.assert_array_equal(J_action_x, X.matrix)
# Test the Jacobian numerically.
delta = 1e-3 * np.ones((3, 1))
taylor_diff = X.action(x + delta) - (X.action(x) + J_action_x @ delta)
np.testing.assert_almost_equal(taylor_diff, np.zeros((3, 1)), 14)
def test_jacobian_action_Xx_wrt_X():
X = SO3.from_angle_axis(3 * np.pi / 4,
np.array([[1, -1, 1]]).T / np.sqrt(3))
x = np.array([[1, 2, 3]]).T
J_action_X = X.jac_action_Xx_wrt_X(x)
# Jacobian should be -X.matrix * SO3.hat(x).
np.testing.assert_array_equal(J_action_X, -X.matrix @ SO3.hat(x))
# Test the Jacobian numerically.
delta = 1e-3 * np.ones((3, 1))
taylor_diff = X.oplus(delta).action(x) - (X.action(x) + J_action_X @ delta)
np.testing.assert_almost_equal(taylor_diff, np.zeros((3, 1)), 5)
def test_jacobian_X_oplus_tau_wrt_X():
X = SO3.from_angle_axis(3 * np.pi / 4,
np.array([[1, -1, 1]]).T / np.sqrt(3))
theta_vec = np.pi / 4 * np.array([[-1, -1, 1]]).T / np.sqrt(3)
J_oplus_X = X.jac_X_oplus_tau_wrt_X(theta_vec)
# Should be Exp(tau).adjoint().inverse()
np.testing.assert_almost_equal(J_oplus_X,
np.linalg.inv(SO3.Exp(theta_vec).adjoint()),
14)
# Test the Jacobian numerically.
delta = 1e-3 * np.ones((3, 1))
taylor_diff = X.oplus(delta).oplus(theta_vec) - X.oplus(theta_vec).oplus(
J_oplus_X @ delta)
np.testing.assert_almost_equal(taylor_diff, np.zeros((3, 1)), 14)
def test_inverse_returns_transposed():
so3 = SO3.from_angle_axis(np.pi / 4, np.array([[1, 1, 0]]).T / np.sqrt(2))
so3_inv = so3.inverse()
np.testing.assert_array_equal(so3_inv.matrix, so3.matrix.T)
np.testing.assert_array_equal(so3_inv.inverse().matrix, so3.matrix)
def test_action_on_vector_with_operator_works():
unit_x = np.array([[1, 0, 0]]).T
so3 = SO3.from_angle_axis(np.pi, np.array([[0, 0, -1]]).T)
np.testing.assert_almost_equal(so3 * unit_x, -unit_x, 14)
def test_adjoint_returns_rotation_matrix():
so3_1 = SO3()
so3_2 = SO3.from_angle_axis(3 * np.pi / 4, np.array([[1, 0, 0]]).T)
np.testing.assert_array_equal(so3_1.adjoint(), so3_1.matrix)
np.testing.assert_array_equal(so3_2.adjoint(), so3_2.matrix)
