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)
