def testTaylorLog(self):
for i in range(100): #Around 0
vec = np.random.uniform(-3.0, 3.0, size=3)
vec = vec / np.linalg.norm(vec)
ang = np.random.uniform(-1e-8, 1e-3)
temp = vec * ang
R = SO3.fromAxisAngle(temp)
logR = SO3.log(R)
logR_true = sp.linalg.logm(R.R)
if np.linalg.norm(logR_true - logR, ord='fro') > 1e-8:
Pdb().set_trace()
debug = 1
temp = SO3.log(R)
np.testing.assert_allclose(logR_true, logR, atol=1e-10)
for i in range(100): #Around pi
vec = np.random.uniform(-1.0, 1.0, size=3)
ang = np.random.uniform(-0, 1e-8)
vec = vec / np.linalg.norm(vec) * (np.pi - ang)
R = SO3.fromAxisAngle(vec)
logR = SO3.log(R)
logR_true = sp.linalg.logm(R.R)
def testFromAxisEuler(self):
for i in range(100):
theta = np.random.uniform(-np.pi, np.pi)
vec = np.random.uniform(-1, 1, size=3)
vec = vec / np.linalg.norm(vec)
R = SO3.fromAxisAngle(theta * vec)
R_true = Rotation.from_rotvec(vec * theta).as_matrix()
np.testing.assert_allclose(R_true, R.arr)
def testFromAxisAngleTaylor(self):
for i in range(100): #Taylor series
theta = np.random.uniform(0, 1e-3)
v = np.random.uniform(-10, 10, size=3)
vec = theta * v / np.linalg.norm(v)
R = SO3.fromAxisAngle(vec).R
q = Quaternion.fromAxisAngle(vec)
np.testing.assert_allclose(R, q.R.T, atol=1e-5)
def testFromAxisAngle(self):
for i in range(100):
theta = np.random.uniform(0, np.pi)
v = np.random.uniform(-10, 10, size=3)
vec = theta * v / np.linalg.norm(v)
R = SO3.fromAxisAngle(vec).R
q = Quaternion.fromAxisAngle(vec)
np.testing.assert_allclose(R, q.R.T)
def test_boxplusl(self):
for i in range(100):
R = SO3.random()
theta = np.random.uniform(0, np.pi)
vec = np.random.uniform(-1, 1, size=3)
vec = vec / np.linalg.norm(vec) * theta
R2 = R.boxplusl(vec)
R2_true = SO3.fromAxisAngle(vec) * R
np.testing.assert_allclose(R2_true.R, R2.R)
def testFromQuaternion(self):
for i in range(100):
theta = np.random.uniform(-np.pi, np.pi)
vec = np.random.uniform(-10.0, 10.0, size=3)
vec = vec / np.linalg.norm(vec)
q = Quaternion.fromAxisAngle(vec * theta)
R = SO3.fromQuaternion(q.q)
R2 = SO3.fromAxisAngle(vec * theta)
np.testing.assert_allclose(R.R, R2.R)