def test_jacobian_right():
theta = 3 * np.pi / 4
J_r = SO2.jac_right(theta)
# Test the Jacobian numerically.
delta = 1e-3 * np.ones((1, 1))
taylor_diff = SO2.Exp(theta + delta) - (SO2.Exp(theta) + J_r @ delta)
np.testing.assert_almost_equal(taylor_diff, 0.0, 5)
def test_jacobian_right_inverse():
theta = 3 * np.pi / 4
X = SO2.Exp(theta)
J_r_inv = SO2.jac_right_inverse(theta)
# Should have J_l * J_r_inv = Exp(theta).adjoint().
J_l = SO2.jac_left(theta)
np.testing.assert_almost_equal(J_l @ J_r_inv, SO2.Exp(theta).adjoint(), 14)
# Test the Jacobian numerically.
delta = 1e-3 * np.ones((1, 1))
taylor_diff = X.oplus(delta).Log() - (X.Log() + J_r_inv @ delta)
np.testing.assert_almost_equal(taylor_diff, 0.0, 5)
def test_difference_for_simple_rotation_with_operator_works():
X = SO2()
theta = 3 * np.pi / 4
Y = SO2.Exp(theta)
theta_diff = Y - X
np.testing.assert_almost_equal(theta_diff, theta, 14)
def test_difference_for_simple_rotation_works():
X = SO2()
theta = np.pi / 3
Y = SO2.Exp(theta)
theta_diff = Y.ominus(X)
np.testing.assert_array_equal(theta_diff, theta)
def test_log_returns_correct_theta():
theta = np.pi / 6
so3 = SO2.Exp(theta)
theta_log = so3.Log()
np.testing.assert_almost_equal(theta_log, theta, 14)
def test_jacobian_X_oplus_tau_wrt_X():
X = SO2(3 * np.pi / 4)
theta = np.pi / 4
J_oplus_X = X.jac_X_oplus_tau_wrt_X(theta)
# Should be Exp(tau).adjoint().inverse()
np.testing.assert_almost_equal(J_oplus_X, SO2.Exp(theta).adjoint(), 14)
# Test the Jacobian numerically.
delta = 1e-3 * np.ones((1, 1))
taylor_diff = X.oplus(delta).oplus(theta) - X.oplus(theta).oplus(
J_oplus_X @ delta)
np.testing.assert_almost_equal(taylor_diff, 0.0, 14)
def test_exp_returns_correct_rotation():
theta = 0.5 * np.pi
so2 = SO2.Exp(theta)
np.testing.assert_equal(so2.angle, theta)
