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 testLog(self): #This has issues sometimes. It is infrequent though
for i in range(100):
temp = Rotation.random().as_matrix()
R = SO3(temp)
logR = SO3.log(R)
logR_true = sp.linalg.logm(temp)
if np.linalg.norm(logR_true - logR, ord='fro') > 1e-3:
Pdb().set_trace()
debug = 1
temp = SO3.log(R)
np.testing.assert_allclose(logR_true, logR, atol=1e-10)