def jac_left_inverse(xi_vec): """Compute the left derivative of Log(X) with respect to X for xi_vec = Log(X). :param xi_vec: The tangent space 6D column vector xi_vec = [rho_vec, theta_vec]^T. :return: The Jacobian (6x6 matrix) """ theta_vec = xi_vec[3:] J_l_inv_theta = SO3.jac_left_inverse(theta_vec) Q_l = SE3._Q_left(xi_vec) return np.block([[J_l_inv_theta, -J_l_inv_theta @ Q_l @ J_l_inv_theta], [np.zeros((3, 3)), J_l_inv_theta]])
def test_jacobian_Y_ominus_X_wrt_X(): X = SO3.from_angle_axis(np.pi / 4, np.array([[1, -1, 1]]).T / np.sqrt(3)) Y = SO3.from_angle_axis(np.pi / 2, np.array([[1, 0, 1]]).T / np.sqrt(2)) J_ominus_X = Y.jac_Y_ominus_X_wrt_X(X) # Should be -J_l_inv. np.testing.assert_equal(J_ominus_X, -SO3.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)
def test_jacobian_left_inverse(): theta_vec = np.pi / 4 * np.array([[-1, -1, 1]]).T / np.sqrt(3) # Should have J_l_inv(theta_vec) == J_r_inv(theta_vec).T np.testing.assert_almost_equal(SO3.jac_left_inverse(theta_vec), SO3.jac_right_inverse(theta_vec).T, 14)