def testGroupOperator(self): for i in range(100): t1 = np.random.uniform(-10, 10, size=2) theta1 = np.random.uniform(-np.pi, np.pi) t2 = np.random.uniform(-10, 10, size=2) theta2 = np.random.uniform(-np.pi, np.pi) ct1 = np.cos(theta1) ct2 = np.cos(theta2) st1 = np.sin(theta1) st2 = np.sin(theta2) R1 = np.array([[ct1, -st1], [st1, ct1]]) R2 = np.array([[ct2, -st2], [st2, ct2]]) T1 = SE2.fromRandt(R1, t1) T2 = SE2.fromRandt(R2, t2) T = T1 * T2 R_true = R1 @ R2 t_true = R1 @ t2 + t1 T_true = SE2.fromRandt(R_true, t_true) np.testing.assert_allclose(T_true.arr, T.arr)
def testInv(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) T_inv = T.inv() T_inv_true = np.eye(3) T_inv_true[:2, :2] = R.T T_inv_true[:2, 2] = -R.T @ t np.testing.assert_allclose(T_inv_true, T_inv.arr)
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)
def testAdjoint(self): for i in range(100): t = np.random.uniform(-10, 10, size=2) theta = np.random.uniform(-np.pi, np.pi) u = np.random.uniform(-1, 1, size=2) phi = np.random.uniform(-np.pi, np.pi) # delta = np.array([phi, u[0], u[1]]) delta = np.array([u[0], u[1], phi]) ct = np.cos(theta) st = np.sin(theta) R = np.array([[ct, -st], [st, ct]]) T = SE2.fromRandt(R, t) adj = T.Adj T2_true = T * SE2.Exp(delta) T2 = SE2.Exp(adj @ delta) * T np.testing.assert_allclose(T2_true.arr, T2.arr)