def test_right_jacobian_of_composition(self):
T1 = SE3.random()
T2 = SE3.random()
T3, Jr = T1.compose(T2, Jr=np.eye(6))
Jr_true = np.linalg.inv(T2.Adj)
np.testing.assert_allclose(Jr_true, Jr)
def test_jacobians_of_composition_second_element(self):
for i in range(100):
T1 = SE3.random()
T2 = SE3.random()
T3, Jr2 = T1.compose(T2, Jr2=np.eye(6))
_, Jl2 = T1.compose(T2, Jl2=np.eye(6))
Jl2_true = T3.Adj @ Jr2 @ np.linalg.inv(T2.Adj)
np.testing.assert_allclose(Jl2_true, Jl2)
def test_left_jacobian_of_composition(self):
for i in range(100):
T1 = SE3.random()
T2 = SE3.random()
T3, Jr = T1.compose(T2, Jr=np.eye(6))
_, Jl = T1.compose(T2, Jl=np.eye(6))
Jl_true = T3.Adj @ Jr @ T1.inv().Adj
np.testing.assert_allclose(Jl_true, Jl, atol=1e-10)
def test_right_jacobian_of_logarithm(self):
for i in range(100):
T = SE3.random()
logT, Jr_inv = SE3.Log(T, Jr=np.eye(6))
_, Jr = SE3.Exp(logT, Jr=np.eye(6))
np.testing.assert_allclose(np.linalg.inv(Jr), Jr_inv)
def test_boxminusl(self):
transforms2 = [SE3.random() for i in range(100)]
for (T1, T2) in zip(self.transforms, transforms2):
v = T1.boxminusl(T2)
T = T2.boxplusl(v)
np.testing.assert_allclose(T.T, T1.T)
def test_jacobians_of_boxminusl_second_element(self):
for T1 in self.transforms:
T2 = SE3.random()
diff, Jr2 = T1.boxminusl(T2, Jr2=np.eye(6))
diff, Jl2 = T1.boxminusl(T2, Jl2=np.eye(6))
Jl_true = np.eye(6) @ Jr2 @ np.linalg.inv(T2.Adj)
np.testing.assert_allclose(Jl_true, Jl2)
def test_jacobians_of_boxminusl(self):
for T1 in self.transforms:
T2 = SE3.random()
diff, Jr = T1.boxminusl(T2, Jr1=np.eye(6))
diff, Jl = T1.boxminusl(T2, Jl1=np.eye(6))
Jl_true = np.eye(6) @ Jr @ np.linalg.inv(T1.Adj)
np.testing.assert_allclose(Jl_true, Jl)
def test_left_jacobian_of_transformation(self):
for i in range(100):
T = SE3.random()
v = np.random.uniform(-10, 10, size=3)
vp, Jl = T.transa(v, Jl=np.eye(6))
_, Jr = T.transa(v, Jr=np.eye(6))
Jl_true = np.eye(3) @ Jr @ np.linalg.inv(T.Adj)
np.testing.assert_allclose(Jl_true, Jl, atol=1e-10)
def test_right_jacobian_of_transformation(self):
for i in range(100):
T = SE3.random()
v = np.random.uniform(-10, 10, size=3)
vp, Jr = T.transa(v, Jr=np.eye(6))
vx = np.array([[0, -v[2], v[1]], [v[2], 0, -v[0]],
[-v[1], v[0], 0]])
Jr_true = np.block([T.R.T, -T.R.T @ vx])
np.testing.assert_allclose(Jr_true, Jr)
def test_left_jacobian_of_inversion(self):
T = SE3.random()
T_inv, Jr = T.inv(Jr=np.eye(6))
_, Jl = T.inv(Jl=np.eye(6))
Adj_T = T.Adj
Adj_Tinv = T_inv.Adj
Jl_true = Adj_Tinv @ Jr @ np.linalg.inv(Adj_T)
np.testing.assert_allclose(Jl_true, Jl)
def test_right_jacobians_of_boxminusr(self):
for T1 in self.transforms:
T2 = SE3.random()
tau, Jr1 = T1.boxminusr(T2, Jr1=np.eye(6))
dT = T2.inv() * T1
_, Jr1_true = SE3.Log(dT, Jr=np.eye(6))
_, Jr2 = T1.boxminusr(T2, Jr2=np.eye(6))
_, Jr2_true = SE3.Log(dT, Jl=np.eye(6))
np.testing.assert_allclose(Jr1_true, Jr1)
np.testing.assert_allclose(-Jr2_true, Jr2)
def test_left_jacobians_of_boxminusr(self):
for T1 in self.transforms:
T2 = SE3.random()
tau, Jl1 = T1.boxminusr(T2, Jl1=np.eye(6))
_, Jr1 = T1.boxminusr(T2, Jr1=np.eye(6))
Jl1_true = np.eye(6) @ Jr1 @ np.linalg.inv(T1.Adj)
_, Jl2 = T1.boxminusr(T2, Jl2=np.eye(6))
_, Jr2 = T1.boxminusr(T2, Jr2=np.eye(6))
Jl2_true = np.eye(6) @ Jr2 @ np.linalg.inv(T2.Adj)
np.testing.assert_allclose(Jl1_true, Jl1)
np.testing.assert_allclose(Jl2_true, Jl2)
def test_composition(self):
transforms2 = [SE3.random() for i in range(100)]
for (T1, T2) in zip(self.transforms, transforms2):
t1 = T1.t
t2 = T2.t
T3 = T1 * T2
q3_true = np.array([
T1.qw * T2.qw - T1.qx * T2.qx - T1.qy * T2.qy - T1.qz * T2.qz,
T1.qw * T2.qx + T1.qx * T2.qw + T1.qy * T2.qz - T1.qz * T2.qy,
T1.qw * T2.qy - T1.qx * T2.qz + T1.qy * T2.qw + T1.qz * T2.qx,
T1.qw * T2.qz + T1.qx * T2.qy - T1.qy * T2.qx + T1.qz * T2.qw
])
if q3_true[0] < 0:
q3_true *= -1
t3_true = T1.t + T1.R.T @ T2.t
T3_true = SE3(Quaternion(q3_true), t3_true)
np.testing.assert_allclose(T3_true.q_arr, T3.q_arr)
np.testing.assert_allclose(T3_true.t, T3.t)
def test_right_jacobian_of_inversion(self):
T = SE3.random()
T_inv, Jr = T.inv(Jr=np.eye(6))
np.testing.assert_allclose(-T.Adj, Jr)
def setUp(self):
self.transforms = [SE3.random() for i in range(100)]
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)
# Jacobian of composition works
T2 = SE3.random()
T3, Jr1 = T.compose(T2, Jr=np.eye(6))
T3, Jl1 = T.compose(T2, Jl=np.eye(6))
T3, Jr2 = T.compose(T2, Jr2=np.eye(6))
T3, Jl2 = T.compose(T2, Jl2=np.eye(6))
Jr1n, Jl1n, Jr2n, Jl2n = se3ComposeJacobians(T, T2)
# Exponential Jacobian works
rho = np.random.uniform(-10, 10, size=3)
theta = np.random.uniform(-np.pi, np.pi, size=3)