def test_jacobian_right():
rho_vec = np.array([[1, 2, 3]]).T
theta_vec = 3 * np.pi / 4 * np.array([[1, -1, 1]]).T / np.sqrt(3)
xi_vec = np.vstack((rho_vec, theta_vec))
J_r = SE3.jac_right(xi_vec)
# Test the Jacobian numerically.
delta = 1e-3 * np.ones((6, 1))
taylor_diff = SE3.Exp(xi_vec + delta) - (SE3.Exp(xi_vec) + J_r @ delta)
np.testing.assert_almost_equal(taylor_diff, np.zeros((6, 1)), 5)
def test_jacobian_left():
rho_vec = np.array([[2, 1, 2]]).T
theta_vec = np.pi / 4 * np.array([[-1, -1, -1]]).T / np.sqrt(3)
xi_vec = np.vstack((rho_vec, theta_vec))
J_l = SE3.jac_left(xi_vec)
# Should have J_l(xi_vec) == J_r(-xi_vec).
np.testing.assert_almost_equal(J_l, SE3.jac_right(-xi_vec), 14)
# Test the Jacobian numerically (using Exps and Logs, since left oplus and ominus have not been defined).
delta = 1e-3 * np.ones((6, 1))
taylor_diff = SE3.Log(SE3.Exp(xi_vec + delta) @ (SE3.Exp(J_l @ delta) @ SE3.Exp(xi_vec)).inverse())
np.testing.assert_almost_equal(taylor_diff, np.zeros((6, 1)), 5)