Exemplo n.º 1
0
def test_jacobian_Y_ominus_X_wrt_Y():
    X = SO2(np.pi / 4)
    Y = SO2(np.pi / 3)

    J_ominus_Y = Y.jac_Y_ominus_X_wrt_Y(X)

    # Should be J_r_inv.
    np.testing.assert_equal(J_ominus_Y, SO2.jac_right_inverse(Y - X))

    # Test the Jacobian numerically.
    delta = 1e-3 * np.ones((1, 1))
    taylor_diff = Y.oplus(delta).ominus(X) - (Y.ominus(X) +
                                              (J_ominus_Y @ delta))
    np.testing.assert_almost_equal(taylor_diff, 0.0, 6)
Exemplo n.º 2
0
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)
Exemplo n.º 3
0
def test_jacobian_left_inverse():
    theta = np.pi / 4

    # Should have J_l_inv(theta) == J_r_inv(theta).T
    np.testing.assert_almost_equal(SO2.jac_left_inverse(theta),
                                   SO2.jac_right_inverse(theta).T, 14)