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)
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)
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)