Beispiel #1
0
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)
Beispiel #2
0
def test_jacobian_left():
    theta = np.pi / 4

    # Should have J_l(theta) == J_r(-theta).
    np.testing.assert_almost_equal(SO2.jac_left(theta), SO2.jac_right(-theta),
                                   14)