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)