def test_jacobian_Y_ominus_X_wrt_Y():
X = SE2((SO2(np.pi / 8), np.array([[1, 1]]).T))
Y = SE2((SO2(np.pi / 7), np.array([[2, 0]]).T))
J_ominus_Y = Y.jac_Y_ominus_X_wrt_Y(X)
# Should be J_r_inv.
np.testing.assert_equal(J_ominus_Y, SE2.jac_right_inverse(Y - X))
# Test the Jacobian numerically.
delta = 1e-3 * np.ones((3, 1))
taylor_diff = Y.oplus(delta).ominus(X) - (Y.ominus(X) +
(J_ominus_Y @ delta))
np.testing.assert_almost_equal(taylor_diff, np.zeros((3, 1)), 6)
def test_jacobian_right_inverse():
X = SE2((SO2(np.pi / 8), np.array([[1, 1]]).T))
xi_vec = X.Log()
J_r_inv = SE2.jac_right_inverse(xi_vec)
# Should have J_l * J_r_inv = Exp(xi_vec).adjoint().
J_l = SE2.jac_left(xi_vec)
np.testing.assert_almost_equal(J_l @ J_r_inv,
SE2.Exp(xi_vec).adjoint(), 14)
# Test the Jacobian numerically.
delta = 1e-3 * np.ones((3, 1))
taylor_diff = X.oplus(delta).Log() - (X.Log() + J_r_inv @ delta)
np.testing.assert_almost_equal(taylor_diff, np.zeros((3, 1)), 5)
