def test_transformp(self):
pt = np.array([1, 0, 0])
T = SE3.fromAxisAngleAndt(np.pi / 2 * np.array([0, 0, 1]),
np.array([0, 1, 0]))
pt_p = T.transp(pt)
pt_true = np.array([-1, -1, 0])
np.testing.assert_allclose(pt_true, pt_p, atol=1e-10)
def test_from_axis_angle_taylor_series(self):
angle = [np.random.uniform(0.0, 1e-3) for i in range(100)]
axis = [np.random.uniform(-10.0, 10.0, size=3) for i in range(100)]
trans = [np.random.uniform(-10.0, 10.0, size=3) for i in range(100)]
for (v, phi, t) in zip(axis, angle, trans):
v = v / np.linalg.norm(v) * phi
T = SE3.fromAxisAngleAndt(v, t)
q = Quaternion.fromAxisAngle(v)
np.testing.assert_allclose(q.q, T.q_arr)
np.testing.assert_allclose(t, T.t)
def test_logarithmic_map_taylor_series(self):
angle = [np.random.uniform(0.0, 1e-8) for i in range(100)]
axis = [np.random.uniform(-10.0, 10.0, size=3) for i in range(100)]
trans = [np.random.uniform(-10.0, 10.0, size=3) for i in range(100)]
for (v, phi, t) in zip(axis, angle, trans):
v = v / np.linalg.norm(v) * phi
T = SE3.fromAxisAngleAndt(v, t)
logT = SE3.log(T)
R = T.R.T
t = T.t
T2 = np.block([[R, t[:, None]], [np.zeros((1, 3)), 1]])
temp = sp.linalg.logm(T2)
logT_true = np.array([
temp[0, 3], temp[1, 3], temp[2, 3], 0, temp[2, 1], temp[0, 2],
temp[1, 0]
])
np.testing.assert_allclose(logT_true, logT, atol=1e-3, rtol=1e-3)