def jac_right_inverse(xi_vec):
"""Compute the right 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_r_inv_theta = SO3.jac_right_inverse(theta_vec)
Q_r = SE3._Q_right(xi_vec)
return np.block([[J_r_inv_theta, -J_r_inv_theta @ Q_r @ J_r_inv_theta],
[np.zeros((3, 3)), J_r_inv_theta]])
def test_jacobian_Y_ominus_X_wrt_Y():
X = SO3.from_angle_axis(np.pi / 4, np.array([[1, 1, 1]]).T / np.sqrt(3))
Y = SO3.from_angle_axis(np.pi / 3, np.array([[1, 0, -1]]).T / np.sqrt(2))
J_ominus_Y = Y.jac_Y_ominus_X_wrt_Y(X)
# Should be J_r_inv.
np.testing.assert_equal(J_ominus_Y, SO3.jac_right_inverse(Y - X))
# Test the Jacobian numerically.
delta = 1e-3 * np.ones((3, 1))
taylor_diff = Y.oplus(delta).ominus(X) - (Y.ominus(X) +
(J_ominus_Y @ delta))
np.testing.assert_almost_equal(taylor_diff, np.zeros((3, 1)), 6)
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_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)
