Ejemplo n.º 1
0
    def test_right_jacobians_of_boxminusr(self):
        for i in range(100):
            T1, T2 = SE2.random(), SE2.random()

            tau, Jr1 = T1.boxminusr(T2, Jr1=np.eye(3))
            dT = T2.inv() * T1
            _, Jr1_true = SE2.Log(dT, Jr=np.eye(3))

            _, Jr2 = T1.boxminusr(T2, Jr2=np.eye(3))
            _, Jr2_true = SE2.Log(dT, Jl=np.eye(3))

            np.testing.assert_allclose(Jr1_true, Jr1)
            np.testing.assert_allclose(-Jr2_true, Jr2)
Ejemplo n.º 2
0
    def test_left_jacobian_or_logarithm(self):
        T = SE2.random()
        tau, Jl_inv = SE2.Log(T, Jl=np.eye(3))
        _, Jl = SE2.Exp(tau, Jl=np.eye(3))

        np.testing.assert_allclose(np.linalg.inv(Jl), Jl_inv)
Ejemplo n.º 3
0
    def test_right_jacobian_of_logarithm(self):
        T = SE2.random()
        tau, Jr_inv = SE2.Log(T, Jr=np.eye(3))
        _, Jr = SE2.Exp(tau, Jr=np.eye(3))

        np.testing.assert_allclose(np.linalg.inv(Jr), Jr_inv)