def test_jacobian_right():
theta_vec = 3 * np.pi / 4 * np.array([[1, -1, 1]]).T / np.sqrt(3)
J_r = SO3.jac_right(theta_vec)
# Test the Jacobian numerically.
delta = 1e-3 * np.ones((3, 1))
taylor_diff = SO3.Exp(theta_vec + delta) - (SO3.Exp(theta_vec) +
J_r @ delta)
np.testing.assert_almost_equal(taylor_diff, np.zeros((3, 1)), 5)
def jac_right(xi_vec):
"""Compute the right 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_r_theta = SO3.jac_right(theta_vec)
Q_r = SE3._Q_right(xi_vec)
return np.block([[J_r_theta, Q_r], [np.zeros((3, 3)), J_r_theta]])
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)
