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 testFromAngle(self): for i in range(100): t = np.random.uniform(-10, 10, size=2) theta = np.random.uniform(-np.pi, np.pi) T = SE2.fromAngleAndt(theta, t) ct = np.cos(theta) st = np.sin(theta) R = np.array([[ct, -st], [st, ct]]) T_true = np.eye(3) T_true[:2, :2] = R T_true[:2, 2] = t np.testing.assert_allclose(T_true, T.arr)
def testActionOnVector(self): for i in range(100): t = np.random.uniform(-10, 10, size=2) theta = np.random.uniform(-np.pi, np.pi) T = SE2.fromAngleAndt(theta, t) vec = np.random.uniform(-5, 5, size=2) pt = T.transa(vec) pt_true = T.R @ vec + T.t np.testing.assert_allclose(pt_true, pt) for i in range(100): T = SE2.random() vec = np.random.uniform(-5, 5, size=2) pt = T.transp(vec) pt_true = T.R.T @ vec - T.R.T @ T.t np.testing.assert_allclose(pt_true, pt)