def testHat(self):
for i in range(100):
u = np.random.uniform(-10, 10, size=2)
theta = np.random.uniform(-np.pi, np.pi)
arr = np.array([u[0], u[1], theta])
X_true = np.array([[0, -theta, u[0]], [theta, 0, u[1]], [0, 0, 0]])
X = SE2.hat(arr)
np.testing.assert_allclose(X_true, X)
def testTaylorExp(self):
for i in range(100):
v = np.random.uniform(-10, 10, size=2)
theta = np.random.uniform(-1e-8, 1e-8)
arr = np.array([v[0], v[1], theta])
T = SE2.Exp(arr)
T_true = spl.expm(SE2.hat(arr))
np.testing.assert_allclose(T_true, T.arr)