def test_right_jacobians_of_boxminusr(self): for i in range(100): q1, q2 = Quaternion.random(), Quaternion.random() theta, Jr1 = q1.boxminusr(q2, Jr1=np.eye(3)) dq = q2.inv() * q1 _, Jr1_true = Quaternion.Log(dq, Jr=np.eye(3)) _, Jr2 = q1.boxminusr(q2, Jr2=np.eye(3)) _, Jr2_true = Quaternion.Log(dq, Jl=np.eye(3)) np.testing.assert_allclose(Jr1_true, Jr1) np.testing.assert_allclose(-Jr2_true, Jr2)
def test_left_jacobian_or_logarithm(self): for i in range(100): q = Quaternion.random() logq, Jl_inv = Quaternion.Log(q, Jl=np.eye(3)) _, Jl = Quaternion.Exp(logq, Jl=np.eye(3)) np.testing.assert_allclose(np.linalg.inv(Jl), Jl_inv)
def LogJacobian(q): Jr, Jl = np.zeros((3, 3)), np.zeros((3, 3)) dx = 1e-4 for i in range(3): delta = np.zeros(3) delta[i] = dx q2r = q.boxplusr(delta) q2l = q.boxplusl(delta) vecr = Quaternion.Log(q2r) - Quaternion.Log(q) vecl = Quaternion.Log(q2l) - Quaternion.Log(q) Jr[:, i] = vecr / dx Jl[:, i] = vecl / dx return Jr, Jl
def testLogTaylor(self): for i in range(100): theta = np.random.uniform(0, 1e-3) v = np.random.uniform(-10, 10, size=3) vec = theta * v / np.linalg.norm(v) q = Quaternion.fromAxisAngle(vec) w = Quaternion.Log(q)
def testLog(self): for i in range(100): q = Quaternion.random() R = SO3.fromQuaternion(q.q) w_true = SO3.Log(R) w = Quaternion.Log(q) np.testing.assert_allclose(w_true, w)
q2 = Quaternion.random() q3, Jr1 = q.compose(q2, Jr=np.eye(3)) q3, Jr2 = q.compose(q2, Jr2=np.eye(3)) q3, Jl1 = q.compose(q2, Jl=np.eye(3)) q3, Jl2 = q.compose(q2, Jl2=np.eye(3)) Jr1n, Jl1n, Jr2n, Jl2n = compositionJacobian(q, q2) # Exponential is correct theta = np.random.uniform(-np.pi, np.pi, size=3) q, Jr = Quaternion.Exp(theta, Jr=np.eye(3)) q, Jl = Quaternion.Exp(theta, Jl=np.eye(3)) Jrn, Jln = ExponentialJacobian(theta) debug = 1 # Logarithm Jacobian is correct theta, Jr = Quaternion.Log(q, Jr=np.eye(3)) theta, Jl = Quaternion.Log(q, Jl=np.eye(3)) Jrn, Jln = LogJacobian(q) # Rotation jacobians vec = np.random.uniform(-10, 10, size=3) v2, Jr = q.rota(vec, Jr=np.eye(3)) v2, Jl = q.rota(vec, Jl=np.eye(3)) Jrn, Jln = rotationJacobian(q, vec) debug = 1 # Jacobian of inverse se3 works T = SE3.random() T_inv, Jr = T.inv(Jr=np.eye(6)) T_inv, Jl = T.inv(Jl=np.eye(6)) Jrn, Jln = se3InverseJacobian(T)