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_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():
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_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_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_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_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_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_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 __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_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_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_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_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 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_composition_with_operator_works():
so2_45 = SO2(np.pi / 4)
so2_comp = so2_45 @ so2_45
expected = np.array([[0, -1], [1, 0]])
np.testing.assert_almost_equal(so2_comp.to_matrix(), expected, 14)
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_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_to_matrix():
so2 = SO2(np.pi / 4)
t = np.array([[3, 2]]).T
se2 = SE2((so2, t))
T = se2.to_matrix()
np.testing.assert_equal(T[0:2, 0:2], so2.to_matrix())
np.testing.assert_equal(T[0:2, 2:3], t)
np.testing.assert_equal(T[2:, :], np.array([[0, 0, 1]]))
def test_composition_with_identity():
X = SE2((SO2(np.pi / 10), np.array([[3, 1]]).T))
comp_with_identity = X.compose(SE2())
comp_from_identity = SE2().compose(X)
np.testing.assert_almost_equal(comp_with_identity.to_matrix(),
X.to_matrix(), 14)
np.testing.assert_almost_equal(comp_from_identity.to_matrix(),
X.to_matrix(), 14)
def test_jacobian_left_inverse():
X = SE2((SO2(np.pi / 8), np.array([[2, 1]]).T))
xi_vec = X.Log()
J_l_inv = SE2.jac_left_inverse(xi_vec)
# Test the Jacobian numerically (using Exps and Logs, since left oplus and ominus have not been defined).
delta = 1e-3 * np.ones((3, 1))
taylor_diff = (SE2.Exp(delta) @ X).Log() - (X.Log() + J_l_inv @ delta)
np.testing.assert_almost_equal(taylor_diff, np.zeros((3, 1)), 5)
def test_log_with_no_rotation():
X = SE2((SO2(), np.array([[-1, 2]]).T))
xi_vec = X.Log()
# Translation part:
np.testing.assert_equal(xi_vec[:2], X.translation)
# Rotation part:
np.testing.assert_equal(xi_vec[2].item(), 0)
def test_construct_with_matrix():
so2 = SO2(np.pi / 10)
t = np.array([[1, 2]]).T
T = np.block([[so2.to_matrix(), t], [0, 0, 1]])
se2 = SE2.from_matrix(T)
np.testing.assert_almost_equal(se2.rotation.to_matrix(), so2.to_matrix(),
14)
np.testing.assert_equal(se2.translation, t)
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_inverse():
X = SE2((SO2(np.pi / 4), np.array([[1, -2]]).T))
X_inv = X.inverse()
np.testing.assert_equal(X_inv.rotation.to_matrix(),
X.rotation.inverse().to_matrix())
np.testing.assert_equal(X_inv.translation,
-(X.rotation.inverse() * X.translation))
np.testing.assert_almost_equal((X @ X_inv).to_matrix(), np.identity(3))
def test_ominus_with_oplus_diff_with_operators():
X = SE2((SO2(np.pi / 7), np.array([[1, 0]]).T))
rho_vec = np.array([[1, 3]]).T
theta = 2 * np.pi / 3
xi_vec = np.vstack((rho_vec, theta))
Y = X + xi_vec
xi_vec_diff = Y - X
np.testing.assert_almost_equal(xi_vec_diff, xi_vec, 14)
