def test_difference_for_simple_rotation_with_operator_works():
X = SO2()
theta = 3 * np.pi / 4
Y = SO2.Exp(theta)
theta_diff = Y - X
np.testing.assert_almost_equal(theta_diff, theta, 14)
def test_difference_for_simple_rotation_works():
X = SO2()
theta = np.pi / 3
Y = SO2.Exp(theta)
theta_diff = Y.ominus(X)
np.testing.assert_array_equal(theta_diff, theta)
def test_oplus_with_ominus_diff():
X = SE2((SO2(-np.pi / 5), np.array([[2, 1]]).T))
Y = SE2((SO2(np.pi / 7), np.array([[1, 0]]).T))
xi_vec_diff = Y.ominus(X)
Y_from_X = X.oplus(xi_vec_diff)
np.testing.assert_almost_equal(Y_from_X.to_matrix(), Y.to_matrix(), 14)
def test_composition_returns_correct_rotation():
so2_180 = SO2(np.pi)
so2_90 = SO2(np.pi / 2)
so2_comp = so2_180.compose(so2_90)
expected = np.array([[0, 1], [-1, 0]])
np.testing.assert_almost_equal(so2_comp.to_matrix(), expected, 14)
def test_jacobian_right():
theta = 3 * np.pi / 4
J_r = SO2.jac_right(theta)
# Test the Jacobian numerically.
delta = 1e-3 * np.ones((1, 1))
taylor_diff = SO2.Exp(theta + delta) - (SO2.Exp(theta) + J_r @ delta)
np.testing.assert_almost_equal(taylor_diff, 0.0, 5)
def test_adjoint():
np.testing.assert_equal(SE2().adjoint(), np.identity(3))
X = SE2((SO2(3 * np.pi / 2), np.array([[1, 2]]).T))
Adj = X.adjoint()
np.testing.assert_almost_equal(Adj[:2, :2], X.rotation.to_matrix(), 14)
np.testing.assert_almost_equal(Adj[:2, 2:3], -SO2.hat(1.0) @ X.translation,
14)
np.testing.assert_almost_equal(Adj[2, :], np.array([0, 0, 1]), 14)
def test_composition_with_identity_works():
so2 = SO2(np.pi / 4)
comp_with_identity = so2.compose(SO2())
comp_from_identity = SO2().compose(so2)
np.testing.assert_almost_equal(comp_with_identity.to_matrix(),
so2.to_matrix(), 14)
np.testing.assert_almost_equal(comp_from_identity.to_matrix(),
so2.to_matrix(), 14)
def test_composition_with_operator():
X = SE2((SO2(np.pi), np.array([[1, 2]]).T))
Y = SE2((SO2(-np.pi / 2), np.array([[-1, 0]]).T))
Z = X @ Y
rot_expected = SO2(np.pi / 2)
t_expected = np.array([[2, 2]]).T
np.testing.assert_almost_equal(Z.rotation.to_matrix(),
rot_expected.to_matrix(), 14)
np.testing.assert_almost_equal(Z.translation, t_expected, 14)
def test_composition():
X = SE2((SO2(-np.pi / 2), np.array([[1, 2]]).T))
Y = SE2((SO2(3 * np.pi / 2), np.array([[0, 1]]).T))
Z = X.compose(Y)
rot_expected = SO2(np.pi)
t_expected = np.array([[2, 2]]).T
np.testing.assert_almost_equal(Z.rotation.to_matrix(),
rot_expected.to_matrix(), 14)
np.testing.assert_almost_equal(Z.translation, t_expected, 14)
def test_jacobian_action_Xx_wrt_X():
X = SO2(3 * np.pi / 4)
x = np.array([[1, 2]]).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.to_matrix() @ SO2.hat(1.0) @ x)
# Test the Jacobian numerically.
delta = 1e-3 * np.ones((1, 1))
taylor_diff = X.oplus(delta).action(x) - (X.action(x) + J_action_X @ delta)
np.testing.assert_almost_equal(taylor_diff, 0.0, 5)
def test_jacobian_composition_XY_wrt_Y():
X = SE2((SO2(np.pi / 10), np.array([[3, 2]]).T))
Y = SE2((SO2(np.pi / 7), np.array([[1, 0]]).T))
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_composition_XY_wrt_X():
X = SO2(np.pi / 4)
Y = SO2(np.pi / 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((1, 1))
taylor_diff = X.oplus(delta).compose(Y) - X.compose(Y).oplus(
J_comp_X @ delta)
np.testing.assert_almost_equal(taylor_diff, 0, 14)
def test_jacobian_Y_ominus_X_wrt_Y():
X = SE2((SO2(np.pi / 8), np.array([[1, 1]]).T))
Y = SE2((SO2(np.pi / 7), np.array([[2, 0]]).T))
J_ominus_Y = Y.jac_Y_ominus_X_wrt_Y(X)
# Should be J_r_inv.
np.testing.assert_equal(J_ominus_Y, SE2.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_right_inverse():
theta = 3 * np.pi / 4
X = SO2.Exp(theta)
J_r_inv = SO2.jac_right_inverse(theta)
# Should have J_l * J_r_inv = Exp(theta).adjoint().
J_l = SO2.jac_left(theta)
np.testing.assert_almost_equal(J_l @ J_r_inv, SO2.Exp(theta).adjoint(), 14)
# Test the Jacobian numerically.
delta = 1e-3 * np.ones((1, 1))
taylor_diff = X.oplus(delta).Log() - (X.Log() + J_r_inv @ delta)
np.testing.assert_almost_equal(taylor_diff, 0.0, 5)
def test_jacobian_composition_XY_wrt_Y():
X = SO2(np.pi / 4)
Y = SO2(-np.pi / 3)
J_comp_Y = X.jac_composition_XY_wrt_Y()
# Jacobian should be identity
np.testing.assert_array_equal(J_comp_Y, 1.0)
# Test the Jacobian numerically.
delta = 1e-3 * np.ones((1, 1))
taylor_diff = X.compose(Y.oplus(delta)) - X.compose(Y).oplus(
J_comp_Y @ delta)
np.testing.assert_almost_equal(taylor_diff, 0.0, 14)
def test_jacobian_Y_ominus_X_wrt_Y():
X = SO2(np.pi / 4)
Y = SO2(np.pi / 3)
J_ominus_Y = Y.jac_Y_ominus_X_wrt_Y(X)
# Should be J_r_inv.
np.testing.assert_equal(J_ominus_Y, SO2.jac_right_inverse(Y - X))
# Test the Jacobian numerically.
delta = 1e-3 * np.ones((1, 1))
taylor_diff = Y.oplus(delta).ominus(X) - (Y.ominus(X) +
(J_ominus_Y @ delta))
np.testing.assert_almost_equal(taylor_diff, 0.0, 6)
def test_jacobian_Y_ominus_X_wrt_X():
X = SO2(np.pi / 4)
Y = SO2(np.pi / 2)
J_ominus_X = Y.jac_Y_ominus_X_wrt_X(X)
# Should be -J_l_inv.
np.testing.assert_equal(J_ominus_X, -SO2.jac_left_inverse(Y - X))
# Test the Jacobian numerically.
delta = 1e-3 * np.ones((1, 1))
taylor_diff = Y.ominus(X.oplus(delta)) - (Y.ominus(X) +
(J_ominus_X @ delta))
np.testing.assert_almost_equal(taylor_diff, 0.0, 6)
def test_jacobian_X_oplus_tau_wrt_X():
X = SO2(3 * np.pi / 4)
theta = np.pi / 4
J_oplus_X = X.jac_X_oplus_tau_wrt_X(theta)
# Should be Exp(tau).adjoint().inverse()
np.testing.assert_almost_equal(J_oplus_X, SO2.Exp(theta).adjoint(), 14)
# Test the Jacobian numerically.
delta = 1e-3 * np.ones((1, 1))
taylor_diff = X.oplus(delta).oplus(theta) - X.oplus(theta).oplus(
J_oplus_X @ delta)
np.testing.assert_almost_equal(taylor_diff, 0.0, 14)
def test_jacobian_composition_XY_wrt_X():
X = SE2((SO2(np.pi / 10), np.array([[2, 1]]).T))
Y = SE2((SO2(np.pi / 7), np.array([[1, 0]]).T))
J_comp_X = X.jac_composition_XY_wrt_X(Y)
# Jacobian should be Y.inverse().adjoint()
np.testing.assert_almost_equal(J_comp_X, Y.inverse().adjoint(), 14)
# 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_construct_from_matrix():
theta = np.pi / 3
R = SO2(theta).to_matrix()
so2 = SO2.from_matrix(R)
np.testing.assert_equal(so2.angle, theta)
np.testing.assert_array_equal(so2.to_matrix(), R)
# Matrices not elements of SO(2) should be fitted to SO(2)
R_noisy = R + 0.1 + np.random.rand(2)
so2_fitted = SO2.from_matrix(R_noisy)
R_fitted = so2_fitted.to_matrix()
assert np.any(np.not_equal(R_fitted, R_noisy))
np.testing.assert_almost_equal(np.linalg.det(R_fitted), 1, 14)
np.testing.assert_almost_equal((R_fitted.T @ R_fitted), np.identity(2), 14)
def test_construct_with_tuple():
so2 = SO2(0.1)
t = np.array([[1, 2]]).T
se2 = SE2((so2, t))
np.testing.assert_equal(se2.rotation.to_matrix(), so2.to_matrix())
np.testing.assert_equal(se2.translation, t)
def __init__(self, pose_tuple=(SO2(), np.zeros((2, 1)))):
"""Constructs an SE(2) element.
The default is the identity element.
:param pose_tuple: A tuple (rotation (SO2), translation (2D column vector) (optional).
"""
self.rotation, self.translation = pose_tuple
def test_inverse_returns_transposed():
so2 = SO2(np.pi / 4)
so2_inv = so2.inverse()
np.testing.assert_almost_equal(so2_inv.to_matrix(), so2.to_matrix().T, 14)
np.testing.assert_almost_equal(so2_inv.inverse().to_matrix(),
so2.to_matrix(), 14)
def test_hat_returns_skew_symmetric_matrix():
theta = 1.0
theta_hat = SO2.hat(theta)
assert theta_hat[0, 0] == 0
assert theta_hat[0, 1] == -theta
assert theta_hat[1, 0] == theta
assert theta_hat[1, 1] == 0
def Exp(xi_vec):
"""Computes the Exp-map on the Lie algebra vector xi_vec,
which transfers it to the corresponding Lie group element.
:param xi_vec: 3D tangent space column vector xi_vec = [rho_vec, theta]^T.
:return: Corresponding SE(2) element
"""
rho_vec = xi_vec[:2]
theta = xi_vec[2].item()
if np.abs(theta) < 1e-10:
return SE2((SO2(theta), rho_vec))
V = (np.sin(theta) / theta) * np.identity(2) + (
(1 - np.cos(theta)) / theta) * SO2.hat(1)
return SE2((SO2(theta), V @ rho_vec))
def test_action_on_vectors_with_operator():
X = SE2((SO2(3 * np.pi / 2), np.array([[1, 2]]).T))
x = np.array([[-1, 0]]).T
vec_expected = np.array([[1, 3]]).T
np.testing.assert_almost_equal(X * x, vec_expected, 14)
def test_action_on_vectors():
unit_x = np.array([[1, 0]]).T
unit_y = np.array([[0, 1]]).T
t = np.array([[3, 1]]).T
X = SE2((SO2(np.pi / 2), t))
np.testing.assert_almost_equal(X.action(unit_x), unit_y + t, 14)
def test_hat():
rho_vec = np.array([[1, 2]]).T
theta = np.pi / 3
xi_vec = np.vstack((rho_vec, theta))
xi_hat_expected = np.block([[SO2.hat(theta), rho_vec], [np.zeros((1, 3))]])
np.testing.assert_equal(SE2.hat(xi_vec), xi_hat_expected)
def test_to_tuple():
so2 = SO2(-np.pi / 3)
t = np.array([[-1, 1]]).T
se2 = SE2((so2, t))
pose_tuple = se2.to_tuple()
np.testing.assert_equal(pose_tuple[0], so2.to_matrix())
np.testing.assert_equal(pose_tuple[1], t)
def test_jacobian_action_Xx_wrt_X():
X = SE2((SO2(np.pi / 8), np.array([[1, 1]]).T))
x = np.array([[1, 2]]).T
J_action_X = X.jac_action_Xx_wrt_X(x)
# Jacobian should be [R, R*SO3.hat(1)*x].
np.testing.assert_array_equal(
J_action_X,
np.block(
[[X.rotation.to_matrix(),
X.rotation.to_matrix() @ SO2.hat(1) @ 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((2, 1)), 5)