def jac_left(xi_vec): """Compute the left derivative of Exp(xi_vec) with respect to xi_vec. :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_theta = SO3.jac_left(theta_vec) Q_l = SE3._Q_left(xi_vec) return np.block([[J_l_theta, Q_l], [np.zeros((3, 3)), J_l_theta]])
def test_jacobian_right_inverse(): theta_vec = 3 * np.pi / 4 * np.array([[1, -1, 0]]).T / np.sqrt(2) X = SO3.Exp(theta_vec) J_r_inv = SO3.jac_right_inverse(theta_vec) # Should have J_l * J_r_inv = Exp(theta_vec).adjoint(). J_l = SO3.jac_left(theta_vec) np.testing.assert_almost_equal(J_l @ J_r_inv, SO3.Exp(theta_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)
def test_jacobian_left(): theta_vec = np.pi / 4 * np.array([[-1, -1, 1]]).T / np.sqrt(3) # Should have J_l(theta_vec) == J_r(-theta_vec). np.testing.assert_almost_equal(SO3.jac_left(theta_vec), SO3.jac_right(-theta_vec), 14)