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_left_inverse(): theta = np.pi / 4 # Should have J_l_inv(theta) == J_r_inv(theta).T np.testing.assert_almost_equal(SO2.jac_left_inverse(theta), SO2.jac_right_inverse(theta).T, 14)