def test_jacobian_left_inverse():
X = SE2((SO2(np.pi / 8), np.array([[2, 1]]).T))
xi_vec = X.Log()
J_l_inv = SE2.jac_left_inverse(xi_vec)
# Test the Jacobian numerically (using Exps and Logs, since left oplus and ominus have not been defined).
delta = 1e-3 * np.ones((3, 1))
taylor_diff = (SE2.Exp(delta) @ X).Log() - (X.Log() + J_l_inv @ delta)
np.testing.assert_almost_equal(taylor_diff, np.zeros((3, 1)), 5)
def test_jacobian_Y_ominus_X_wrt_X():
X = SE2((SO2(np.pi / 8), np.array([[1, 1]]).T))
Y = SE2((SO2(np.pi / 7), np.array([[1, 0]]).T))
J_ominus_X = Y.jac_Y_ominus_X_wrt_X(X)
# Should be -J_l_inv.
np.testing.assert_equal(J_ominus_X, -SE2.jac_left_inverse(Y - X))
# Test the Jacobian numerically.
delta = 1e-3 * np.ones((3, 1))
taylor_diff = Y.ominus(X.oplus(delta)) - (Y.ominus(X) +
(J_ominus_X @ delta))
np.testing.assert_almost_equal(taylor_diff, np.zeros((3, 1)), 6)
