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_left():
theta = np.pi / 4
# Should have J_l(theta) == J_r(-theta).
np.testing.assert_almost_equal(SO2.jac_left(theta), SO2.jac_right(-theta),
14)