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)