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 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 adjoint(self):
"""The adjoint at the element.
:return: The adjoint, a 3x3 matrix.
"""
R = self.rotation.to_matrix()
t = self.translation
return np.block([[R, -SO2.hat(1.0) @ t], [np.zeros((1, 2)), 1.0]])
def jac_composition_XY_wrt_X(Y):
"""Computes the Jacobian of the composition X.compose(Y) with respect to the element X.
:param Y: SE2 element Y
:return: The Jacobian (3x3 matrix)
"""
R_Y_inv = Y.rotation.inverse().to_matrix()
return np.block([[R_Y_inv, (R_Y_inv @ SO2.hat(1.0) @ Y.translation)],
[np.array([0, 0, 1])]])
def hat(xi_vec):
"""Performs the hat operator on the tangent space vector xi_vec,
which returns the corresponding Lie Algebra matrix xi_hat.
:param xi_vec: 3D tangent space column vector xi_vec = [rho_vec, theta]^T.
:return: The Lie Algebra (3x3 matrix).
"""
return np.block([[SO2.hat(xi_vec[2].item()), xi_vec[:2]],
[np.zeros((1, 3))]])
def jac_action_Xx_wrt_X(X, x):
"""Computes the Jacobian of the action X.action(x) with respect to the element X.
:param x: The 2D column vector x.
:return: The Jacobian (3x2 matrix)
"""
return np.block(
[[X.rotation.to_matrix(),
X.rotation.to_matrix() @ SO2.hat(1) @ x]])
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_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 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_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)
def test_vee_extracts_correct_angle_from_skew_symmetric_matrix():
theta = 3.0
theta_hat = SO2.hat(theta)
theta_hat_vee = SO2.vee(theta_hat)
np.testing.assert_array_equal(theta_hat_vee, theta)
