def testTaylorLog(self):
for i in range(100): #Test taylor series
t = np.random.uniform(-10, 10, size=2)
theta = np.random.uniform(-1e-8, 1e-8)
T = SE2.fromAngleAndt(theta, t)
logT = SE2.log(T)
logT_true = spl.logm(T.arr)
if np.linalg.norm(logT_true - logT, ord='fro') > 1e-8:
Pdb().set_trace()
debug = 1
temp = SE2.log(T)
np.testing.assert_allclose(logT_true, logT, atol=1e-7)
def testLog(self):
for i in range(100):
t = np.random.uniform(-10, 10, size=2)
theta = np.random.uniform(-np.pi, np.pi)
ct = np.cos(theta)
st = np.sin(theta)
R = np.array([[ct, -st], [st, ct]])
T = SE2.fromRandt(R, t)
logT = SE2.log(T)
logT_true = spl.logm(T.arr)
if np.linalg.norm(logT_true - logT, ord='fro') > 1e-3:
Pdb().set_trace()
debug = 1
temp = SE2.log(T)
np.testing.assert_allclose(logT_true, logT, atol=1e-7)
