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)