def test_jacobian_Y_ominus_X_wrt_X(): X = SE3((SO3.from_roll_pitch_yaw(np.pi / 8, np.pi / 2, 5 * np.pi / 6), np.array([[2, 1, 1]]).T)) Y = SE3((SO3.from_roll_pitch_yaw(np.pi / 7, np.pi / 3, 4 * np.pi / 6), np.array([[2, 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, -SE3.jac_left_inverse(Y - X)) # Test the Jacobian numerically. delta = 1e-3 * np.ones((6, 1)) taylor_diff = Y.ominus(X.oplus(delta)) - (Y.ominus(X) + (J_ominus_X @ delta)) np.testing.assert_almost_equal(taylor_diff, np.zeros((6, 1)), 6)
def test_jacobian_left_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_l_inv = SE3.jac_left_inverse(xi_vec) # Should have that left and right are block transposes of each other. J_r_inv = SE3.jac_right_inverse(xi_vec) np.testing.assert_almost_equal(J_l_inv[:3, :3], J_r_inv[:3, :3].T, 14) np.testing.assert_almost_equal(J_l_inv[:3, 3:], J_r_inv[:3, 3:].T, 14) np.testing.assert_almost_equal(J_l_inv[3:, 3:], J_r_inv[3:, 3:].T, 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.Exp(delta) @ X).Log() - (X.Log() + J_l_inv @ delta) np.testing.assert_almost_equal(taylor_diff, np.zeros((6, 1)), 5)