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)