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)