def test_jacobian_right_inverse(): X = SE3((SO3.from_roll_pitch_yaw(np.pi / 8, np.pi / 2, 5 * np.pi / 6), np.array([[2, 1, 1]]).T)) xi_vec = X.Log() J_r_inv = SE3.jac_right_inverse(xi_vec) # Should have J_l * J_r_inv = Exp(xi_vec).adjoint(). J_l = SE3.jac_left(xi_vec) np.testing.assert_almost_equal(J_l @ J_r_inv, SE3.Exp(xi_vec).adjoint(), 14) # Test the Jacobian numerically. delta = 1e-3 * np.ones((6, 1)) taylor_diff = X.oplus(delta).Log() - (X.Log() + J_r_inv @ 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)